├── LICENSE ├── README.md └── rune_detector ├── CMakeLists.txt ├── include └── rune_detector │ └── detector.hpp ├── package.xml ├── src └── detector.cpp └── test ├── sample └── test.png ├── test_detector.cpp └── test_node_startup.cpp /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Chen Jun 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rm_power_rune 2 | RoboMaster power rune task 3 | -------------------------------------------------------------------------------- /rune_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(rune_detector) 3 | 4 | ## Use C++14 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | ## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, 9 | ## enforcing cleaner code. 10 | add_definitions(-Wall -Werror) 11 | 12 | ## Export compile commands for clangd 13 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 14 | 15 | ####################### 16 | ## Find dependencies ## 17 | ####################### 18 | 19 | find_package(ament_cmake_auto REQUIRED) 20 | ament_auto_find_build_dependencies() 21 | 22 | ########### 23 | ## Build ## 24 | ########### 25 | 26 | ament_auto_add_library(${PROJECT_NAME} SHARED 27 | DIRECTORY src 28 | ) 29 | 30 | # rclcpp_components_register_node(${PROJECT_NAME} 31 | # PLUGIN rm_auto_aim::RgbDetectorNode 32 | # EXECUTABLE rgb_detector_node 33 | # ) 34 | 35 | ############# 36 | ## Testing ## 37 | ############# 38 | 39 | if(BUILD_TESTING) 40 | find_package(ament_lint_auto REQUIRED) 41 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 42 | ament_cmake_copyright 43 | ament_cmake_uncrustify 44 | ) 45 | # ament_lint_auto_find_test_dependencies() 46 | 47 | find_package(ament_cmake_gtest) 48 | # ament_add_gtest(test_node_startup test/test_node_startup.cpp) 49 | # target_link_libraries(test_node_startup ${PROJECT_NAME}) 50 | 51 | ament_add_gtest(test_detector test/test_detector.cpp) 52 | target_link_libraries(test_detector ${PROJECT_NAME}) 53 | 54 | endif() 55 | 56 | ############# 57 | ## Install ## 58 | ############# 59 | 60 | ament_auto_package( 61 | INSTALL_TO_SHARE 62 | test/sample 63 | ) 64 | -------------------------------------------------------------------------------- /rune_detector/include/rune_detector/detector.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Chen Jun 2 | // Licensed under the MIT License. 3 | 4 | #ifndef RUNE_DETECTOR__DETECTOR_HPP_ 5 | #define RUNE_DETECTOR__DETECTOR_HPP_ 6 | 7 | // OpenCV 8 | #include 9 | #include 10 | 11 | // STD 12 | #include 13 | #include 14 | 15 | namespace rm_power_rune 16 | { 17 | class Detector 18 | { 19 | public: 20 | cv::Mat binarize(const cv::Mat & src); 21 | 22 | cv::Mat floodfill(); 23 | 24 | bool findArmor(cv::RotatedRect & armor); 25 | 26 | bool findCenter(cv::Point2f & center); 27 | 28 | enum class Color { 29 | RED, 30 | BLUE, 31 | } detect_color; 32 | 33 | int bin_thresh; 34 | 35 | int min_armor_area, max_armor_area; 36 | double min_armor_ratio, max_armor_ratio; 37 | double min_strip_ratio, max_strip_ratio; 38 | 39 | std::array sorted_pts; 40 | 41 | private: 42 | cv::Mat bin_; 43 | cv::Mat floodfilled_; 44 | 45 | cv::RotatedRect armor_; 46 | cv::Point2f center_; 47 | }; 48 | 49 | } // namespace rm_power_rune 50 | 51 | #endif // RUNE_DETECTOR__DETECTOR_HPP_ 52 | -------------------------------------------------------------------------------- /rune_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | rune_detector 7 | 0.1.0 8 | A template for ROS packages. 9 | Chen Jun 10 | BSD 11 | https://github.com/chenjunnn/rm_power_rune 12 | https://github.com/chenjunnn/rm_power_rune/issues 13 | Chen Jun 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | rclcpp 20 | rclcpp_components 21 | sensor_msgs 22 | geometry_msgs 23 | visualization_msgs 24 | cv_bridge 25 | image_transport 26 | image_transport_plugins 27 | 28 | ament_lint_auto 29 | ament_lint_common 30 | ament_cmake_clang_format 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /rune_detector/src/detector.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022 ChenJun 2 | // Licensed under the MIT License. 3 | 4 | #include "rune_detector/detector.hpp" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace rm_power_rune 15 | { 16 | cv::Mat Detector::binarize(const cv::Mat & src) 17 | { 18 | // Split the image into three channels 19 | std::vector channels; 20 | cv::split(src, channels); 21 | 22 | // Subtract between the red and blue channels 23 | cv::Mat diff; 24 | if (detect_color == Color::RED) { 25 | // 0-R 1-G 2-B 26 | cv::subtract(channels[0], channels[2], diff); 27 | } else { 28 | cv::subtract(channels[2], channels[0], diff); 29 | } 30 | 31 | // Threshold the image 32 | cv::threshold(diff, bin_, bin_thresh, 255, cv::THRESH_BINARY); 33 | 34 | // Close morphological operation 35 | cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5)); 36 | cv::morphologyEx(bin_, bin_, cv::MORPH_CLOSE, kernel); 37 | 38 | return bin_; 39 | } 40 | 41 | cv::Mat Detector::floodfill() 42 | { 43 | // Inverse the image 44 | cv::Mat inv; 45 | cv::bitwise_not(bin_, inv); 46 | 47 | cv::Mat tmp = inv.clone(); 48 | // Floodfill the image from top left corner 49 | floodFill(inv, cv::Point(0, 0), cv::Scalar(0)); 50 | floodfilled_ = inv; 51 | 52 | // If there is closed area at the top left corner, which causes the floodfill to fail, 53 | if (countNonZero(floodfilled_) > floodfilled_.rows * floodfilled_.cols * 0.5) { 54 | // Floodfill again from the bottom right corner 55 | floodFill(tmp, cv::Point(bin_.cols - 1, bin_.rows - 1), cv::Scalar(0)); 56 | floodfilled_ = tmp; 57 | } 58 | 59 | return floodfilled_; 60 | } 61 | 62 | bool Detector::findArmor(cv::RotatedRect & armor) 63 | { 64 | // Find the contours 65 | std::vector> contours; 66 | cv::findContours(floodfilled_, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); 67 | 68 | // TODO 69 | cv::Mat bin_clone = floodfilled_.clone(); 70 | cv::cvtColor(bin_clone, bin_clone, cv::COLOR_GRAY2RGB); 71 | 72 | std::vector armors; 73 | std::vector strips; 74 | for (const auto & contour : contours) { 75 | // Approximate the contour 76 | auto rect = cv::minAreaRect(contour); 77 | 78 | // Use area to check if the contour is part of a rune 79 | if (rect.size.area() > min_armor_area) { 80 | // Use ratio to distinguish rect is armor or not 81 | float ratio = rect.size.width > rect.size.height ? rect.size.width / rect.size.height 82 | : rect.size.height / rect.size.width; 83 | 84 | cv::putText( 85 | bin_clone, std::to_string(rect.size.area()), rect.center, cv::FONT_HERSHEY_SIMPLEX, 0.5, 86 | cv::Scalar(0, 255, 0), 2); 87 | 88 | if (min_armor_ratio < ratio && ratio < max_armor_ratio && rect.size.area() < max_armor_area) { 89 | armors.emplace_back(rect); 90 | } else { 91 | strips.emplace_back(rect); 92 | } 93 | } else { 94 | continue; 95 | } 96 | } 97 | 98 | // cv::imshow("armor", bin_clone); 99 | // cv::waitKey(1); 100 | 101 | // Find available armor 102 | auto armor_it = armors.begin(); 103 | while (armor_it != armors.end()) { 104 | bool available = true; 105 | 106 | auto strip_it = strips.begin(); 107 | while (strip_it != strips.end()) { 108 | cv::Point2f armor_pts[4], strip_pts[4]; 109 | armor_it->points(armor_pts); 110 | strip_it->points(strip_pts); 111 | 112 | // Calculate the min distance between the armor and the strip 113 | double min_dist = std::numeric_limits::max(); 114 | for (const auto & armor_pt : armor_pts) { 115 | for (const auto & strip_pt : strip_pts) { 116 | auto dist = cv::norm(armor_pt - strip_pt); 117 | min_dist = std::min(min_dist, dist); 118 | } 119 | } 120 | 121 | // Normalize the distance 122 | min_dist /= std::max(armor_it->size.width, armor_it->size.height); 123 | 124 | // The armor is close enough to the strip 125 | if (min_dist < 0.25) { 126 | available = false; 127 | strip_it = strips.erase(strip_it); 128 | } else { 129 | strip_it++; 130 | } 131 | } 132 | 133 | if (!available) { 134 | armor_it = armors.erase(armor_it); 135 | } else { 136 | armor_it++; 137 | } 138 | } 139 | 140 | if (armors.size() == 1) { 141 | this->armor_ = armors[0]; 142 | armor = armors[0]; 143 | return true; 144 | } else { 145 | return false; 146 | } 147 | } 148 | 149 | bool Detector::findCenter(cv::Point2f & center) 150 | { 151 | cv::Point2f armor_pts[4]; 152 | armor_.points(armor_pts); 153 | 154 | // Judge which side is the long side 155 | if (cv::norm(armor_pts[0] - armor_pts[1]) > cv::norm(armor_pts[1] - armor_pts[2])) { 156 | // 0-1 is the long side 157 | sorted_pts = {armor_pts[0], armor_pts[1], armor_pts[2], armor_pts[3]}; 158 | } else { 159 | // 1-2 is the long side 160 | sorted_pts = {armor_pts[1], armor_pts[2], armor_pts[3], armor_pts[0]}; 161 | } 162 | 163 | // Judge which side the flowing water lights are on 164 | auto short_side = sorted_pts[1] - sorted_pts[2]; 165 | 166 | auto getROI = [&](const std::vector & roi_pts) -> cv::Mat { 167 | cv::Mat mask = cv::Mat::zeros(bin_.size(), CV_8UC1); 168 | std::vector> vpts = {roi_pts}; 169 | cv::fillPoly(mask, vpts, cv::Scalar(255)); 170 | return bin_.mul(mask); 171 | }; 172 | 173 | std::vector lights_roi1_pts = { 174 | sorted_pts[0] + 3 * short_side, sorted_pts[1] + 3 * short_side, sorted_pts[1], sorted_pts[0]}; 175 | double sum1 = cv::sum(getROI(lights_roi1_pts))[0]; 176 | 177 | std::vector lights_roi2_pts = { 178 | sorted_pts[2] - 3 * short_side, sorted_pts[3] - 3 * short_side, sorted_pts[3], sorted_pts[2]}; 179 | double sum2 = cv::sum(getROI(lights_roi2_pts))[0]; 180 | 181 | if (sum1 > sum2) { 182 | // lights are near 0-1 side 183 | } else { 184 | // lights are near 1-2 side 185 | sorted_pts = {sorted_pts[2], sorted_pts[3], sorted_pts[0], sorted_pts[1]}; 186 | short_side = sorted_pts[1] - sorted_pts[2]; 187 | } 188 | 189 | // Get the roi of the center 190 | cv::Point2f roi_center = armor_.center + short_side * 6.0; 191 | cv::Mat mask = cv::Mat::zeros(bin_.size(), CV_8UC1); 192 | int radius = cv::norm(short_side) * 8.0; 193 | cv::circle(mask, roi_center, radius, cv::Scalar(255), -1); 194 | cv::Mat roi = bin_.mul(mask); 195 | 196 | // TODO 197 | cv::Mat bin_clone = bin_.clone(); 198 | cv::cvtColor(bin_clone, bin_clone, cv::COLOR_GRAY2RGB); 199 | cv::circle(bin_clone, roi_center, radius, cv::Scalar(0, 255, 0), 2); 200 | 201 | // Find the center 202 | std::vector> contours; 203 | cv::findContours(roi, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); 204 | 205 | for (const auto & contour : contours) { 206 | cv::Rect rect = cv::boundingRect(contour); 207 | 208 | cv::putText( 209 | bin_clone, std::to_string(rect.area()), rect.br(), cv::FONT_HERSHEY_SIMPLEX, 0.5, 210 | cv::Scalar(0, 255, 0), 2); 211 | 212 | cv::putText( 213 | bin_clone, std::to_string(rect.size().aspectRatio()), rect.tl(), cv::FONT_HERSHEY_SIMPLEX, 0.5, 214 | cv::Scalar(0, 255, 0), 2); 215 | 216 | if (250 < rect.area() && rect.area() < 400) { 217 | center = center_ = (rect.br() + rect.tl()) * 0.5; 218 | return true; 219 | } 220 | } 221 | 222 | // TODO 223 | // cv::imshow("center", bin_clone); 224 | // cv::waitKey(1); 225 | 226 | return false; 227 | } 228 | 229 | } // namespace rm_power_rune 230 | -------------------------------------------------------------------------------- /rune_detector/test/sample/test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chenjunnn/rm_power_rune/3f63c8e0339eb18497eea6c3b25a5a73deb753a1/rune_detector/test/sample/test.png -------------------------------------------------------------------------------- /rune_detector/test/test_detector.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Chen Jun 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // STL 11 | #include 12 | #include 13 | 14 | #include "rune_detector/detector.hpp" 15 | 16 | std::unique_ptr detector; 17 | 18 | cv::Mat src; 19 | cv::Mat bin; 20 | cv::Mat floodfilled; 21 | cv::Point2f center; 22 | 23 | TEST(test_detector, init) 24 | { 25 | detector = std::make_unique(); 26 | detector->detect_color = rm_power_rune::Detector::Color::RED; 27 | detector->bin_thresh = 80; 28 | 29 | src = 30 | cv::imread(ament_index_cpp::get_package_share_directory("rune_detector") + "/sample/test.png"); 31 | // BGR -> RGB 32 | cv::cvtColor(src, src, cv::COLOR_BGR2RGB); 33 | } 34 | 35 | TEST(test_detector, binarize) 36 | { 37 | bin = detector->binarize(src); 38 | 39 | cv::imwrite("/tmp/bin.png", bin); 40 | } 41 | 42 | TEST(test_detector, floodfill) 43 | { 44 | floodfilled = detector->floodfill(); 45 | 46 | cv::imwrite("/tmp/floodfilled.png", floodfilled); 47 | } 48 | 49 | TEST(test_detector, findArmor) 50 | { 51 | detector->min_armor_ratio = 1.5; 52 | detector->max_armor_ratio = 2.3; 53 | detector->min_strip_ratio = 3.8; 54 | detector->max_strip_ratio = 4.6; 55 | 56 | cv::RotatedRect armor; 57 | EXPECT_TRUE(detector->findArmor(armor)); 58 | 59 | // Draw the armor 60 | cv::Mat src_with_armor = src.clone(); 61 | cv::cvtColor(src_with_armor, src_with_armor, cv::COLOR_RGB2BGR); 62 | cv::ellipse(src_with_armor, armor, cv::Scalar(0, 255, 0), 2); 63 | 64 | cv::imwrite("/tmp/armor.png", src_with_armor); 65 | } 66 | 67 | TEST(test_detector, findCenter) 68 | { 69 | EXPECT_TRUE(detector->findCenter(center)); 70 | 71 | // Draw the center 72 | cv::Mat src_with_center = src.clone(); 73 | cv::cvtColor(src_with_center, src_with_center, cv::COLOR_RGB2BGR); 74 | cv::circle(src_with_center, center, 2, cv::Scalar(0, 255, 0), 2); 75 | 76 | cv::imwrite("/tmp/center.png", src_with_center); 77 | } 78 | 79 | TEST(test_detector, drawResult) 80 | { 81 | cv::Mat result = src.clone(); 82 | cv::cvtColor(result, result, cv::COLOR_RGB2BGR); 83 | 84 | auto pts = detector->sorted_pts; 85 | std::vector pts_vec = {pts[0], pts[3], pts[2], pts[1], center}; 86 | cv::polylines(result, pts_vec, true, cv::Scalar(0, 255, 0), 2); 87 | 88 | cv::imwrite("/tmp/result.png", result); 89 | } 90 | -------------------------------------------------------------------------------- /rune_detector/test/test_node_startup.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Chen Jun 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | // STD 10 | #include 11 | 12 | #include "armor_detector/detector_node.hpp" 13 | 14 | TEST(RgbDetectorNodeTest, NodeStartupTest) 15 | { 16 | rclcpp::NodeOptions options; 17 | auto node = std::make_shared(options); 18 | node.reset(); 19 | } 20 | 21 | TEST(RgbDepthDetectorNodeTest, NodeStartupTest) 22 | { 23 | rclcpp::NodeOptions options; 24 | auto node = std::make_shared(options); 25 | node.reset(); 26 | } 27 | 28 | int main(int argc, char ** argv) 29 | { 30 | testing::InitGoogleTest(&argc, argv); 31 | rclcpp::init(argc, argv); 32 | auto result = RUN_ALL_TESTS(); 33 | rclcpp::shutdown(); 34 | return result; 35 | } 36 | --------------------------------------------------------------------------------