diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..2f8d64b --- /dev/null +++ b/.clang-format @@ -0,0 +1,18 @@ +--- +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false \ No newline at end of file diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..bf3d848 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,55 @@ +--- +Checks: '-*, + performance-*, + -performance-unnecessary-value-param, + llvm-namespace-comment, + modernize-redundant-void-arg, + modernize-use-nullptr, + modernize-use-default, + modernize-use-override, + modernize-loop-convert, + modernize-make-shared, + modernize-make-unique, + misc-unused-parameters, + readability-named-parameter, + readability-redundant-smartptr-get, + readability-redundant-string-cstr, + readability-simplify-boolean-expr, + readability-container-size-empty, + readability-identifier-naming, + ' +HeaderFilterRegex: '' +AnalyzeTemporaryDtors: false +CheckOptions: + - key: llvm-namespace-comment.ShortNamespaceLines + value: '10' + - key: llvm-namespace-comment.SpacesBeforeComments + value: '2' + - key: misc-unused-parameters.StrictMode + value: '1' + - key: readability-braces-around-statements.ShortStatementLines + value: '2' + # type names + - key: readability-identifier-naming.ClassCase + value: CamelCase + - key: readability-identifier-naming.EnumCase + value: CamelCase + - key: readability-identifier-naming.UnionCase + value: CamelCase + # method names + - key: readability-identifier-naming.MethodCase + value: camelBack + # variable names + - key: readability-identifier-naming.VariableCase + value: lower_case + - key: readability-identifier-naming.ClassMemberSuffix + value: '_' + # const static or global variables are UPPER_CASE + - key: readability-identifier-naming.EnumConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.StaticConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.ClassConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.GlobalVariableCase + value: UPPER_CASE \ No newline at end of file diff --git a/.github/workflows/ros_ci.yml b/.github/workflows/ros_ci.yml new file mode 100644 index 0000000..17f4850 --- /dev/null +++ b/.github/workflows/ros_ci.yml @@ -0,0 +1,30 @@ +name: Build and Test (humble) +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] +jobs: + build-and-test: + runs-on: ubuntu-22.04 + steps: + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@v0.4 + with: + required-ros-distributions: humble + - name: Build rm_auto_aim + uses: ros-tooling/action-ros-ci@v0.2 + with: + package-name: rm_auto_aim + target-ros2-distro: humble + skip-tests: true + - name: Test armor_detector + run: | + source /opt/ros/humble/setup.sh + cd ros_ws + colcon test --packages-select armor_detector --event-handlers console_cohesion+ --return-code-on-test-failure + - name: Test armor_tracker + run: | + source /opt/ros/humble/setup.sh + cd ros_ws + colcon test --packages-select armor_tracker --event-handlers console_cohesion+ --return-code-on-test-failure diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3f4f07a --- /dev/null +++ b/.gitignore @@ -0,0 +1,170 @@ + +# Created by https://www.gitignore.io/api/c++,linux,macos,clion +# Edit at https://www.gitignore.io/?templates=c++,linux,macos,clion + +### 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 + +### CLion ### +# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and WebStorm +# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 + +# User-specific stuff +.idea/**/workspace.xml +.idea/**/tasks.xml +.idea/**/usage.statistics.xml +.idea/**/dictionaries +.idea/**/shelf + +# Generated files +.idea/**/contentModel.xml + +# Sensitive or high-churn files +.idea/**/dataSources/ +.idea/**/dataSources.ids +.idea/**/dataSources.local.xml +.idea/**/sqlDataSources.xml +.idea/**/dynamic.xml +.idea/**/uiDesigner.xml +.idea/**/dbnavigator.xml + +# Gradle +.idea/**/gradle.xml +.idea/**/libraries + +# Gradle and Maven with auto-import +# When using Gradle or Maven with auto-import, you should exclude module files, +# since they will be recreated, and may cause churn. Uncomment if using +# auto-import. +# .idea/modules.xml +# .idea/*.iml +# .idea/modules +# *.iml +# *.ipr + +# CMake +cmake-build-*/ + +# Mongo Explorer plugin +.idea/**/mongoSettings.xml + +# File-based project format +*.iws + +# IntelliJ +out/ + +# mpeltonen/sbt-idea plugin +.idea_modules/ + +# JIRA plugin +atlassian-ide-plugin.xml + +# Cursive Clojure plugin +.idea/replstate.xml + +# Crashlytics plugin (for Android Studio and IntelliJ) +com_crashlytics_export_strings.xml +crashlytics.properties +crashlytics-build.properties +fabric.properties + +# Editor-based Rest Client +.idea/httpRequests + +# Android studio 3.1+ serialized cache file +.idea/caches/build_file_checksums.ser + +### CLion Patch ### +# Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721 + +# *.iml +# modules.xml +# .idea/misc.xml +# *.ipr + +# Sonarlint plugin +.idea/**/sonarlint/ + +# SonarQube Plugin +.idea/**/sonarIssues.xml + +# Markdown Navigator plugin +.idea/**/markdown-navigator.xml +.idea/**/markdown-navigator/ + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +# End of https://www.gitignore.io/api/c++,linux,macos,clion diff --git a/README.md b/README.md new file mode 100644 index 0000000..f5c6472 --- /dev/null +++ b/README.md @@ -0,0 +1,61 @@ +# rm_auto_aim + +## Overview + +RoboMaster 装甲板自瞄算法模块 + +rm_vision + +该项目为 [rm_vision](https://github.com/chenjunnn/rm_vision) 的子模块 + +若有帮助请Star这个项目，感谢~ + +### License + +The source code is released under a [MIT license](rm_auto_aim/LICENSE). + +[![License: MIT](https://img.shields.io/badge/License-MIT-blue.svg)](https://opensource.org/licenses/MIT) + +Author: Chen Jun + +运行环境：Ubuntu 22.04 / ROS2 Humble (未在其他环境下测试) + +![Build Status](https://github.com/chenjunnn/rm_auto_aim/actions/workflows/ros_ci.yml/badge.svg) + +## Building from Source + +### Building + +在 Ubuntu 22.04 环境下安装 [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) + +创建 ROS 工作空间后 clone 项目，使用 rosdep 安装依赖后编译代码 + + cd ros_ws/src + git clone https://github.com/chenjunnn/rm_auto_aim.git + cd .. + rosdep install --from-paths src --ignore-src -r -y + colcon build --symlink-install --packages-up-to auto_aim_bringup + +### Testing + +Run the tests with + + colcon test --packages-up-to auto_aim_bringup + +## Packages + +- [armor_detector](armor_detector) + + 订阅相机参数及图像流进行装甲板的识别并解算三维位置，输出识别到的装甲板在输入frame下的三维位置 (一般是以相机光心为原点的相机坐标系) + +- [armor_tracker](armor_tracker) + + 订阅识别节点发布的装甲板三维位置及机器人的坐标转换信息，将装甲板三维位置变换到指定惯性系（一般是以云台中心为原点，IMU 上电时的 Yaw 朝向为 X 轴的惯性系）下，然后将装甲板目标送入跟踪器中，输出跟踪机器人在指定惯性系下的状态 + +- auto_aim_interfaces + + 定义了识别节点和处理节点的接口以及定义了用于 Debug 的信息 + +- auto_aim_bringup + + 包含启动识别节点和处理节点的默认参数文件及 launch 文件 diff --git a/armor_detector/CMakeLists.txt b/armor_detector/CMakeLists.txt new file mode 100644 index 0000000..f1cd143 --- /dev/null +++ b/armor_detector/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.10) +project(armor_detector) + +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +add_definitions(-Wall -Werror) + +## Export compile commands for clangd +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +####################### +## Find dependencies ## +####################### + +find_package(ament_cmake_auto REQUIRED) +find_package(OpenCV REQUIRED) +ament_auto_find_build_dependencies() + +########### +## Build ## +########### + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_include_directories(${PROJECT_NAME} PUBLIC ${OpenCV_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN rm_auto_aim::ArmorDetectorNode + EXECUTABLE armor_detector_node +) + +############# +## Testing ## +############# + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_copyright + ament_cmake_uncrustify + ament_cmake_cpplint + ) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest) + ament_add_gtest(test_node_startup test/test_node_startup.cpp) + target_link_libraries(test_node_startup ${PROJECT_NAME}) + + ament_add_gtest(test_number_cls test/test_number_cls.cpp) + target_link_libraries(test_number_cls ${PROJECT_NAME}) + +endif() + +############# +## Install ## +############# + +ament_auto_package( + INSTALL_TO_SHARE + model +) diff --git a/armor_detector/README.md b/armor_detector/README.md new file mode 100644 index 0000000..d5d49db --- /dev/null +++ b/armor_detector/README.md @@ -0,0 +1,104 @@ +# armor_detector + +- [DetectorNode](#basedetectornode) + - [Detector](#detector) + - [NumberClassifier](#numberclassifier) + - [PnPSolver](#pnpsolver) + +## 识别节点 + +订阅相机参数及图像流进行装甲板的识别并解算三维位置，输出识别到的装甲板在输入frame下的三维位置 (一般是以相机光心为原点的相机坐标系) + +### DetectorNode +装甲板识别节点 + +包含[Detector](#detector) +包含[PnPSolver](#pnpsolver) + +订阅： +- 相机参数 `/camera_info` +- 彩色图像 `/image_raw` + +发布： +- 识别目标 `/detector/armors` + +静态参数： +- 筛选灯条的参数 `light` + - 长宽比范围 `min/max_ratio` + - 最大倾斜角度 `max_angle` +- 筛选灯条配对结果的参数 `armor` + - 两灯条的最小长度之比（短边/长边）`min_light_ratio ` + - 装甲板两灯条中心的距离范围（大装甲板）`min/max_large_center_distance` + - 装甲板两灯条中心的距离范围（小装甲板）`min/max_small_center_distance` + - 装甲板的最大倾斜角度 `max_angle` + +动态参数： +- 是否发布 debug 信息 `debug` +- 识别目标颜色 `detect_color` +- 二值化的最小阈值 `binary_thres` +- 数字分类器 `classifier` + - 置信度阈值 `threshold` + +## Detector +装甲板识别器 + +### preprocessImage +预处理 + +| ![](docs/raw.png) | ![](docs/hsv_bin.png) | ![](docs/gray_bin.png) | +| :---------------: | :-------------------: | :--------------------: | +| 原图 | 通过颜色二值化 | 通过灰度二值化 | + +由于一般工业相机的动态范围不够大，导致若要能够清晰分辨装甲板的数字，得到的相机图像中灯条中心就会过曝，灯条中心的像素点的值往往都是 R=B。根据颜色信息来进行二值化效果不佳，因此此处选择了直接通过灰度图进行二值化，将灯条的颜色判断放到后续处理中。 + +### findLights +寻找灯条 + +通过 findContours 得到轮廓，再通过 minAreaRect 获得最小外接矩形，对其进行长宽比和倾斜角度的判断，可以高效的筛除形状不满足的亮斑。 + +判断灯条颜色这里采用了对轮廓内的的R/B值求和，判断两和的的大小的方法，若 `sum_r > sum_b` 则认为是红色灯条，反之则认为是蓝色灯条。 + +| ![](docs/red.png) | ![](docs/blue.png) | +| :---------------: | :----------------: | +| 提取出的红色灯条 | 提取出的蓝色灯条 | + +### matchLights +配对灯条 + +根据 `detect_color` 选择对应颜色的灯条进行两两配对，首先筛除掉两条灯条中间包含另一个灯条的情况，然后根据两灯条的长度之比、两灯条中心的距离、配对出装甲板的倾斜角度来筛选掉条件不满足的结果，得到形状符合装甲板特征的灯条配对。 + +## NumberClassifier +数字分类器 + +### extractNumbers +提取数字 + +| ![](docs/num_raw.png) | ![](docs/num_warp.png) | ![](docs/num_roi.png) | ![](docs/num_bin.png) | +| :-------------------: | :--------------------: | :-------------------: | :-------------------: | +| 原图 | 透视变换 | 取ROI | 二值化 | + +将每条灯条上下的角点拉伸到装甲板的上下边缘作为待变换点，进行透视变换，再对变换后的图像取ROI。考虑到数字图案实质上就是黑色背景+白色图案，所以此处使用了大津法进行二值化。 + +### Classify +分类 + +由于上一步对于数字的提取效果已经非常好，数字图案的特征非常清晰明显，装甲板的远近、旋转都不会使图案产生过多畸变，且图案像素点少，所以我们使用多层感知机（MLP）进行分类。 + +网络结构中定义了两个隐藏层和一个分类层，将二值化后的数字展平成 20x28=560 维的输入，送入网络进行分类。 + +网络结构： + +![](docs/model.svg) + + + + + +## PnPSolver +PnP解算器 + +[Perspective-n-Point (PnP) pose computation](https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html) + +PnP解算器将 `cv::solvePnP()` 封装，接口中传入 `Armor` 类型的数据即可得到 `geometry_msgs::msg::Point` 类型的三维坐标。 + +考虑到装甲板的四个点在一个平面上，在PnP解算方法上我们选择了 `cv::SOLVEPNP_IPPE` (Method is based on the paper of T. Collins and A. Bartoli. ["Infinitesimal Plane-Based Pose Estimation"](https://link.springer.com/article/10.1007/s11263-014-0725-5). This method requires coplanar object points.) diff --git a/armor_detector/docs/blue.png b/armor_detector/docs/blue.png new file mode 100644 index 0000000..f10db16 Binary files /dev/null and b/armor_detector/docs/blue.png differ diff --git a/armor_detector/docs/gray_bin.png b/armor_detector/docs/gray_bin.png new file mode 100644 index 0000000..5c35e71 Binary files /dev/null and b/armor_detector/docs/gray_bin.png differ diff --git a/armor_detector/docs/hsv_bin.png b/armor_detector/docs/hsv_bin.png new file mode 100644 index 0000000..075e1c6 Binary files /dev/null and b/armor_detector/docs/hsv_bin.png differ diff --git a/armor_detector/docs/model.svg b/armor_detector/docs/model.svg new file mode 100644 index 0000000..48b6f52 --- /dev/null +++ b/armor_detector/docs/model.svg @@ -0,0 +1 @@ +1×1×20×281×9inputfloat32[1,1,20,28]ReshapeReshape_1int64[2]shape〈2〉GemmGemm_2float32[164,560]B〈164×560〉float32[164]C〈164〉ReluRelu_3GemmGemm_4float32[84,164]B〈84×164〉float32[84]C〈84〉ReluRelu_5GemmGemm_6float32[9,84]B〈9×84〉float32[9]C〈9〉ReluRelu_7outputfloat32[1,9] \ No newline at end of file diff --git a/armor_detector/docs/num_bin.png b/armor_detector/docs/num_bin.png new file mode 100644 index 0000000..1d47d99 Binary files /dev/null and b/armor_detector/docs/num_bin.png differ diff --git a/armor_detector/docs/num_raw.png b/armor_detector/docs/num_raw.png new file mode 100644 index 0000000..302c852 Binary files /dev/null and b/armor_detector/docs/num_raw.png differ diff --git a/armor_detector/docs/num_roi.png b/armor_detector/docs/num_roi.png new file mode 100644 index 0000000..b542bc5 Binary files /dev/null and b/armor_detector/docs/num_roi.png differ diff --git a/armor_detector/docs/num_warp.png b/armor_detector/docs/num_warp.png new file mode 100644 index 0000000..3fe6d74 Binary files /dev/null and b/armor_detector/docs/num_warp.png differ diff --git a/armor_detector/docs/raw.png b/armor_detector/docs/raw.png new file mode 100644 index 0000000..af656a0 Binary files /dev/null and b/armor_detector/docs/raw.png differ diff --git a/armor_detector/docs/red.png b/armor_detector/docs/red.png new file mode 100644 index 0000000..6e6bca0 Binary files /dev/null and b/armor_detector/docs/red.png differ diff --git a/armor_detector/docs/result.png b/armor_detector/docs/result.png new file mode 100644 index 0000000..df8bb14 Binary files /dev/null and b/armor_detector/docs/result.png differ diff --git a/armor_detector/docs/train_acc.png b/armor_detector/docs/train_acc.png new file mode 100644 index 0000000..5814d6b Binary files /dev/null and b/armor_detector/docs/train_acc.png differ diff --git a/armor_detector/docs/train_loss.png b/armor_detector/docs/train_loss.png new file mode 100644 index 0000000..12a5c6b Binary files /dev/null and b/armor_detector/docs/train_loss.png differ diff --git a/armor_detector/docs/val_acc.png b/armor_detector/docs/val_acc.png new file mode 100644 index 0000000..c77a786 Binary files /dev/null and b/armor_detector/docs/val_acc.png differ diff --git a/armor_detector/docs/val_loss.png b/armor_detector/docs/val_loss.png new file mode 100644 index 0000000..28793eb Binary files /dev/null and b/armor_detector/docs/val_loss.png differ diff --git a/armor_detector/include/armor_detector/armor.hpp b/armor_detector/include/armor_detector/armor.hpp new file mode 100644 index 0000000..fec27bd --- /dev/null +++ b/armor_detector/include/armor_detector/armor.hpp @@ -0,0 +1,73 @@ +// Copyright 2022 Chen Jun +// Licensed under the MIT License. + +#ifndef ARMOR_DETECTOR__ARMOR_HPP_ +#define ARMOR_DETECTOR__ARMOR_HPP_ + +#include + +// STL +#include +#include + +namespace rm_auto_aim +{ +const int RED = 0; +const int BLUE = 1; + +enum class ArmorType { SMALL, LARGE, INVALID }; +const std::string ARMOR_TYPE_STR[3] = {"small", "large", "invalid"}; + +struct Light : public cv::RotatedRect +{ + Light() = default; + explicit Light(cv::RotatedRect box) : cv::RotatedRect(box) + { + cv::Point2f p[4]; + box.points(p); + std::sort(p, p + 4, [](const cv::Point2f & a, const cv::Point2f & b) { return a.y < b.y; }); + top = (p[0] + p[1]) / 2; + bottom = (p[2] + p[3]) / 2; + + length = cv::norm(top - bottom); + width = cv::norm(p[0] - p[1]); + + tilt_angle = std::atan2(std::abs(top.x - bottom.x), std::abs(top.y - bottom.y)); + tilt_angle = tilt_angle / CV_PI * 180; + } + + int color; + cv::Point2f top, bottom; + double length; + double width; + float tilt_angle; +}; + +struct Armor +{ + Armor() = default; + Armor(const Light & l1, const Light & l2) + { + if (l1.center.x < l2.center.x) { + left_light = l1, right_light = l2; + } else { + left_light = l2, right_light = l1; + } + center = (left_light.center + right_light.center) / 2; + } + + // Light pairs part + Light left_light, right_light; + cv::Point2f center; + ArmorType type; + + // Number part + cv::Mat number_img; + std::string number; + float confidence; + std::string classfication_result; +}; + +} // namespace rm_auto_aim + +#endif // ARMOR_DETECTOR__ARMOR_HPP_ diff --git a/armor_detector/include/armor_detector/detector.hpp b/armor_detector/include/armor_detector/detector.hpp new file mode 100644 index 0000000..5e3cd74 --- /dev/null +++ b/armor_detector/include/armor_detector/detector.hpp @@ -0,0 +1,83 @@ +// Copyright 2022 Chen Jun +// Licensed under the MIT License. + +#ifndef ARMOR_DETECTOR__DETECTOR_HPP_ +#define ARMOR_DETECTOR__DETECTOR_HPP_ + +// OpenCV +#include +#include + +// STD +#include +#include +#include + +#include "armor_detector/armor.hpp" +#include "armor_detector/number_classifier.hpp" +#include "auto_aim_interfaces/msg/debug_armors.hpp" +#include "auto_aim_interfaces/msg/debug_lights.hpp" + +namespace rm_auto_aim +{ +class Detector +{ +public: + struct LightParams + { + // width / height + double min_ratio; + double max_ratio; + // vertical angle + double max_angle; + }; + + struct ArmorParams + { + double min_light_ratio; + // light pairs distance + double min_small_center_distance; + double max_small_center_distance; + double min_large_center_distance; + double max_large_center_distance; + // horizontal angle + double max_angle; + }; + + Detector(const int & bin_thres, const int & color, const LightParams & l, const ArmorParams & a); + + std::vector detect(const cv::Mat & input); + + cv::Mat preprocessImage(const cv::Mat & input); + std::vector findLights(const cv::Mat & rbg_img, const cv::Mat & binary_img); + std::vector matchLights(const std::vector & lights); + + // For debug usage + cv::Mat getAllNumbersImage(); + void drawResults(cv::Mat & img); + + int binary_thres; + int detect_color; + LightParams l; + ArmorParams a; + + std::unique_ptr classifier; + + // Debug msgs + cv::Mat binary_img; + auto_aim_interfaces::msg::DebugLights debug_lights; + auto_aim_interfaces::msg::DebugArmors debug_armors; + +private: + bool isLight(const Light & possible_light); + bool containLight( + const Light & light_1, const Light & light_2, const std::vector & lights); + ArmorType isArmor(const Light & light_1, const Light & light_2); + + std::vector lights_; + std::vector armors_; +}; + +} // namespace rm_auto_aim + +#endif // ARMOR_DETECTOR__DETECTOR_HPP_ diff --git a/armor_detector/include/armor_detector/detector_node.hpp b/armor_detector/include/armor_detector/detector_node.hpp new file mode 100644 index 0000000..0cd88f3 --- /dev/null +++ b/armor_detector/include/armor_detector/detector_node.hpp @@ -0,0 +1,81 @@ +// Copyright 2022 Chen Jun +// Licensed under the MIT License. + +#ifndef ARMOR_DETECTOR__DETECTOR_NODE_HPP_ +#define ARMOR_DETECTOR__DETECTOR_NODE_HPP_ + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include + +// STD +#include +#include +#include + +#include "armor_detector/detector.hpp" +#include "armor_detector/number_classifier.hpp" +#include "armor_detector/pnp_solver.hpp" +#include "auto_aim_interfaces/msg/armors.hpp" + +namespace rm_auto_aim +{ + +class ArmorDetectorNode : public rclcpp::Node +{ +public: + ArmorDetectorNode(const rclcpp::NodeOptions & options); + +private: + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg); + + std::unique_ptr initDetector(); + std::vector detectArmors(const sensor_msgs::msg::Image::ConstSharedPtr & img_msg); + + void createDebugPublishers(); + void destroyDebugPublishers(); + + void publishMarkers(); + + // Armor Detector + std::unique_ptr detector_; + + // Detected armors publisher + auto_aim_interfaces::msg::Armors armors_msg_; + rclcpp::Publisher::SharedPtr armors_pub_; + + // Visualization marker publisher + visualization_msgs::msg::Marker armor_marker_; + visualization_msgs::msg::Marker text_marker_; + visualization_msgs::msg::MarkerArray marker_array_; + rclcpp::Publisher::SharedPtr marker_pub_; + + // Camera info part + rclcpp::Subscription::SharedPtr cam_info_sub_; + cv::Point2f cam_center_; + std::shared_ptr cam_info_; + std::unique_ptr pnp_solver_; + + // Image subscrpition + rclcpp::Subscription::SharedPtr img_sub_; + + // Debug information + bool debug_; + std::shared_ptr debug_param_sub_; + std::shared_ptr debug_cb_handle_; + rclcpp::Publisher::SharedPtr lights_data_pub_; + rclcpp::Publisher::SharedPtr armors_data_pub_; + image_transport::Publisher binary_img_pub_; + image_transport::Publisher number_img_pub_; + image_transport::Publisher result_img_pub_; +}; + +} // namespace rm_auto_aim + +#endif // ARMOR_DETECTOR__DETECTOR_NODE_HPP_ diff --git a/armor_detector/include/armor_detector/number_classifier.hpp b/armor_detector/include/armor_detector/number_classifier.hpp new file mode 100644 index 0000000..7a68799 --- /dev/null +++ b/armor_detector/include/armor_detector/number_classifier.hpp @@ -0,0 +1,40 @@ +// Copyright 2022 Chen Jun + +#ifndef ARMOR_DETECTOR__NUMBER_CLASSIFIER_HPP_ +#define ARMOR_DETECTOR__NUMBER_CLASSIFIER_HPP_ + +// OpenCV +#include + +// STL +#include +#include +#include +#include +#include + +#include "armor_detector/armor.hpp" + +namespace rm_auto_aim +{ +class NumberClassifier +{ +public: + NumberClassifier( + const std::string & model_path, const std::string & label_path, const double threshold, + const std::vector & ignore_classes = {}); + + void extractNumbers(const cv::Mat & src, std::vector & armors); + + void classify(std::vector & armors); + + double threshold; + +private: + cv::dnn::Net net_; + std::vector class_names_; + std::vector ignore_classes_; +}; +} // namespace rm_auto_aim + +#endif // ARMOR_DETECTOR__NUMBER_CLASSIFIER_HPP_ diff --git a/armor_detector/include/armor_detector/pnp_solver.hpp b/armor_detector/include/armor_detector/pnp_solver.hpp new file mode 100644 index 0000000..e0005ae --- /dev/null +++ b/armor_detector/include/armor_detector/pnp_solver.hpp @@ -0,0 +1,48 @@ +// Copyright 2022 Chen Jun +// Licensed under the MIT License. + +#ifndef ARMOR_DETECTOR__PNP_SOLVER_HPP_ +#define ARMOR_DETECTOR__PNP_SOLVER_HPP_ + +#include +#include + +// STD +#include +#include + +#include "armor_detector/armor.hpp" + +namespace rm_auto_aim +{ +class PnPSolver +{ +public: + PnPSolver( + const std::array & camera_matrix, + const std::vector & distortion_coefficients); + + // Get 3d position + bool solvePnP(const Armor & armor, cv::Mat & rvec, cv::Mat & tvec); + + // Calculate the distance between armor center and image center + float calculateDistanceToCenter(const cv::Point2f & image_point); + +private: + cv::Mat camera_matrix_; + cv::Mat dist_coeffs_; + + // Unit: mm + static constexpr float SMALL_ARMOR_WIDTH = 135; + static constexpr float SMALL_ARMOR_HEIGHT = 55; + static constexpr float LARGE_ARMOR_WIDTH = 225; + static constexpr float LARGE_ARMOR_HEIGHT = 55; + + // Four vertices of armor in 3d + std::vector small_armor_points_; + std::vector large_armor_points_; +}; + +} // namespace rm_auto_aim + +#endif // ARMOR_DETECTOR__PNP_SOLVER_HPP_ diff --git a/armor_detector/model/label.txt b/armor_detector/model/label.txt new file mode 100644 index 0000000..64bf0fd --- /dev/null +++ b/armor_detector/model/label.txt @@ -0,0 +1,9 @@ +1 +2 +3 +4 +5 +outpost +guard +base +negative \ No newline at end of file diff --git a/armor_detector/model/mlp.onnx b/armor_detector/model/mlp.onnx new file mode 100644 index 0000000..2089380 Binary files /dev/null and b/armor_detector/model/mlp.onnx differ diff --git a/armor_detector/package.xml b/armor_detector/package.xml new file mode 100644 index 0000000..94f0a09 --- /dev/null +++ b/armor_detector/package.xml @@ -0,0 +1,37 @@ + + + + armor_detector + 0.1.0 + A template for ROS packages. + Chen Jun + BSD + https://github.com/chenjunnn/rm_auto_aim + https://github.com/chenjunnn/rm_auto_aim/issues + Chen Jun + + + ament_cmake + + + rclcpp + rclcpp_components + sensor_msgs + geometry_msgs + visualization_msgs + message_filters + cv_bridge + image_transport + image_transport_plugins + auto_aim_interfaces + vision_opencv + tf2_geometry_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_clang_format + + + ament_cmake + + diff --git a/armor_detector/src/detector.cpp b/armor_detector/src/detector.cpp new file mode 100644 index 0000000..18887eb --- /dev/null +++ b/armor_detector/src/detector.cpp @@ -0,0 +1,245 @@ +// Copyright (c) 2022 ChenJun +// Licensed under the MIT License. + +// OpenCV +#include +#include +#include +#include +#include + +// STD +#include +#include +#include + +#include "armor_detector/detector.hpp" +#include "auto_aim_interfaces/msg/debug_armor.hpp" +#include "auto_aim_interfaces/msg/debug_light.hpp" + +namespace rm_auto_aim +{ +Detector::Detector( + const int & bin_thres, const int & color, const LightParams & l, const ArmorParams & a) +: binary_thres(bin_thres), detect_color(color), l(l), a(a) +{ +} + +std::vector Detector::detect(const cv::Mat & input) +{ + binary_img = preprocessImage(input); + lights_ = findLights(input, binary_img); + armors_ = matchLights(lights_); + + if (!armors_.empty()) { + classifier->extractNumbers(input, armors_); + classifier->classify(armors_); + } + + return armors_; +} + +cv::Mat Detector::preprocessImage(const cv::Mat & rgb_img) +{ + cv::Mat gray_img; + cv::cvtColor(rgb_img, gray_img, cv::COLOR_RGB2GRAY); + + cv::Mat binary_img; + cv::threshold(gray_img, binary_img, binary_thres, 255, cv::THRESH_BINARY); + + return binary_img; +} + +std::vector Detector::findLights(const cv::Mat & rbg_img, const cv::Mat & binary_img) +{ + using std::vector; + vector> contours; + vector hierarchy; + cv::findContours(binary_img, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + vector lights; + this->debug_lights.data.clear(); + + for (const auto & contour : contours) { + if (contour.size() < 5) continue; + + auto r_rect = cv::minAreaRect(contour); + auto light = Light(r_rect); + + if (isLight(light)) { + auto rect = light.boundingRect(); + if ( // Avoid assertion failed + 0 <= rect.x && 0 <= rect.width && rect.x + rect.width <= rbg_img.cols && 0 <= rect.y && + 0 <= rect.height && rect.y + rect.height <= rbg_img.rows) { + int sum_r = 0, sum_b = 0; + auto roi = rbg_img(rect); + // Iterate through the ROI + for (int i = 0; i < roi.rows; i++) { + for (int j = 0; j < roi.cols; j++) { + if (cv::pointPolygonTest(contour, cv::Point2f(j + rect.x, i + rect.y), false) >= 0) { + // if point is inside contour + sum_r += roi.at(i, j)[0]; + sum_b += roi.at(i, j)[2]; + } + } + } + // Sum of red pixels > sum of blue pixels ? + light.color = sum_r > sum_b ? RED : BLUE; + lights.emplace_back(light); + } + } + } + + return lights; +} + +bool Detector::isLight(const Light & light) +{ + // The ratio of light (short side / long side) + float ratio = light.width / light.length; + bool ratio_ok = l.min_ratio < ratio && ratio < l.max_ratio; + + bool angle_ok = light.tilt_angle < l.max_angle; + + bool is_light = ratio_ok && angle_ok; + + // Fill in debug information + auto_aim_interfaces::msg::DebugLight light_data; + light_data.center_x = light.center.x; + light_data.ratio = ratio; + light_data.angle = light.tilt_angle; + light_data.is_light = is_light; + this->debug_lights.data.emplace_back(light_data); + + return is_light; +} + +std::vector Detector::matchLights(const std::vector & lights) +{ + std::vector armors; + this->debug_armors.data.clear(); + + // Loop all the pairing of lights + for (auto light_1 = lights.begin(); light_1 != lights.end(); light_1++) { + for (auto light_2 = light_1 + 1; light_2 != lights.end(); light_2++) { + if (light_1->color != detect_color || light_2->color != detect_color) continue; + + if (containLight(*light_1, *light_2, lights)) { + continue; + } + + auto type = isArmor(*light_1, *light_2); + if (type != ArmorType::INVALID) { + auto armor = Armor(*light_1, *light_2); + armor.type = type; + armors.emplace_back(armor); + } + } + } + + return armors; +} + +// Check if there is another light in the boundingRect formed by the 2 lights +bool Detector::containLight( + const Light & light_1, const Light & light_2, const std::vector & lights) +{ + auto points = std::vector{light_1.top, light_1.bottom, light_2.top, light_2.bottom}; + auto bounding_rect = cv::boundingRect(points); + + for (const auto & test_light : lights) { + if (test_light.center == light_1.center || test_light.center == light_2.center) continue; + + if ( + bounding_rect.contains(test_light.top) || bounding_rect.contains(test_light.bottom) || + bounding_rect.contains(test_light.center)) { + return true; + } + } + + return false; +} + +ArmorType Detector::isArmor(const Light & light_1, const Light & light_2) +{ + // Ratio of the length of 2 lights (short side / long side) + float light_length_ratio = light_1.length < light_2.length ? light_1.length / light_2.length + : light_2.length / light_1.length; + bool light_ratio_ok = light_length_ratio > a.min_light_ratio; + + // Distance between the center of 2 lights (unit : light length) + float avg_light_length = (light_1.length + light_2.length) / 2; + float center_distance = cv::norm(light_1.center - light_2.center) / avg_light_length; + bool center_distance_ok = (a.min_small_center_distance <= center_distance && + center_distance < a.max_small_center_distance) || + (a.min_large_center_distance <= center_distance && + center_distance < a.max_large_center_distance); + + // Angle of light center connection + cv::Point2f diff = light_1.center - light_2.center; + float angle = std::abs(std::atan(diff.y / diff.x)) / CV_PI * 180; + bool angle_ok = angle < a.max_angle; + + bool is_armor = light_ratio_ok && center_distance_ok && angle_ok; + + // Judge armor type + ArmorType type; + if (is_armor) { + type = center_distance > a.min_large_center_distance ? ArmorType::LARGE : ArmorType::SMALL; + } else { + type = ArmorType::INVALID; + } + + // Fill in debug information + auto_aim_interfaces::msg::DebugArmor armor_data; + armor_data.type = ARMOR_TYPE_STR[static_cast(type)]; + armor_data.center_x = (light_1.center.x + light_2.center.x) / 2; + armor_data.light_ratio = light_length_ratio; + armor_data.center_distance = center_distance; + armor_data.angle = angle; + this->debug_armors.data.emplace_back(armor_data); + + return type; +} + +cv::Mat Detector::getAllNumbersImage() +{ + if (armors_.empty()) { + return cv::Mat(cv::Size(20, 28), CV_8UC1); + } else { + std::vector number_imgs; + number_imgs.reserve(armors_.size()); + for (auto & armor : armors_) { + number_imgs.emplace_back(armor.number_img); + } + cv::Mat all_num_img; + cv::vconcat(number_imgs, all_num_img); + return all_num_img; + } +} + +void Detector::drawResults(cv::Mat & img) +{ + // Draw Lights + for (const auto & light : lights_) { + cv::circle(img, light.top, 3, cv::Scalar(255, 255, 255), 1); + cv::circle(img, light.bottom, 3, cv::Scalar(255, 255, 255), 1); + auto line_color = light.color == RED ? cv::Scalar(255, 255, 0) : cv::Scalar(255, 0, 255); + cv::line(img, light.top, light.bottom, line_color, 1); + } + + // Draw armors + for (const auto & armor : armors_) { + cv::line(img, armor.left_light.top, armor.right_light.bottom, cv::Scalar(0, 255, 0), 2); + cv::line(img, armor.left_light.bottom, armor.right_light.top, cv::Scalar(0, 255, 0), 2); + } + + // Show numbers and confidence + for (const auto & armor : armors_) { + cv::putText( + img, armor.classfication_result, armor.left_light.top, cv::FONT_HERSHEY_SIMPLEX, 0.8, + cv::Scalar(0, 255, 255), 2); + } +} + +} // namespace rm_auto_aim diff --git a/armor_detector/src/detector_node.cpp b/armor_detector/src/detector_node.cpp new file mode 100644 index 0000000..4549411 --- /dev/null +++ b/armor_detector/src/detector_node.cpp @@ -0,0 +1,292 @@ +// Copyright 2022 Chen Jun +// Licensed under the MIT License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// STD +#include +#include +#include +#include +#include + +#include "armor_detector/armor.hpp" +#include "armor_detector/detector_node.hpp" + +namespace rm_auto_aim +{ +ArmorDetectorNode::ArmorDetectorNode(const rclcpp::NodeOptions & options) +: Node("armor_detector", options) +{ + RCLCPP_INFO(this->get_logger(), "Starting DetectorNode!"); + + // Detector + detector_ = initDetector(); + + // Armors Publisher + armors_pub_ = this->create_publisher( + "/detector/armors", rclcpp::SensorDataQoS()); + + // Visualization Marker Publisher + // See http://wiki.ros.org/rviz/DisplayTypes/Marker + armor_marker_.ns = "armors"; + armor_marker_.action = visualization_msgs::msg::Marker::ADD; + armor_marker_.type = visualization_msgs::msg::Marker::CUBE; + armor_marker_.scale.x = 0.05; + armor_marker_.scale.z = 0.125; + armor_marker_.color.a = 1.0; + armor_marker_.color.g = 0.5; + armor_marker_.color.b = 1.0; + armor_marker_.lifetime = rclcpp::Duration::from_seconds(0.1); + + text_marker_.ns = "classification"; + text_marker_.action = visualization_msgs::msg::Marker::ADD; + text_marker_.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker_.scale.z = 0.1; + text_marker_.color.a = 1.0; + text_marker_.color.r = 1.0; + text_marker_.color.g = 1.0; + text_marker_.color.b = 1.0; + text_marker_.lifetime = rclcpp::Duration::from_seconds(0.1); + + marker_pub_ = + this->create_publisher("/detector/marker", 10); + + // Debug Publishers + debug_ = this->declare_parameter("debug", false); + if (debug_) { + createDebugPublishers(); + } + + // Debug param change moniter + debug_param_sub_ = std::make_shared(this); + debug_cb_handle_ = + debug_param_sub_->add_parameter_callback("debug", [this](const rclcpp::Parameter & p) { + debug_ = p.as_bool(); + debug_ ? createDebugPublishers() : destroyDebugPublishers(); + }); + + cam_info_sub_ = this->create_subscription( + "/camera_info", rclcpp::SensorDataQoS(), + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info) { + cam_center_ = cv::Point2f(camera_info->k[2], camera_info->k[5]); + cam_info_ = std::make_shared(*camera_info); + pnp_solver_ = std::make_unique(camera_info->k, camera_info->d); + cam_info_sub_.reset(); + }); + + img_sub_ = this->create_subscription( + "/image_raw", rclcpp::SensorDataQoS(), + std::bind(&ArmorDetectorNode::imageCallback, this, std::placeholders::_1)); +} + +void ArmorDetectorNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg) +{ + auto armors = detectArmors(img_msg); + + if (pnp_solver_ != nullptr) { + armors_msg_.header = armor_marker_.header = text_marker_.header = img_msg->header; + armors_msg_.armors.clear(); + marker_array_.markers.clear(); + armor_marker_.id = 0; + text_marker_.id = 0; + + auto_aim_interfaces::msg::Armor armor_msg; + for (const auto & armor : armors) { + cv::Mat rvec, tvec; + bool success = pnp_solver_->solvePnP(armor, rvec, tvec); + if (success) { + // Fill basic info + armor_msg.type = ARMOR_TYPE_STR[static_cast(armor.type)]; + armor_msg.number = armor.number; + + // Fill pose + armor_msg.pose.position.x = tvec.at(0); + armor_msg.pose.position.y = tvec.at(1); + armor_msg.pose.position.z = tvec.at(2); + // rvec to 3x3 rotation matrix + cv::Mat rotation_matrix; + cv::Rodrigues(rvec, rotation_matrix); + // rotation matrix to quaternion + tf2::Matrix3x3 tf2_rotation_matrix( + rotation_matrix.at(0, 0), rotation_matrix.at(0, 1), + rotation_matrix.at(0, 2), rotation_matrix.at(1, 0), + rotation_matrix.at(1, 1), rotation_matrix.at(1, 2), + rotation_matrix.at(2, 0), rotation_matrix.at(2, 1), + rotation_matrix.at(2, 2)); + tf2::Quaternion tf2_q; + tf2_rotation_matrix.getRotation(tf2_q); + armor_msg.pose.orientation = tf2::toMsg(tf2_q); + + // Fill the distance to image center + armor_msg.distance_to_image_center = pnp_solver_->calculateDistanceToCenter(armor.center); + + // Fill the markers + armor_marker_.id++; + armor_marker_.scale.y = armor.type == ArmorType::SMALL ? 0.135 : 0.23; + armor_marker_.pose = armor_msg.pose; + text_marker_.id++; + text_marker_.pose.position = armor_msg.pose.position; + text_marker_.pose.position.y -= 0.1; + text_marker_.text = armor.classfication_result; + armors_msg_.armors.emplace_back(armor_msg); + marker_array_.markers.emplace_back(armor_marker_); + marker_array_.markers.emplace_back(text_marker_); + } else { + RCLCPP_WARN(this->get_logger(), "PnP failed!"); + } + } + + // Publishing detected armors + armors_pub_->publish(armors_msg_); + + // Publishing marker + publishMarkers(); + } +} + +std::unique_ptr ArmorDetectorNode::initDetector() +{ + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.integer_range.resize(1); + param_desc.integer_range[0].step = 1; + param_desc.integer_range[0].from_value = 0; + param_desc.integer_range[0].to_value = 255; + int binary_thres = declare_parameter("binary_thres", 160, param_desc); + + param_desc.description = "0-RED, 1-BLUE"; + param_desc.integer_range[0].from_value = 0; + param_desc.integer_range[0].to_value = 1; + auto detect_color = declare_parameter("detect_color", RED, param_desc); + + Detector::LightParams l_params = { + .min_ratio = declare_parameter("light.min_ratio", 0.1), + .max_ratio = declare_parameter("light.max_ratio", 0.4), + .max_angle = declare_parameter("light.max_angle", 40.0)}; + + Detector::ArmorParams a_params = { + .min_light_ratio = declare_parameter("armor.min_light_ratio", 0.7), + .min_small_center_distance = declare_parameter("armor.min_small_center_distance", 0.8), + .max_small_center_distance = declare_parameter("armor.max_small_center_distance", 3.2), + .min_large_center_distance = declare_parameter("armor.min_large_center_distance", 3.2), + .max_large_center_distance = declare_parameter("armor.max_large_center_distance", 5.5), + .max_angle = declare_parameter("armor.max_angle", 35.0)}; + + auto detector = std::make_unique(binary_thres, detect_color, l_params, a_params); + + // Init classifier + auto pkg_path = ament_index_cpp::get_package_share_directory("armor_detector"); + auto model_path = pkg_path + "/model/mlp.onnx"; + auto label_path = pkg_path + "/model/label.txt"; + double threshold = this->declare_parameter("classifier_threshold", 0.7); + std::vector ignore_classes = + this->declare_parameter("ignore_classes", std::vector{"negative"}); + detector->classifier = + std::make_unique(model_path, label_path, threshold, ignore_classes); + + return detector; +} + +std::vector ArmorDetectorNode::detectArmors( + const sensor_msgs::msg::Image::ConstSharedPtr & img_msg) +{ + // Convert ROS img to cv::Mat + auto img = cv_bridge::toCvShare(img_msg, "rgb8")->image; + + // Update params + detector_->binary_thres = get_parameter("binary_thres").as_int(); + detector_->detect_color = get_parameter("detect_color").as_int(); + detector_->classifier->threshold = get_parameter("classifier_threshold").as_double(); + + auto armors = detector_->detect(img); + + auto final_time = this->now(); + auto latency = (final_time - img_msg->header.stamp).seconds() * 1000; + RCLCPP_DEBUG_STREAM(this->get_logger(), "Latency: " << latency << "ms"); + + // Publish debug info + if (debug_) { + binary_img_pub_.publish( + cv_bridge::CvImage(img_msg->header, "mono8", detector_->binary_img).toImageMsg()); + + // Sort lights and armors data by x coordinate + std::sort( + detector_->debug_lights.data.begin(), detector_->debug_lights.data.end(), + [](const auto & l1, const auto & l2) { return l1.center_x < l2.center_x; }); + std::sort( + detector_->debug_armors.data.begin(), detector_->debug_armors.data.end(), + [](const auto & a1, const auto & a2) { return a1.center_x < a2.center_x; }); + + lights_data_pub_->publish(detector_->debug_lights); + armors_data_pub_->publish(detector_->debug_armors); + + if (!armors.empty()) { + auto all_num_img = detector_->getAllNumbersImage(); + number_img_pub_.publish( + *cv_bridge::CvImage(img_msg->header, "mono8", all_num_img).toImageMsg()); + } + + detector_->drawResults(img); + // Draw camera center + cv::circle(img, cam_center_, 5, cv::Scalar(255, 0, 0), 2); + // Draw latency + std::stringstream latency_ss; + latency_ss << "Latency: " << std::fixed << std::setprecision(2) << latency << "ms"; + auto latency_s = latency_ss.str(); + cv::putText( + img, latency_s, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 0), 2); + result_img_pub_.publish(cv_bridge::CvImage(img_msg->header, "rgb8", img).toImageMsg()); + } + + return armors; +} + +void ArmorDetectorNode::createDebugPublishers() +{ + lights_data_pub_ = + this->create_publisher("/detector/debug_lights", 10); + armors_data_pub_ = + this->create_publisher("/detector/debug_armors", 10); + + binary_img_pub_ = image_transport::create_publisher(this, "/detector/binary_img"); + number_img_pub_ = image_transport::create_publisher(this, "/detector/number_img"); + result_img_pub_ = image_transport::create_publisher(this, "/detector/result_img"); +} + +void ArmorDetectorNode::destroyDebugPublishers() +{ + lights_data_pub_.reset(); + armors_data_pub_.reset(); + + binary_img_pub_.shutdown(); + number_img_pub_.shutdown(); + result_img_pub_.shutdown(); +} + +void ArmorDetectorNode::publishMarkers() +{ + using Marker = visualization_msgs::msg::Marker; + armor_marker_.action = armors_msg_.armors.empty() ? Marker::DELETE : Marker::ADD; + marker_array_.markers.emplace_back(armor_marker_); + marker_pub_->publish(marker_array_); +} + +} // namespace rm_auto_aim + +#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(rm_auto_aim::ArmorDetectorNode) diff --git a/armor_detector/src/number_classifier.cpp b/armor_detector/src/number_classifier.cpp new file mode 100644 index 0000000..dacce60 --- /dev/null +++ b/armor_detector/src/number_classifier.cpp @@ -0,0 +1,147 @@ +// Copyright 2022 Chen Jun +// Licensed under the MIT License. + +// OpenCV +#include +#include +#include +#include +#include +#include +#include +#include + +// STL +#include +#include +#include +#include +#include +#include + +#include "armor_detector/armor.hpp" +#include "armor_detector/number_classifier.hpp" + +namespace rm_auto_aim +{ +NumberClassifier::NumberClassifier( + const std::string & model_path, const std::string & label_path, const double thre, + const std::vector & ignore_classes) +: threshold(thre), ignore_classes_(ignore_classes) +{ + net_ = cv::dnn::readNetFromONNX(model_path); + + std::ifstream label_file(label_path); + std::string line; + while (std::getline(label_file, line)) { + class_names_.push_back(line); + } +} + +void NumberClassifier::extractNumbers(const cv::Mat & src, std::vector & armors) +{ + // Light length in image + const int light_length = 12; + // Image size after warp + const int warp_height = 28; + const int small_armor_width = 32; + const int large_armor_width = 54; + // Number ROI size + const cv::Size roi_size(20, 28); + + for (auto & armor : armors) { + // Warp perspective transform + cv::Point2f lights_vertices[4] = { + armor.left_light.bottom, armor.left_light.top, armor.right_light.top, + armor.right_light.bottom}; + + const int top_light_y = (warp_height - light_length) / 2 - 1; + const int bottom_light_y = top_light_y + light_length; + const int warp_width = armor.type == ArmorType::SMALL ? small_armor_width : large_armor_width; + cv::Point2f target_vertices[4] = { + cv::Point(0, bottom_light_y), + cv::Point(0, top_light_y), + cv::Point(warp_width - 1, top_light_y), + cv::Point(warp_width - 1, bottom_light_y), + }; + cv::Mat number_image; + auto rotation_matrix = cv::getPerspectiveTransform(lights_vertices, target_vertices); + cv::warpPerspective(src, number_image, rotation_matrix, cv::Size(warp_width, warp_height)); + + // Get ROI + number_image = + number_image(cv::Rect(cv::Point((warp_width - roi_size.width) / 2, 0), roi_size)); + + // Binarize + cv::cvtColor(number_image, number_image, cv::COLOR_RGB2GRAY); + cv::threshold(number_image, number_image, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU); + + armor.number_img = number_image; + } +} + +void NumberClassifier::classify(std::vector & armors) +{ + for (auto & armor : armors) { + cv::Mat image = armor.number_img.clone(); + + // Normalize + image = image / 255.0; + + // Create blob from image + cv::Mat blob; + cv::dnn::blobFromImage(image, blob); + + // Set the input blob for the neural network + net_.setInput(blob); + // Forward pass the image blob through the model + cv::Mat outputs = net_.forward(); + + // Do softmax + float max_prob = *std::max_element(outputs.begin(), outputs.end()); + cv::Mat softmax_prob; + cv::exp(outputs - max_prob, softmax_prob); + float sum = static_cast(cv::sum(softmax_prob)[0]); + softmax_prob /= sum; + + double confidence; + cv::Point class_id_point; + minMaxLoc(softmax_prob.reshape(1, 1), nullptr, &confidence, nullptr, &class_id_point); + int label_id = class_id_point.x; + + armor.confidence = confidence; + armor.number = class_names_[label_id]; + + std::stringstream result_ss; + result_ss << armor.number << ": " << std::fixed << std::setprecision(1) + << armor.confidence * 100.0 << "%"; + armor.classfication_result = result_ss.str(); + } + + armors.erase( + std::remove_if( + armors.begin(), armors.end(), + [this](const Armor & armor) { + if (armor.confidence < threshold) { + return true; + } + + for (const auto & ignore_class : ignore_classes_) { + if (armor.number == ignore_class) { + return true; + } + } + + bool mismatch_armor_type = false; + if (armor.type == ArmorType::LARGE) { + mismatch_armor_type = + armor.number == "outpost" || armor.number == "2" || armor.number == "guard"; + } else if (armor.type == ArmorType::SMALL) { + mismatch_armor_type = armor.number == "1" || armor.number == "base"; + } + return mismatch_armor_type; + }), + armors.end()); +} + +} // namespace rm_auto_aim diff --git a/armor_detector/src/pnp_solver.cpp b/armor_detector/src/pnp_solver.cpp new file mode 100644 index 0000000..d4b4f72 --- /dev/null +++ b/armor_detector/src/pnp_solver.cpp @@ -0,0 +1,58 @@ +// Copyright 2022 Chen Jun + +#include "armor_detector/pnp_solver.hpp" + +#include +#include + +namespace rm_auto_aim +{ +PnPSolver::PnPSolver( + const std::array & camera_matrix, const std::vector & dist_coeffs) +: camera_matrix_(cv::Mat(3, 3, CV_64F, const_cast(camera_matrix.data())).clone()), + dist_coeffs_(cv::Mat(1, 5, CV_64F, const_cast(dist_coeffs.data())).clone()) +{ + // Unit: m + constexpr double small_half_y = SMALL_ARMOR_WIDTH / 2.0 / 1000.0; + constexpr double small_half_z = SMALL_ARMOR_HEIGHT / 2.0 / 1000.0; + constexpr double large_half_y = LARGE_ARMOR_WIDTH / 2.0 / 1000.0; + constexpr double large_half_z = LARGE_ARMOR_HEIGHT / 2.0 / 1000.0; + + // Start from bottom left in clockwise order + // Model coordinate: x forward, y left, z up + small_armor_points_.emplace_back(cv::Point3f(0, small_half_y, -small_half_z)); + small_armor_points_.emplace_back(cv::Point3f(0, small_half_y, small_half_z)); + small_armor_points_.emplace_back(cv::Point3f(0, -small_half_y, small_half_z)); + small_armor_points_.emplace_back(cv::Point3f(0, -small_half_y, -small_half_z)); + + large_armor_points_.emplace_back(cv::Point3f(0, large_half_y, -large_half_z)); + large_armor_points_.emplace_back(cv::Point3f(0, large_half_y, large_half_z)); + large_armor_points_.emplace_back(cv::Point3f(0, -large_half_y, large_half_z)); + large_armor_points_.emplace_back(cv::Point3f(0, -large_half_y, -large_half_z)); +} + +bool PnPSolver::solvePnP(const Armor & armor, cv::Mat & rvec, cv::Mat & tvec) +{ + std::vector image_armor_points; + + // Fill in image points + image_armor_points.emplace_back(armor.left_light.bottom); + image_armor_points.emplace_back(armor.left_light.top); + image_armor_points.emplace_back(armor.right_light.top); + image_armor_points.emplace_back(armor.right_light.bottom); + + // Solve pnp + auto object_points = armor.type == ArmorType::SMALL ? small_armor_points_ : large_armor_points_; + return cv::solvePnP( + object_points, image_armor_points, camera_matrix_, dist_coeffs_, rvec, tvec, false, + cv::SOLVEPNP_IPPE); +} + +float PnPSolver::calculateDistanceToCenter(const cv::Point2f & image_point) +{ + float cx = camera_matrix_.at(0, 2); + float cy = camera_matrix_.at(1, 2); + return cv::norm(image_point - cv::Point2f(cx, cy)); +} + +} // namespace rm_auto_aim diff --git a/armor_detector/test/test_node_startup.cpp b/armor_detector/test/test_node_startup.cpp new file mode 100644 index 0000000..d584cef --- /dev/null +++ b/armor_detector/test/test_node_startup.cpp @@ -0,0 +1,28 @@ +// Copyright 2022 Chen Jun + +#include + +#include +#include +#include + +// STD +#include + +#include "armor_detector/detector_node.hpp" + +TEST(ArmorDetectorNodeTest, NodeStartupTest) +{ + rclcpp::NodeOptions options; + auto node = std::make_shared(options); + node.reset(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + auto result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/armor_detector/test/test_number_cls.cpp b/armor_detector/test/test_number_cls.cpp new file mode 100644 index 0000000..2f216df --- /dev/null +++ b/armor_detector/test/test_number_cls.cpp @@ -0,0 +1,53 @@ +// Copyright 2022 Chen Jun + +#include + +#include +#include + +// STL +#include +#include +#include +#include +#include + +#include "armor_detector/number_classifier.hpp" + +using hrc = std::chrono::high_resolution_clock; + +TEST(test_nc, benchmark) +{ + auto pkg_path = ament_index_cpp::get_package_share_directory("armor_detector"); + auto model_path = pkg_path + "/model/mlp.onnx"; + auto label_path = pkg_path + "/model/label.txt"; + rm_auto_aim::NumberClassifier nc(model_path, label_path, 0.5); + + auto dummy_armors = std::vector(1); + auto test_mat = cv::Mat(20, 28, CV_8UC1); + dummy_armors[0].number_img = test_mat; + + int loop_num = 200; + int warm_up = 30; + + double time_min = DBL_MAX; + double time_max = -DBL_MAX; + double time_avg = 0; + + for (int i = 0; i < warm_up + loop_num; i++) { + auto start = hrc::now(); + nc.classify(dummy_armors); + auto end = hrc::now(); + double time = std::chrono::duration(end - start).count(); + if (i >= warm_up) { + time_min = std::min(time_min, time); + time_max = std::max(time_max, time); + time_avg += time; + } + } + time_avg /= loop_num; + + std::cout << "time_min: " << time_min << "ms" << std::endl; + std::cout << "time_max: " << time_max << "ms" << std::endl; + std::cout << "time_avg: " << time_avg << "ms" << std::endl; +} diff --git a/armor_tracker/CMakeLists.txt b/armor_tracker/CMakeLists.txt new file mode 100644 index 0000000..e87210d --- /dev/null +++ b/armor_tracker/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.10) +project(armor_tracker) + +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +add_definitions(-Wall -Werror) + +## Export compile commands for clangd +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +####################### +## Find dependencies ## +####################### + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +########### +## Build ## +########### + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN rm_auto_aim::ArmorTrackerNode + EXECUTABLE ${PROJECT_NAME}_node +) + +############# +## Testing ## +############# + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_copyright + ament_cmake_uncrustify + ament_cmake_cpplint + ) + ament_lint_auto_find_test_dependencies() + + # find_package(ament_cmake_gtest) + # set(TEST_NAME test_kalman_filter) + # ament_add_gtest(${TEST_NAME} test/${TEST_NAME}.cpp) + # target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) +endif() + +############# +## Install ## +############# + +ament_auto_package() \ No newline at end of file diff --git a/armor_tracker/README.md b/armor_tracker/README.md new file mode 100644 index 0000000..bed3067 --- /dev/null +++ b/armor_tracker/README.md @@ -0,0 +1,84 @@ +# armor_tracker + +- [ArmorTrackerNode](#armortrackernode) + - [Tracker](#tracker) + - [KalmanFilter](#kalmanfilter) + +## ArmorTrackerNode +装甲板处理节点 + +订阅识别节点发布的装甲板三维位置及机器人的坐标转换信息，将装甲板三维位置变换到指定惯性系（一般是以云台中心为原点，IMU 上电时的 Yaw 朝向为 X 轴的惯性系）下，然后将装甲板目标送入跟踪器中，输出跟踪机器人在指定惯性系下的状态 + +订阅： +- 已识别到的装甲板 `/detector/armors` +- 机器人的坐标转换信息 `/tf` `/tf_static` + +发布： +- 最终锁定的目标 `/tracker/target` + +参数： +- 跟踪器参数 tracker + - 两帧间目标可匹配的最大距离 max_match_distance + - `DETECTING` 状态进入 `TRACKING` 状态的阈值 tracking_threshold + - `TRACKING` 状态进入 `LOST` 状态的阈值 lost_threshold + +## ExtendedKalmanFilter + +$$ x_c = x_a + r * cos (\theta) $$ +$$ y_c = y_a + r * sin (\theta) $$ + +$$ x = [x_c, y_c,z, yaw, v_{xc}, v_{yc},v_z, v_{yaw}, r]^T $$ + +参考 OpenCV 中的卡尔曼滤波器使用 Eigen 进行了实现 + +[卡尔曼滤波器](https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2) + +![](docs/Kalman_filter_model.png) + +考虑到自瞄任务中对于目标只有观测没有控制，所以输入－控制模型 $B$ 和控制器向量 $u$ 可忽略。 + +预测及更新的公式如下： + +预测： + +$$ x_{k|k-1} = F * x_{k-1|k-1} $$ + +$$ P_{k|k-1} = F * P_{k-1|k-1}* F^T + Q $$ + +更新: + +$$ K = P_{k|k-1} * H^T * (H * P_{k|k-1} * H^T + R)^{-1} $$ + +$$ x_{k|k} = x_{k|k-1} + K * (z_k - H * x_{k|k-1}) $$ + +$$ P_{k|k} = (I - K * H) * P_{k|k-1} $$ + +## Tracker + +参考 [SORT(Simple online and realtime tracking)](https://ieeexplore.ieee.org/abstract/document/7533003/) 中对于目标匹配的方法，使用卡尔曼滤波器对单目标在三维空间中进行跟踪 + +在此跟踪器中，卡尔曼滤波器观测量为目标在指定惯性系中的位置（xyz），状态量为目标位置及速度（xyz+vx vy vz） + +在对目标的运动模型建模为在指定惯性系中的匀速线性运动，即状态转移为 $x_{pre} = x_{post} + v_{post} * dt$ + +目标关联的判断依据为三维位置的 L2 欧式距离 + +跟踪器共有四个状态： +- `DETECTING` ：短暂识别到目标，需要更多帧识别信息才能进入跟踪状态 +- `TRACKING` ：跟踪器正常跟踪目标中 +- `TEMP_LOST` ：跟踪器短暂丢失目标，通过卡尔曼滤波器预测目标 +- `LOST` ：跟踪器完全丢失目标 + +工作流程： + +- init： + + 跟踪器默认选择离相机光心最近的目标作为跟踪对象，选择目标后初始化卡尔曼滤波器，初始状态设为当前目标位置，速度设为 0 + +- update: + + 首先由卡尔曼滤波器得到目标在当前帧的预测位置，然后遍历当前帧中的目标位置与预测位置进行匹配，若当前帧不存在目标或所有目标位置与预测位置的偏差都过大则认为目标丢失，重置卡尔曼滤波器。 + + 最后选取位置相差最小的目标作为最佳匹配项，更新卡尔曼滤波器，将更新后的状态作为跟踪器的结果输出 + + diff --git a/armor_tracker/docs/Kalman_filter_model.png b/armor_tracker/docs/Kalman_filter_model.png new file mode 100644 index 0000000..f603ff2 Binary files /dev/null and b/armor_tracker/docs/Kalman_filter_model.png differ diff --git a/armor_tracker/include/armor_tracker/extended_kalman_filter.hpp b/armor_tracker/include/armor_tracker/extended_kalman_filter.hpp new file mode 100644 index 0000000..1e7f118 --- /dev/null +++ b/armor_tracker/include/armor_tracker/extended_kalman_filter.hpp @@ -0,0 +1,74 @@ +// Copyright 2022 Chen Jun + +#ifndef ARMOR_PROCESSOR__KALMAN_FILTER_HPP_ +#define ARMOR_PROCESSOR__KALMAN_FILTER_HPP_ + +#include +#include + +namespace rm_auto_aim +{ + +class ExtendedKalmanFilter +{ +public: + ExtendedKalmanFilter() = default; + + using VecVecFunc = std::function; + using VecMatFunc = std::function; + using VoidMatFunc = std::function; + + explicit ExtendedKalmanFilter( + const VecVecFunc & f, const VecVecFunc & h, const VecMatFunc & j_f, const VecMatFunc & j_h, + const VoidMatFunc & u_q, const VecMatFunc & u_r, const Eigen::MatrixXd & P0); + + // Set the initial state + void setState(const Eigen::VectorXd & x0); + + // Compute a predicted state + Eigen::MatrixXd predict(); + + // Update the estimated state based on measurement + Eigen::MatrixXd update(const Eigen::VectorXd & z); + +private: + // Process nonlinear vector function + VecVecFunc f; + // Observation nonlinear vector function + VecVecFunc h; + // Jacobian of f() + VecMatFunc jacobian_f; + Eigen::MatrixXd F; + // Jacobian of h() + VecMatFunc jacobian_h; + Eigen::MatrixXd H; + // Process noise covariance matrix + VoidMatFunc update_Q; + Eigen::MatrixXd Q; + // Measurement noise covariance matrix + VecMatFunc update_R; + Eigen::MatrixXd R; + + // Priori error estimate covariance matrix + Eigen::MatrixXd P_pri; + // Posteriori error estimate covariance matrix + Eigen::MatrixXd P_post; + + // Kalman gain + Eigen::MatrixXd K; + + // System dimensions + int n; + + // N-size identity + Eigen::MatrixXd I; + + // Priori state + Eigen::VectorXd x_pri; + // Posteriori state + Eigen::VectorXd x_post; +}; + +} // namespace rm_auto_aim + +#endif // ARMOR_PROCESSOR__KALMAN_FILTER_HPP_ diff --git a/armor_tracker/include/armor_tracker/tracker.hpp b/armor_tracker/include/armor_tracker/tracker.hpp new file mode 100644 index 0000000..b094ce2 --- /dev/null +++ b/armor_tracker/include/armor_tracker/tracker.hpp @@ -0,0 +1,87 @@ +// Copyright 2022 Chen Jun + +#ifndef ARMOR_PROCESSOR__TRACKER_HPP_ +#define ARMOR_PROCESSOR__TRACKER_HPP_ + +// Eigen +#include + +// ROS +#include +#include +#include + +// STD +#include +#include + +#include "armor_tracker/extended_kalman_filter.hpp" +#include "auto_aim_interfaces/msg/armors.hpp" +#include "auto_aim_interfaces/msg/target.hpp" + +namespace rm_auto_aim +{ + +enum class ArmorsNum { NORMAL_4 = 4, BALANCE_2 = 2, OUTPOST_3 = 3 }; + +class Tracker +{ +public: + Tracker(double max_match_distance, double max_match_yaw_diff); + + using Armors = auto_aim_interfaces::msg::Armors; + using Armor = auto_aim_interfaces::msg::Armor; + + void init(const Armors::SharedPtr & armors_msg); + + void update(const Armors::SharedPtr & armors_msg); + + ExtendedKalmanFilter ekf; + + int tracking_thres; + int lost_thres; + + enum State { + LOST, + DETECTING, + TRACKING, + TEMP_LOST, + } tracker_state; + + std::string tracked_id; + Armor tracked_armor; + ArmorsNum tracked_armors_num; + + double info_position_diff; + double info_yaw_diff; + + Eigen::VectorXd measurement; + + Eigen::VectorXd target_state; + + // To store another pair of armors message + double dz, another_r; + +private: + void initEKF(const Armor & a); + + void updateArmorsNum(const Armor & a); + + void handleArmorJump(const Armor & a); + + double orientationToYaw(const geometry_msgs::msg::Quaternion & q); + + Eigen::Vector3d getArmorPositionFromState(const Eigen::VectorXd & x); + + double max_match_distance_; + double max_match_yaw_diff_; + + int detect_count_; + int lost_count_; + + double last_yaw_; +}; + +} // namespace rm_auto_aim + +#endif // ARMOR_PROCESSOR__TRACKER_HPP_ diff --git a/armor_tracker/include/armor_tracker/tracker_node.hpp b/armor_tracker/include/armor_tracker/tracker_node.hpp new file mode 100644 index 0000000..064fd78 --- /dev/null +++ b/armor_tracker/include/armor_tracker/tracker_node.hpp @@ -0,0 +1,80 @@ +// Copyright 2022 Chen Jun + +#ifndef ARMOR_PROCESSOR__PROCESSOR_NODE_HPP_ +#define ARMOR_PROCESSOR__PROCESSOR_NODE_HPP_ + +// ROS +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// STD +#include +#include +#include + +#include "armor_tracker/tracker.hpp" +#include "auto_aim_interfaces/msg/armors.hpp" +#include "auto_aim_interfaces/msg/target.hpp" +#include "auto_aim_interfaces/msg/tracker_info.hpp" + +namespace rm_auto_aim +{ +using tf2_filter = tf2_ros::MessageFilter; +class ArmorTrackerNode : public rclcpp::Node +{ +public: + explicit ArmorTrackerNode(const rclcpp::NodeOptions & options); + +private: + void armorsCallback(const auto_aim_interfaces::msg::Armors::SharedPtr armors_ptr); + + void publishMarkers(const auto_aim_interfaces::msg::Target & target_msg); + + // Maximum allowable armor distance in the XOY plane + double max_armor_distance_; + + // The time when the last message was received + rclcpp::Time last_time_; + double dt_; + + // Armor tracker + double s2qxyz_, s2qyaw_, s2qr_; + double r_xyz_factor, r_yaw; + double lost_time_thres_; + std::unique_ptr tracker_; + + // Reset tracker service + rclcpp::Service::SharedPtr reset_tracker_srv_; + + // Subscriber with tf2 message_filter + std::string target_frame_; + std::shared_ptr tf2_buffer_; + std::shared_ptr tf2_listener_; + message_filters::Subscriber armors_sub_; + std::shared_ptr tf2_filter_; + + // Tracker info publisher + rclcpp::Publisher::SharedPtr info_pub_; + + // Publisher + rclcpp::Publisher::SharedPtr target_pub_; + + // Visualization marker publisher + visualization_msgs::msg::Marker position_marker_; + visualization_msgs::msg::Marker linear_v_marker_; + visualization_msgs::msg::Marker angular_v_marker_; + visualization_msgs::msg::Marker armor_marker_; + rclcpp::Publisher::SharedPtr marker_pub_; +}; + +} // namespace rm_auto_aim + +#endif // ARMOR_PROCESSOR__PROCESSOR_NODE_HPP_ diff --git a/armor_tracker/package.xml b/armor_tracker/package.xml new file mode 100644 index 0000000..a585a34 --- /dev/null +++ b/armor_tracker/package.xml @@ -0,0 +1,35 @@ + + + + armor_tracker + 0.1.0 + A template for ROS packages. + Chen Jun + BSD + https://github.com/chenjunnn/rm_auto_aim + https://github.com/chenjunnn/rm_auto_aim/issues + Chen Jun + + + ament_cmake + + + rclcpp + rclcpp_components + eigen + angles + std_srvs + geometry_msgs + visualization_msgs + message_filters + tf2_geometry_msgs + auto_aim_interfaces + + ament_lint_auto + ament_lint_common + ament_cmake_clang_format + + + ament_cmake + + diff --git a/armor_tracker/src/extended_kalman_filter.cpp b/armor_tracker/src/extended_kalman_filter.cpp new file mode 100644 index 0000000..fe1a56c --- /dev/null +++ b/armor_tracker/src/extended_kalman_filter.cpp @@ -0,0 +1,51 @@ +// Copyright 2022 Chen Jun + +#include "armor_tracker/extended_kalman_filter.hpp" + +namespace rm_auto_aim +{ +ExtendedKalmanFilter::ExtendedKalmanFilter( + const VecVecFunc & f, const VecVecFunc & h, const VecMatFunc & j_f, const VecMatFunc & j_h, + const VoidMatFunc & u_q, const VecMatFunc & u_r, const Eigen::MatrixXd & P0) +: f(f), + h(h), + jacobian_f(j_f), + jacobian_h(j_h), + update_Q(u_q), + update_R(u_r), + P_post(P0), + n(P0.rows()), + I(Eigen::MatrixXd::Identity(n, n)), + x_pri(n), + x_post(n) +{ +} + +void ExtendedKalmanFilter::setState(const Eigen::VectorXd & x0) { x_post = x0; } + +Eigen::MatrixXd ExtendedKalmanFilter::predict() +{ + F = jacobian_f(x_post), Q = update_Q(); + + x_pri = f(x_post); + P_pri = F * P_post * F.transpose() + Q; + + // handle the case when there will be no measurement before the next predict + x_post = x_pri; + P_post = P_pri; + + return x_pri; +} + +Eigen::MatrixXd ExtendedKalmanFilter::update(const Eigen::VectorXd & z) +{ + H = jacobian_h(x_pri), R = update_R(z); + + K = P_pri * H.transpose() * (H * P_pri * H.transpose() + R).inverse(); + x_post = x_pri + K * (z - h(x_pri)); + P_post = (I - K * H) * P_pri; + + return x_post; +} + +} // namespace rm_auto_aim diff --git a/armor_tracker/src/tracker.cpp b/armor_tracker/src/tracker.cpp new file mode 100644 index 0000000..2304640 --- /dev/null +++ b/armor_tracker/src/tracker.cpp @@ -0,0 +1,239 @@ +// Copyright 2022 Chen Jun + +#include "armor_tracker/tracker.hpp" + +#include +#include +#include +#include + +#include +#include + +// STD +#include +#include +#include + +namespace rm_auto_aim +{ +Tracker::Tracker(double max_match_distance, double max_match_yaw_diff) +: tracker_state(LOST), + tracked_id(std::string("")), + measurement(Eigen::VectorXd::Zero(4)), + target_state(Eigen::VectorXd::Zero(9)), + max_match_distance_(max_match_distance), + max_match_yaw_diff_(max_match_yaw_diff) +{ +} + +void Tracker::init(const Armors::SharedPtr & armors_msg) +{ + if (armors_msg->armors.empty()) { + return; + } + + // Simply choose the armor that is closest to image center + double min_distance = DBL_MAX; + tracked_armor = armors_msg->armors[0]; + for (const auto & armor : armors_msg->armors) { + if (armor.distance_to_image_center < min_distance) { + min_distance = armor.distance_to_image_center; + tracked_armor = armor; + } + } + + initEKF(tracked_armor); + RCLCPP_DEBUG(rclcpp::get_logger("armor_tracker"), "Init EKF!"); + + tracked_id = tracked_armor.number; + tracker_state = DETECTING; + + updateArmorsNum(tracked_armor); +} + +void Tracker::update(const Armors::SharedPtr & armors_msg) +{ + // KF predict + Eigen::VectorXd ekf_prediction = ekf.predict(); + RCLCPP_DEBUG(rclcpp::get_logger("armor_tracker"), "EKF predict"); + + bool matched = false; + // Use KF prediction as default target state if no matched armor is found + target_state = ekf_prediction; + + if (!armors_msg->armors.empty()) { + // Find the closest armor with the same id + Armor same_id_armor; + int same_id_armors_count = 0; + auto predicted_position = getArmorPositionFromState(ekf_prediction); + double min_position_diff = DBL_MAX; + double yaw_diff = DBL_MAX; + for (const auto & armor : armors_msg->armors) { + // Only consider armors with the same id + if (armor.number == tracked_id) { + same_id_armor = armor; + same_id_armors_count++; + // Calculate the difference between the predicted position and the current armor position + auto p = armor.pose.position; + Eigen::Vector3d position_vec(p.x, p.y, p.z); + double position_diff = (predicted_position - position_vec).norm(); + if (position_diff < min_position_diff) { + // Find the closest armor + min_position_diff = position_diff; + yaw_diff = abs(orientationToYaw(armor.pose.orientation) - ekf_prediction(6)); + tracked_armor = armor; + } + } + } + + // Store tracker info + info_position_diff = min_position_diff; + info_yaw_diff = yaw_diff; + + // Check if the distance and yaw difference of closest armor are within the threshold + if (min_position_diff < max_match_distance_ && yaw_diff < max_match_yaw_diff_) { + // Matched armor found + matched = true; + auto p = tracked_armor.pose.position; + // Update EKF + double measured_yaw = orientationToYaw(tracked_armor.pose.orientation); + measurement = Eigen::Vector4d(p.x, p.y, p.z, measured_yaw); + target_state = ekf.update(measurement); + RCLCPP_DEBUG(rclcpp::get_logger("armor_tracker"), "EKF update"); + } else if (same_id_armors_count == 1 && yaw_diff > max_match_yaw_diff_) { + // Matched armor not found, but there is only one armor with the same id + // and yaw has jumped, take this case as the target is spinning and armor jumped + handleArmorJump(same_id_armor); + } else { + // No matched armor found + RCLCPP_WARN(rclcpp::get_logger("armor_tracker"), "No matched armor found!"); + } + } + + // Prevent radius from spreading + if (target_state(8) < 0.12) { + target_state(8) = 0.12; + ekf.setState(target_state); + } else if (target_state(8) > 0.4) { + target_state(8) = 0.4; + ekf.setState(target_state); + } + + // Tracking state machine + if (tracker_state == DETECTING) { + if (matched) { + detect_count_++; + if (detect_count_ > tracking_thres) { + detect_count_ = 0; + tracker_state = TRACKING; + } + } else { + detect_count_ = 0; + tracker_state = LOST; + } + } else if (tracker_state == TRACKING) { + if (!matched) { + tracker_state = TEMP_LOST; + lost_count_++; + } + } else if (tracker_state == TEMP_LOST) { + if (!matched) { + lost_count_++; + if (lost_count_ > lost_thres) { + lost_count_ = 0; + tracker_state = LOST; + } + } else { + tracker_state = TRACKING; + lost_count_ = 0; + } + } +} + +void Tracker::initEKF(const Armor & a) +{ + double xa = a.pose.position.x; + double ya = a.pose.position.y; + double za = a.pose.position.z; + last_yaw_ = 0; + double yaw = orientationToYaw(a.pose.orientation); + + // Set initial position at 0.2m behind the target + target_state = Eigen::VectorXd::Zero(9); + double r = 0.26; + double xc = xa + r * cos(yaw); + double yc = ya + r * sin(yaw); + dz = 0, another_r = r; + target_state << xc, 0, yc, 0, za, 0, yaw, 0, r; + + ekf.setState(target_state); +} + +void Tracker::updateArmorsNum(const Armor & armor) +{ + if (armor.type == "large" && (tracked_id == "3" || tracked_id == "4" || tracked_id == "5")) { + tracked_armors_num = ArmorsNum::BALANCE_2; + } else if (tracked_id == "outpost") { + tracked_armors_num = ArmorsNum::OUTPOST_3; + } else { + tracked_armors_num = ArmorsNum::NORMAL_4; + } +} + +void Tracker::handleArmorJump(const Armor & current_armor) +{ + double yaw = orientationToYaw(current_armor.pose.orientation); + target_state(6) = yaw; + updateArmorsNum(current_armor); + // Only 4 armors has 2 radius and height + if (tracked_armors_num == ArmorsNum::NORMAL_4) { + dz = target_state(4) - current_armor.pose.position.z; + target_state(4) = current_armor.pose.position.z; + std::swap(target_state(8), another_r); + } + RCLCPP_WARN(rclcpp::get_logger("armor_tracker"), "Armor jump!"); + + // If position difference is larger than max_match_distance_, + // take this case as the ekf diverged, reset the state + auto p = current_armor.pose.position; + Eigen::Vector3d current_p(p.x, p.y, p.z); + Eigen::Vector3d infer_p = getArmorPositionFromState(target_state); + if ((current_p - infer_p).norm() > max_match_distance_) { + double r = target_state(8); + target_state(0) = p.x + r * cos(yaw); // xc + target_state(1) = 0; // vxc + target_state(2) = p.y + r * sin(yaw); // yc + target_state(3) = 0; // vyc + target_state(4) = p.z; // za + target_state(5) = 0; // vza + RCLCPP_ERROR(rclcpp::get_logger("armor_tracker"), "Reset State!"); + } + + ekf.setState(target_state); +} + +double Tracker::orientationToYaw(const geometry_msgs::msg::Quaternion & q) +{ + // Get armor yaw + tf2::Quaternion tf_q; + tf2::fromMsg(q, tf_q); + double roll, pitch, yaw; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + // Make yaw change continuous (-pi~pi to -inf~inf) + yaw = last_yaw_ + angles::shortest_angular_distance(last_yaw_, yaw); + last_yaw_ = yaw; + return yaw; +} + +Eigen::Vector3d Tracker::getArmorPositionFromState(const Eigen::VectorXd & x) +{ + // Calculate predicted position of the current armor + double xc = x(0), yc = x(2), za = x(4); + double yaw = x(6), r = x(8); + double xa = xc - r * cos(yaw); + double ya = yc - r * sin(yaw); + return Eigen::Vector3d(xa, ya, za); +} + +} // namespace rm_auto_aim diff --git a/armor_tracker/src/tracker_node.cpp b/armor_tracker/src/tracker_node.cpp new file mode 100644 index 0000000..492bda8 --- /dev/null +++ b/armor_tracker/src/tracker_node.cpp @@ -0,0 +1,350 @@ +// Copyright 2022 Chen Jun +#include "armor_tracker/tracker_node.hpp" + +// STD +#include +#include + +namespace rm_auto_aim +{ +ArmorTrackerNode::ArmorTrackerNode(const rclcpp::NodeOptions & options) +: Node("armor_tracker", options) +{ + RCLCPP_INFO(this->get_logger(), "Starting TrackerNode!"); + + // Maximum allowable armor distance in the XOY plane + max_armor_distance_ = this->declare_parameter("max_armor_distance", 10.0); + + // Tracker + double max_match_distance = this->declare_parameter("tracker.max_match_distance", 0.15); + double max_match_yaw_diff = this->declare_parameter("tracker.max_match_yaw_diff", 1.0); + tracker_ = std::make_unique(max_match_distance, max_match_yaw_diff); + tracker_->tracking_thres = this->declare_parameter("tracker.tracking_thres", 5); + lost_time_thres_ = this->declare_parameter("tracker.lost_time_thres", 0.3); + + // EKF + // xa = x_armor, xc = x_robot_center + // state: xc, v_xc, yc, v_yc, za, v_za, yaw, v_yaw, r + // measurement: xa, ya, za, yaw + // f - Process function + auto f = [this](const Eigen::VectorXd & x) { + Eigen::VectorXd x_new = x; + x_new(0) += x(1) * dt_; + x_new(2) += x(3) * dt_; + x_new(4) += x(5) * dt_; + x_new(6) += x(7) * dt_; + return x_new; + }; + // J_f - Jacobian of process function + auto j_f = [this](const Eigen::VectorXd &) { + Eigen::MatrixXd f(9, 9); + // clang-format off + f << 1, dt_, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, dt_, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, dt_, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, dt_, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1; + // clang-format on + return f; + }; + // h - Observation function + auto h = [](const Eigen::VectorXd & x) { + Eigen::VectorXd z(4); + double xc = x(0), yc = x(2), yaw = x(6), r = x(8); + z(0) = xc - r * cos(yaw); // xa + z(1) = yc - r * sin(yaw); // ya + z(2) = x(4); // za + z(3) = x(6); // yaw + return z; + }; + // J_h - Jacobian of observation function + auto j_h = [](const Eigen::VectorXd & x) { + Eigen::MatrixXd h(4, 9); + double yaw = x(6), r = x(8); + // clang-format off + // xc v_xc yc v_yc za v_za yaw v_yaw r + h << 1, 0, 0, 0, 0, 0, r*sin(yaw), 0, -cos(yaw), + 0, 0, 1, 0, 0, 0, -r*cos(yaw),0, -sin(yaw), + 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, 0, 0; + // clang-format on + return h; + }; + // update_Q - process noise covariance matrix + s2qxyz_ = declare_parameter("ekf.sigma2_q_xyz", 20.0); + s2qyaw_ = declare_parameter("ekf.sigma2_q_yaw", 100.0); + s2qr_ = declare_parameter("ekf.sigma2_q_r", 800.0); + auto u_q = [this]() { + Eigen::MatrixXd q(9, 9); + double t = dt_, x = s2qxyz_, y = s2qyaw_, r = s2qr_; + double q_x_x = pow(t, 4) / 4 * x, q_x_vx = pow(t, 3) / 2 * x, q_vx_vx = pow(t, 2) * x; + double q_y_y = pow(t, 4) / 4 * y, q_y_vy = pow(t, 3) / 2 * x, q_vy_vy = pow(t, 2) * y; + double q_r = pow(t, 4) / 4 * r; + // clang-format off + // xc v_xc yc v_yc za v_za yaw v_yaw r + q << q_x_x, q_x_vx, 0, 0, 0, 0, 0, 0, 0, + q_x_vx, q_vx_vx,0, 0, 0, 0, 0, 0, 0, + 0, 0, q_x_x, q_x_vx, 0, 0, 0, 0, 0, + 0, 0, q_x_vx, q_vx_vx,0, 0, 0, 0, 0, + 0, 0, 0, 0, q_x_x, q_x_vx, 0, 0, 0, + 0, 0, 0, 0, q_x_vx, q_vx_vx,0, 0, 0, + 0, 0, 0, 0, 0, 0, q_y_y, q_y_vy, 0, + 0, 0, 0, 0, 0, 0, q_y_vy, q_vy_vy,0, + 0, 0, 0, 0, 0, 0, 0, 0, q_r; + // clang-format on + return q; + }; + // update_R - measurement noise covariance matrix + r_xyz_factor = declare_parameter("ekf.r_xyz_factor", 0.05); + r_yaw = declare_parameter("ekf.r_yaw", 0.02); + auto u_r = [this](const Eigen::VectorXd & z) { + Eigen::DiagonalMatrix r; + double x = r_xyz_factor; + r.diagonal() << abs(x * z[0]), abs(x * z[1]), abs(x * z[2]), r_yaw; + return r; + }; + // P - error estimate covariance matrix + Eigen::DiagonalMatrix p0; + p0.setIdentity(); + tracker_->ekf = ExtendedKalmanFilter{f, h, j_f, j_h, u_q, u_r, p0}; + + // Reset tracker service + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + reset_tracker_srv_ = this->create_service( + "/tracker/reset", [this]( + const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr response) { + tracker_->tracker_state = Tracker::LOST; + response->success = true; + RCLCPP_INFO(this->get_logger(), "Tracker reset!"); + return; + }); + + // Subscriber with tf2 message_filter + // tf2 relevant + tf2_buffer_ = std::make_shared(this->get_clock()); + // Create the timer interface before call to waitForTransform, + // to avoid a tf2_ros::CreateTimerInterfaceException exception + auto timer_interface = std::make_shared( + this->get_node_base_interface(), this->get_node_timers_interface()); + tf2_buffer_->setCreateTimerInterface(timer_interface); + tf2_listener_ = std::make_shared(*tf2_buffer_); + // subscriber and filter + armors_sub_.subscribe(this, "/detector/armors", rmw_qos_profile_sensor_data); + target_frame_ = this->declare_parameter("target_frame", "odom"); + tf2_filter_ = std::make_shared( + armors_sub_, *tf2_buffer_, target_frame_, 10, this->get_node_logging_interface(), + this->get_node_clock_interface(), std::chrono::duration(1)); + // Register a callback with tf2_ros::MessageFilter to be called when transforms are available + tf2_filter_->registerCallback(&ArmorTrackerNode::armorsCallback, this); + + // Measurement publisher (for debug usage) + info_pub_ = this->create_publisher("/tracker/info", 10); + + // Publisher + target_pub_ = this->create_publisher( + "/tracker/target", rclcpp::SensorDataQoS()); + + // Visualization Marker Publisher + // See http://wiki.ros.org/rviz/DisplayTypes/Marker + position_marker_.ns = "position"; + position_marker_.type = visualization_msgs::msg::Marker::SPHERE; + position_marker_.scale.x = position_marker_.scale.y = position_marker_.scale.z = 0.1; + position_marker_.color.a = 1.0; + position_marker_.color.g = 1.0; + linear_v_marker_.type = visualization_msgs::msg::Marker::ARROW; + linear_v_marker_.ns = "linear_v"; + linear_v_marker_.scale.x = 0.03; + linear_v_marker_.scale.y = 0.05; + linear_v_marker_.color.a = 1.0; + linear_v_marker_.color.r = 1.0; + linear_v_marker_.color.g = 1.0; + angular_v_marker_.type = visualization_msgs::msg::Marker::ARROW; + angular_v_marker_.ns = "angular_v"; + angular_v_marker_.scale.x = 0.03; + angular_v_marker_.scale.y = 0.05; + angular_v_marker_.color.a = 1.0; + angular_v_marker_.color.b = 1.0; + angular_v_marker_.color.g = 1.0; + armor_marker_.ns = "armors"; + armor_marker_.type = visualization_msgs::msg::Marker::CUBE; + armor_marker_.scale.x = 0.03; + armor_marker_.scale.z = 0.125; + armor_marker_.color.a = 1.0; + armor_marker_.color.r = 1.0; + marker_pub_ = this->create_publisher("/tracker/marker", 10); +} + +void ArmorTrackerNode::armorsCallback(const auto_aim_interfaces::msg::Armors::SharedPtr armors_msg) +{ + // Tranform armor position from image frame to world coordinate + for (auto & armor : armors_msg->armors) { + geometry_msgs::msg::PoseStamped ps; + ps.header = armors_msg->header; + ps.pose = armor.pose; + try { + armor.pose = tf2_buffer_->transform(ps, target_frame_).pose; + } catch (const tf2::ExtrapolationException & ex) { + RCLCPP_ERROR(get_logger(), "Error while transforming %s", ex.what()); + return; + } + } + + // Filter abnormal armors + armors_msg->armors.erase( + std::remove_if( + armors_msg->armors.begin(), armors_msg->armors.end(), + [this](const auto_aim_interfaces::msg::Armor & armor) { + return abs(armor.pose.position.z) > 1.2 || + Eigen::Vector2d(armor.pose.position.x, armor.pose.position.y).norm() > + max_armor_distance_; + }), + armors_msg->armors.end()); + + // Init message + auto_aim_interfaces::msg::TrackerInfo info_msg; + auto_aim_interfaces::msg::Target target_msg; + rclcpp::Time time = armors_msg->header.stamp; + target_msg.header.stamp = time; + target_msg.header.frame_id = target_frame_; + + // Update tracker + if (tracker_->tracker_state == Tracker::LOST) { + tracker_->init(armors_msg); + target_msg.tracking = false; + } else { + dt_ = (time - last_time_).seconds(); + tracker_->lost_thres = static_cast(lost_time_thres_ / dt_); + tracker_->update(armors_msg); + + // Publish Info + info_msg.position_diff = tracker_->info_position_diff; + info_msg.yaw_diff = tracker_->info_yaw_diff; + info_msg.position.x = tracker_->measurement(0); + info_msg.position.y = tracker_->measurement(1); + info_msg.position.z = tracker_->measurement(2); + info_msg.yaw = tracker_->measurement(3); + info_pub_->publish(info_msg); + + if (tracker_->tracker_state == Tracker::DETECTING) { + target_msg.tracking = false; + } else if ( + tracker_->tracker_state == Tracker::TRACKING || + tracker_->tracker_state == Tracker::TEMP_LOST) { + target_msg.tracking = true; + // Fill target message + const auto & state = tracker_->target_state; + target_msg.id = tracker_->tracked_id; + target_msg.armors_num = static_cast(tracker_->tracked_armors_num); + target_msg.position.x = state(0); + target_msg.velocity.x = state(1); + target_msg.position.y = state(2); + target_msg.velocity.y = state(3); + target_msg.position.z = state(4); + target_msg.velocity.z = state(5); + target_msg.yaw = state(6); + target_msg.v_yaw = state(7); + target_msg.radius_1 = state(8); + target_msg.radius_2 = tracker_->another_r; + target_msg.dz = tracker_->dz; + } + } + + last_time_ = time; + + target_pub_->publish(target_msg); + + publishMarkers(target_msg); +} + +void ArmorTrackerNode::publishMarkers(const auto_aim_interfaces::msg::Target & target_msg) +{ + position_marker_.header = target_msg.header; + linear_v_marker_.header = target_msg.header; + angular_v_marker_.header = target_msg.header; + armor_marker_.header = target_msg.header; + + visualization_msgs::msg::MarkerArray marker_array; + if (target_msg.tracking) { + double yaw = target_msg.yaw, r1 = target_msg.radius_1, r2 = target_msg.radius_2; + double xc = target_msg.position.x, yc = target_msg.position.y, za = target_msg.position.z; + double vx = target_msg.velocity.x, vy = target_msg.velocity.y, vz = target_msg.velocity.z; + double dz = target_msg.dz; + + position_marker_.action = visualization_msgs::msg::Marker::ADD; + position_marker_.pose.position.x = xc; + position_marker_.pose.position.y = yc; + position_marker_.pose.position.z = za + dz / 2; + + linear_v_marker_.action = visualization_msgs::msg::Marker::ADD; + linear_v_marker_.points.clear(); + linear_v_marker_.points.emplace_back(position_marker_.pose.position); + geometry_msgs::msg::Point arrow_end = position_marker_.pose.position; + arrow_end.x += vx; + arrow_end.y += vy; + arrow_end.z += vz; + linear_v_marker_.points.emplace_back(arrow_end); + + angular_v_marker_.action = visualization_msgs::msg::Marker::ADD; + angular_v_marker_.points.clear(); + angular_v_marker_.points.emplace_back(position_marker_.pose.position); + arrow_end = position_marker_.pose.position; + arrow_end.z += target_msg.v_yaw / M_PI; + angular_v_marker_.points.emplace_back(arrow_end); + + armor_marker_.action = visualization_msgs::msg::Marker::ADD; + armor_marker_.scale.y = tracker_->tracked_armor.type == "small" ? 0.135 : 0.23; + bool is_current_pair = true; + size_t a_n = target_msg.armors_num; + geometry_msgs::msg::Point p_a; + double r = 0; + for (size_t i = 0; i < a_n; i++) { + double tmp_yaw = yaw + i * (2 * M_PI / a_n); + // Only 4 armors has 2 radius and height + if (a_n == 4) { + r = is_current_pair ? r1 : r2; + p_a.z = za + (is_current_pair ? 0 : dz); + is_current_pair = !is_current_pair; + } else { + r = r1; + p_a.z = za; + } + p_a.x = xc - r * cos(tmp_yaw); + p_a.y = yc - r * sin(tmp_yaw); + + armor_marker_.id = i; + armor_marker_.pose.position = p_a; + tf2::Quaternion q; + q.setRPY(0, target_msg.id == "outpost" ? -0.26 : 0.26, tmp_yaw); + armor_marker_.pose.orientation = tf2::toMsg(q); + marker_array.markers.emplace_back(armor_marker_); + } + } else { + position_marker_.action = visualization_msgs::msg::Marker::DELETE; + linear_v_marker_.action = visualization_msgs::msg::Marker::DELETE; + angular_v_marker_.action = visualization_msgs::msg::Marker::DELETE; + + armor_marker_.action = visualization_msgs::msg::Marker::DELETE; + marker_array.markers.emplace_back(armor_marker_); + } + + marker_array.markers.emplace_back(position_marker_); + marker_array.markers.emplace_back(linear_v_marker_); + marker_array.markers.emplace_back(angular_v_marker_); + marker_pub_->publish(marker_array); +} + +} // namespace rm_auto_aim + +#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(rm_auto_aim::ArmorTrackerNode) diff --git a/auto_aim_interfaces/CMakeLists.txt b/auto_aim_interfaces/CMakeLists.txt new file mode 100644 index 0000000..0545379 --- /dev/null +++ b/auto_aim_interfaces/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(auto_aim_interfaces) + +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(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Armor.msg" + "msg/Armors.msg" + "msg/Target.msg" + + "msg/DebugLight.msg" + "msg/DebugLights.msg" + "msg/DebugArmor.msg" + "msg/DebugArmors.msg" + "msg/TrackerInfo.msg" + + DEPENDENCIES + std_msgs + geometry_msgs +) + + +ament_package() diff --git a/auto_aim_interfaces/msg/Armor.msg b/auto_aim_interfaces/msg/Armor.msg new file mode 100644 index 0000000..723f523 --- /dev/null +++ b/auto_aim_interfaces/msg/Armor.msg @@ -0,0 +1,4 @@ +string number +string type +float32 distance_to_image_center +geometry_msgs/Pose pose \ No newline at end of file diff --git a/auto_aim_interfaces/msg/Armors.msg b/auto_aim_interfaces/msg/Armors.msg new file mode 100644 index 0000000..0a70d30 --- /dev/null +++ b/auto_aim_interfaces/msg/Armors.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +Armor[] armors \ No newline at end of file diff --git a/auto_aim_interfaces/msg/DebugArmor.msg b/auto_aim_interfaces/msg/DebugArmor.msg new file mode 100644 index 0000000..cf96eec --- /dev/null +++ b/auto_aim_interfaces/msg/DebugArmor.msg @@ -0,0 +1,5 @@ +int32 center_x +string type +float32 light_ratio +float32 center_distance +float32 angle diff --git a/auto_aim_interfaces/msg/DebugArmors.msg b/auto_aim_interfaces/msg/DebugArmors.msg new file mode 100644 index 0000000..e12e300 --- /dev/null +++ b/auto_aim_interfaces/msg/DebugArmors.msg @@ -0,0 +1 @@ +DebugArmor[] data \ No newline at end of file diff --git a/auto_aim_interfaces/msg/DebugLight.msg b/auto_aim_interfaces/msg/DebugLight.msg new file mode 100644 index 0000000..c876dbc --- /dev/null +++ b/auto_aim_interfaces/msg/DebugLight.msg @@ -0,0 +1,4 @@ +int32 center_x +bool is_light +float32 ratio +float32 angle \ No newline at end of file diff --git a/auto_aim_interfaces/msg/DebugLights.msg b/auto_aim_interfaces/msg/DebugLights.msg new file mode 100644 index 0000000..14e31ce --- /dev/null +++ b/auto_aim_interfaces/msg/DebugLights.msg @@ -0,0 +1 @@ +DebugLight[] data \ No newline at end of file diff --git a/auto_aim_interfaces/msg/Target.msg b/auto_aim_interfaces/msg/Target.msg new file mode 100644 index 0000000..fc9926b --- /dev/null +++ b/auto_aim_interfaces/msg/Target.msg @@ -0,0 +1,11 @@ +std_msgs/Header header +bool tracking +string id +int32 armors_num +geometry_msgs/Point position +geometry_msgs/Vector3 velocity +float64 yaw +float64 v_yaw +float64 radius_1 +float64 radius_2 +float64 dz diff --git a/auto_aim_interfaces/msg/TrackerInfo.msg b/auto_aim_interfaces/msg/TrackerInfo.msg new file mode 100644 index 0000000..5d6bc35 --- /dev/null +++ b/auto_aim_interfaces/msg/TrackerInfo.msg @@ -0,0 +1,6 @@ +# Difference between the current measurement and prediction +float64 position_diff +float64 yaw_diff +# Unfiltered position and yaw +geometry_msgs/Point position +float64 yaw diff --git a/auto_aim_interfaces/package.xml b/auto_aim_interfaces/package.xml new file mode 100644 index 0000000..11a6ed3 --- /dev/null +++ b/auto_aim_interfaces/package.xml @@ -0,0 +1,21 @@ + + + + auto_aim_interfaces + 0.0.0 + TODO: Package description + chenjun + TODO: License declaration + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + geometry_msgs + + + ament_cmake + + diff --git a/docs/rm_vision.svg b/docs/rm_vision.svg new file mode 100644 index 0000000..a9d40c6 --- /dev/null +++ b/docs/rm_vision.svg @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/rm_auto_aim/CMakeLists.txt b/rm_auto_aim/CMakeLists.txt new file mode 100644 index 0000000..1ec501e --- /dev/null +++ b/rm_auto_aim/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.8) +project(rm_auto_aim) + +find_package(ament_cmake_auto REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/rm_auto_aim/package.xml b/rm_auto_aim/package.xml new file mode 100644 index 0000000..0a6b3d0 --- /dev/null +++ b/rm_auto_aim/package.xml @@ -0,0 +1,22 @@ + + + + rm_auto_aim + 0.0.0 + TODO: Package description + chenjun + TODO: License declaration + + ament_cmake + + auto_aim_interfaces + armor_detector + armor_tracker + + ament_lint_auto + ament_lint_common + + + ament_cmake + +