diff --git a/CMakeLists.txt b/CMakeLists.txt index fecb845..cafb540 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) @@ -71,6 +72,7 @@ target_include_directories( PRIVATE ${visualization_msgs_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} ${tf2_ros_INCLUDE_DIRS} ) target_link_libraries( @@ -84,6 +86,7 @@ target_link_libraries( PRIVATE ${visualization_msgs_LIBRARIES} ${geometry_msgs_LIBRARIES} + ${std_msgs_LIBRARIES} ${tf2_ros_LIBRARIES} ) diff --git a/config/config.yaml b/config/config.yaml index 9c01e30..5c437f3 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -20,8 +20,7 @@ capturer: reconnect_wait_interval: 100 record_enable: true - saving_pathes: - ["/home/root/autoaim/", "/home/ubuntu/autoaim/", "/tmp/autoaim/"] + saving_pathes: ["/root/autoaim/", "/home/ubuntu/autoaim/", "/tmp/autoaim/"] max_duration_seconds: 60 record_fps: 24 max_videos_size_gb: 50 diff --git a/package.xml b/package.xml index c986fdd..ef36406 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,7 @@ rmcs_executor rmcs_description fast_tf + std_msgs rmcs_msgs hikcamera diff --git a/src/kernel/auto_aim.cpp b/src/kernel/auto_aim.cpp index 7e8f906..fee5d75 100644 --- a/src/kernel/auto_aim.cpp +++ b/src/kernel/auto_aim.cpp @@ -50,6 +50,8 @@ struct AutoAim::Impl { auto image = cap.fetch_image(); if (!image) continue; + // @TODO: + // 避免拷贝，而是把需要的具体量拿出来 auto context = SystemContext::kIdentity(); { std::lock_guard lock { self.context_mutex }; @@ -63,9 +65,9 @@ struct AutoAim::Impl { [[maybe_unused]] auto streamer = std::experimental::scope_exit { [&] { visual.draw_later( // 录制开关 - Text { cap.recording() ? "RECORD ON" : "RECORD OFF", { 10, 700 } }); + Canvas::Text { cap.recording() ? "RECORD ON" : "RECORD OFF", { 10, 700 } }); visual.draw_later( // 自瞄帧率 - Text { std::format("FPS: {}", framerate.fps()), { 10, 680 } }); + Canvas::Text { std::format("FPS: {}", framerate.fps()), { 10, 680 } }); visual.update_image(*image); } }; @@ -148,8 +150,10 @@ struct AutoAim::Impl { cmd.yaw_acc, cmd.pitch_acc); } } - visual.draw_later(Text { "ATTACK", { 10, 660 }, cmd.should_shoot ? kRed : kWhite }); - visual.draw_later(Text { "CONTROL", { 10, 640 }, cmd.should_control ? kRed : kWhite }); + visual.draw_later( + Canvas::Text { "ATTACK", { 10, 660 }, cmd.should_shoot ? kRed : kWhite }); + visual.draw_later( + Canvas::Text { "CONTROL", { 10, 640 }, cmd.should_control ? kRed : kWhite }); { std::lock_guard lock { self.command_mutex }; diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 3c48f41..9c96893 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -7,6 +7,7 @@ #include "utility/rclcpp/visual/armor.hpp" #include "utility/rclcpp/visual/arrow.hpp" #include "utility/rclcpp/visual/lightbar.hpp" +#include "utility/rclcpp/visual/scalar.hpp" #include "utility/rclcpp/visual/transform.hpp" #include "utility/serializable.hpp" @@ -61,6 +62,7 @@ struct Visualization::Impl { std::unique_ptr aiming_direction; std::unordered_map armor_publishers; std::unordered_map lightbar_publishers; + std::unordered_map scalar_publishers; bool is_initialized = false; bool size_determined = false; @@ -141,7 +143,6 @@ struct Visualization::Impl { auto publish(std::span armors, const std::string& name) { if (!is_initialized || !config.publishable) return; - auto iter = armor_publishers.find(name); if (iter == armor_publishers.end()) { auto config = visual::Armors::Config { @@ -155,9 +156,7 @@ struct Visualization::Impl { } auto publish(std::span lightbars, const std::string& name) -> void { - if (!is_initialized) return; - if (!config.publishable) return; - + if (!is_initialized || !config.publishable) return; auto iter = lightbar_publishers.find(name); if (iter == lightbar_publishers.end()) { auto config = visual::Lightbars::Config { @@ -175,6 +174,15 @@ struct Visualization::Impl { odom_transform.publish(t); } + auto publish(double value, const std::string& name) -> void { + if (!is_initialized || !config.publishable) return; + auto iter = scalar_publishers.find(name); + if (iter == scalar_publishers.end()) { + iter = scalar_publishers.emplace(name, visual::Scalar { rclcpp, name }).first; + } + iter->second.publish(value); + } + auto update_aiming_direction(double yaw, double pitch) const -> void { if (!is_initialized) return; if (!config.publishable) return; @@ -226,6 +234,10 @@ auto Visualization::publish(const Transform& t, const std::string& name) -> void pimpl->publish(t, name); } +auto Visualization::publish(double value, const std::string& name) -> void { + pimpl->publish(value, name); +} + Visualization::Visualization() noexcept : pimpl { std::make_unique() } { } diff --git a/src/kernel/visualization.hpp b/src/kernel/visualization.hpp index b4d38c6..463401c 100644 --- a/src/kernel/visualization.hpp +++ b/src/kernel/visualization.hpp @@ -38,6 +38,8 @@ class Visualization { auto publish(const Transform& t, const std::string& name) -> void; + auto publish(double value, const std::string& name) -> void; + auto update_aiming_direction(double yaw, double pitch) const -> void; auto update_mpc_plan(double yaw, double pitch, double yaw_rate, double pitch_rate, diff --git a/src/module/perception/rotation_estimator.cpp b/src/module/perception/rotation_estimator.cpp new file mode 100644 index 0000000..fff9b71 --- /dev/null +++ b/src/module/perception/rotation_estimator.cpp @@ -0,0 +1,419 @@ +/// @brief 基于成熟 VIO 视觉前端思想的轻量旋转速度估计器。 +/// +/// 算法来源： +/// - VINS-Mono FeatureTracker: +/// https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/feature_tracker/src/feature_tracker.cpp +/// - OpenVINS TrackKLT: +/// https://github.com/rpng/open_vins/blob/master/ov_core/src/track/TrackKLT.cpp +/// +/// 本实现没有复制上述 GPL 项目源码，只参考其通用流程：维护上一帧特征点，使用 KLT +/// 光流跟踪到当前帧，通过 mask 排除装甲板等动态区域，使用 RANSAC 剔除外点，再根据 +/// 归一化图像平面上的背景点速度输出 yaw/pitch/roll-like 视觉旋转速度波形。 + +#include "rotation_estimator.hpp" +#include "utility/image/image.details.hpp" + +#include +#include +#include + +#include +#include +#include + +using namespace rmcs; + +struct RotationEstimator::Impl { + static constexpr auto kMaxFeatures = 160; + static constexpr auto kMinTrackedPoints = 6; + static constexpr auto kMinInlierPoints = 4; + static constexpr auto kRefillThreshold = 80; + static constexpr auto kMinFeatureDist = 18.0; + static constexpr auto kMaskPaddingRatio = 0.65; + static constexpr auto kScale = 0.3; + static constexpr auto kMinDt = 1e-4; + static constexpr auto kMaxDt = 0.2; + static constexpr auto kBorderSize = 8; + static constexpr auto kMaxOpticalFlowErr = 80.0f; + static constexpr auto kGridStep = 48; + + struct Tracks { + std::vector prev; + std::vector current; + std::vector prev_inliers; + std::vector current_inliers; + double roll_rate_like { 0.0 }; + }; + + cv::Mat prev_gray; + cv::Mat armor_mask; + cv::Mat lightbar_mask; + TimePoint prev_timestamp { }; + std::vector prev_points; + + cv::Mat camera_matrix; + cv::Mat distort_coeff; + Addition addition { }; + + static auto in_border(const cv::Point2f& point, const cv::Size& size) -> bool { + const auto border = static_cast(kBorderSize); + const auto width = static_cast(size.width - kBorderSize); + const auto height = static_cast(size.height - kBorderSize); + + return point.x >= border && point.y >= border && point.x < width && point.y < height; + } + + static auto mask_allows(const cv::Mat& mask, const cv::Point2f& point) -> bool { + const auto x = static_cast(std::lround(point.x)); + const auto y = static_cast(std::lround(point.y)); + if (x < 0 || y < 0 || x >= mask.cols || y >= mask.rows) return false; + + return mask.at(y, x) != 0; + } + + static auto median(std::vector values) -> double { + if (values.empty()) return 0.0; + + const auto middle = values.begin() + static_cast(values.size() / 2); + std::nth_element(values.begin(), middle, values.end()); + + auto result = *middle; + if (values.size() % 2 == 0) { + const auto lower = std::max_element(values.begin(), middle); + result = (*lower + result) * 0.5; + } + return result; + } + + static auto detect_features(const cv::Mat& gray, const cv::Mat& allowed_mask, + const std::vector& existing) -> std::vector { + auto feature_mask = allowed_mask.clone(); + auto compacted = std::vector { }; + compacted.reserve(kMaxFeatures); + + for (const auto& point : existing) { + if (!in_border(point, gray.size()) || !mask_allows(allowed_mask, point)) continue; + + compacted.push_back(point); + cv::circle(feature_mask, point, static_cast(kMinFeatureDist), cv::Scalar { 0 }, + cv::FILLED); + + if (static_cast(compacted.size()) >= kMaxFeatures) return compacted; + } + + { + auto new_points = std::vector { }; + const auto needed = kMaxFeatures - static_cast(compacted.size()); + if (needed > 0) { + cv::goodFeaturesToTrack( + gray, new_points, needed, 0.01, kMinFeatureDist, feature_mask, 3, false, 0.04); + } + compacted.insert(compacted.end(), new_points.begin(), new_points.end()); + } + + if (static_cast(compacted.size()) >= kMinTrackedPoints) return compacted; + + for (auto y = kGridStep / 2; y < gray.rows; y += kGridStep) { + for (auto x = kGridStep / 2; x < gray.cols; x += kGridStep) { + auto point = cv::Point2f { static_cast(x), static_cast(y) }; + if (!mask_allows(allowed_mask, point)) continue; + + compacted.push_back(point); + if (static_cast(compacted.size()) >= kMaxFeatures) return compacted; + } + } + return compacted; + } + + static auto into_original_points(const std::vector& points) + -> std::vector { + auto result = std::vector { }; + result.reserve(points.size()); + + constexpr auto inv_scale = 1.0 / kScale; + for (const auto& point : points) { + result.emplace_back(cv::Point2d { point.x * inv_scale, point.y * inv_scale }); + } + return result; + } + + auto configure_camera_matrix(std::array input) -> void { + camera_matrix = cv::Mat(3, 3, CV_64F); + for (auto i = 0; i < 9; ++i) { + camera_matrix.at(i / 3, i % 3) = input[static_cast(i)]; + } + } + + auto configure_distort_coeff(std::array input) -> void { + distort_coeff = cv::Mat(1, 5, CV_64F); + for (auto i = 0; i < 5; ++i) { + distort_coeff.at(0, i) = input[static_cast(i)]; + } + } + + auto reset() -> void { + prev_gray.release(); + armor_mask.release(); + lightbar_mask.release(); + prev_points.clear(); + prev_timestamp = { }; + addition = { }; + } + + auto update_mask(std::span armors) -> void { + if (prev_gray.empty()) return; + + auto mask = cv::Mat { prev_gray.size(), CV_8UC1, cv::Scalar { 255 } }; + for (const auto& armor : armors) { + auto points = armor.points(); + for (auto& p : points) { + p.x *= static_cast(kScale); + p.y *= static_cast(kScale); + } + + auto rect = cv::boundingRect(points); + { + const auto pad = static_cast( + std::lround(std::max(rect.width, rect.height) * kMaskPaddingRatio)); + rect.x -= pad; + rect.y -= pad; + rect.width += pad * 2; + rect.height += pad * 2; + rect &= cv::Rect { 0, 0, mask.cols, mask.rows }; + } + if (rect.area() > 0) cv::rectangle(mask, rect, cv::Scalar { 0 }, cv::FILLED); + } + armor_mask = mask; + } + + auto update_mask(std::span lightbars) -> void { + if (prev_gray.empty()) return; + + auto mask = cv::Mat { prev_gray.size(), CV_8UC1, cv::Scalar { 255 } }; + for (const auto& bar : lightbars) { + const auto ux = static_cast(bar.upper.x * kScale); + const auto uy = static_cast(bar.upper.y * kScale); + const auto lx = static_cast(bar.lower.x * kScale); + const auto ly = static_cast(bar.lower.y * kScale); + + auto points = std::vector { + cv::Point2f { ux, uy }, + cv::Point2f { lx, ly }, + }; + + auto rect = cv::boundingRect(points); + { + const auto pad = static_cast( + std::lround(std::max(rect.width, rect.height) * kMaskPaddingRatio)); + rect.x -= pad; + rect.y -= pad; + rect.width += pad * 2; + rect.height += pad * 2; + rect &= cv::Rect { 0, 0, mask.cols, mask.rows }; + } + if (rect.area() > 0) cv::rectangle(mask, rect, cv::Scalar { 0 }, cv::FILLED); + } + lightbar_mask = mask; + } + + auto update(const Image& image) -> std::optional { + + const auto& input = image.details().mat; + if (input.empty()) return std::nullopt; + + auto raw_gray = cv::Mat { }; + if (input.channels() == 1) { + raw_gray = input.clone(); + } else { + cv::cvtColor(input, raw_gray, cv::COLOR_BGR2GRAY); + } + + auto gray = cv::Mat { }; + cv::resize(raw_gray, gray, cv::Size { }, kScale, kScale, cv::INTER_AREA); + + auto allowed_mask = cv::Mat { gray.size(), CV_8UC1, cv::Scalar { 255 } }; + if (!armor_mask.empty()) { + cv::bitwise_and(allowed_mask, armor_mask, allowed_mask); + } + if (!lightbar_mask.empty()) { + cv::bitwise_and(allowed_mask, lightbar_mask, allowed_mask); + } + + const auto timestamp = image.get_timestamp(); + + addition = { }; + addition.timestamp = timestamp; + addition.scale = kScale; + + if (prev_gray.empty()) { + prev_points = detect_features(gray, allowed_mask, { }); + prev_gray = gray; + prev_timestamp = timestamp; + addition.feature_points = static_cast(prev_points.size()); + return std::nullopt; + } + + const auto dt = std::chrono::duration(timestamp - prev_timestamp).count(); + addition.dt = dt; + if (!std::isfinite(dt) || dt < kMinDt || dt > kMaxDt || prev_points.empty()) { + prev_points = detect_features(gray, allowed_mask, { }); + prev_gray = gray; + prev_timestamp = timestamp; + addition.feature_points = static_cast(prev_points.size()); + return std::nullopt; + } + + const auto tracked = track(prev_gray, gray, allowed_mask); + const auto tracked_points = static_cast(tracked.prev.size()); + addition.tracked_points = tracked_points; + addition.inlier_points = static_cast(tracked.current_inliers.size()); + addition.tracked_features = into_original_points(tracked.current_inliers); + + const auto estimate = estimate_rotation(tracked, timestamp, dt, tracked_points); + if (estimate) { + addition.valid = true; + addition.confidence = estimate->confidence; + } + + const auto next_points = estimate ? tracked.current_inliers : std::vector { }; + if (static_cast(next_points.size()) >= kRefillThreshold) { + prev_points = next_points; + } else { + prev_points = detect_features(gray, allowed_mask, next_points); + } + prev_gray = gray; + prev_timestamp = timestamp; + addition.feature_points = static_cast(prev_points.size()); + + return estimate; + } + + auto track(const cv::Mat& from, const cv::Mat& to, const cv::Mat& mask) const -> Tracks { + auto result = Tracks { }; + if (prev_points.empty()) return result; + + auto next_points = std::vector { }; + auto status = std::vector { }; + auto error = std::vector { }; + cv::calcOpticalFlowPyrLK(from, to, prev_points, next_points, status, error, + cv::Size { 21, 21 }, 3, + cv::TermCriteria { cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01 }); + + for (auto i = std::size_t { 0 }; i < next_points.size(); ++i) { + if (!status[i]) continue; + if (i < error.size() && error[i] > kMaxOpticalFlowErr) continue; + if (!in_border(prev_points[i], from.size()) || !in_border(next_points[i], to.size())) + continue; + if (!mask_allows(mask, prev_points[i]) || !mask_allows(mask, next_points[i])) continue; + + result.prev.push_back(prev_points[i]); + result.current.push_back(next_points[i]); + } + + if (result.prev.size() >= kMinTrackedPoints) { + auto inliers = std::vector { }; + auto affine = cv::estimateAffinePartial2D( + result.prev, result.current, inliers, cv::RANSAC, 3.0, 2'000, 0.99, 10); + + if (!affine.empty()) { + const auto angle = std::atan2(affine.at(1, 0), affine.at(0, 0)); + result.roll_rate_like = angle; + } + + for (auto i = std::size_t { 0 }; i < inliers.size(); ++i) { + if (!inliers[i]) continue; + + result.prev_inliers.push_back(result.prev[i]); + result.current_inliers.push_back(result.current[i]); + } + + if (result.prev_inliers.size() < kMinInlierPoints) { + result.prev_inliers = result.prev; + result.current_inliers = result.current; + result.roll_rate_like = 0.0; + } + } + + return result; + } + + auto estimate_rotation(const Tracks& tracks, TimePoint timestamp, double dt, + int tracked_points) const -> std::optional { + if (tracked_points < kMinTrackedPoints + || static_cast(tracks.current_inliers.size()) < kMinInlierPoints) { + return std::nullopt; + } + + if (camera_matrix.empty() || camera_matrix.rows != 3 || camera_matrix.cols != 3 + || camera_matrix.type() != CV_64F) { + return std::nullopt; + } + + const auto fx = camera_matrix.at(0, 0) * kScale; + const auto fy = camera_matrix.at(1, 1) * kScale; + const auto cx = camera_matrix.at(0, 2) * kScale; + const auto cy = camera_matrix.at(1, 2) * kScale; + if (!std::isfinite(fx) || !std::isfinite(fy) || std::abs(fx) < 1e-6 + || std::abs(fy) < 1e-6) { + return std::nullopt; + } + + auto xs = std::vector { }; + auto ys = std::vector { }; + xs.reserve(tracks.current_inliers.size()); + ys.reserve(tracks.current_inliers.size()); + + for (auto i = std::size_t { 0 }; i < tracks.current_inliers.size(); ++i) { + const auto prev_x = (tracks.prev_inliers[i].x - cx) / fx; + const auto prev_y = (tracks.prev_inliers[i].y - cy) / fy; + const auto current_x = (tracks.current_inliers[i].x - cx) / fx; + const auto current_y = (tracks.current_inliers[i].y - cy) / fy; + + xs.push_back((current_x - prev_x) / dt); + ys.push_back((current_y - prev_y) / dt); + } + + const auto inlier_points = static_cast(tracks.current_inliers.size()); + const auto inlier_ratio = + tracked_points > 0 ? static_cast(inlier_points) / tracked_points : 0.0; + const auto count_score = std::clamp(static_cast(inlier_points) / 80.0, 0.0, 1.0); + + return Estimate { + .timestamp = timestamp, + .dt = dt, + .yaw_rate = -median(std::move(xs)), + .pitch_rate = +median(std::move(ys)), + .roll_rate = tracks.roll_rate_like / dt, + .confidence = std::clamp(count_score * inlier_ratio, 0.0, 1.0), + }; + } +}; + +auto RotationEstimator::configure(std::array camera_matrix) -> void { + pimpl->configure_camera_matrix(camera_matrix); +} + +auto RotationEstimator::configure(std::array distort_coeff) -> void { + pimpl->configure_distort_coeff(distort_coeff); +} + +auto RotationEstimator::update(const Image& image) -> std::optional { + return pimpl->update(image); +} + +auto RotationEstimator::update_mask(std::span armors) -> void { + pimpl->update_mask(armors); +} + +auto RotationEstimator::update_mask(std::span lightbars) -> void { + pimpl->update_mask(lightbars); +} + +auto RotationEstimator::reset() -> void { pimpl->reset(); } + +auto RotationEstimator::addition() const -> const Addition& { return pimpl->addition; } + +RotationEstimator::RotationEstimator() noexcept + : pimpl { std::make_unique() } { } + +RotationEstimator::~RotationEstimator() noexcept = default; diff --git a/src/module/perception/rotation_estimator.hpp b/src/module/perception/rotation_estimator.hpp new file mode 100644 index 0000000..994f6ae --- /dev/null +++ b/src/module/perception/rotation_estimator.hpp @@ -0,0 +1,72 @@ +#pragma once +#include "utility/clock.hpp" +#include "utility/image/image.hpp" +#include "utility/pimpl.hpp" +#include "utility/robot/armor.hpp" + +#include +#include +#include +#include + +namespace rmcs { + +// 这是一段示例代码，用于发布估计的角加速度 +// +// if (auto result = rotation_estimator.update(*image)) { +// const auto& addition = rotation_estimator.addition(); +// for (const auto& point : addition.tracked_features) { +// visual.draw_later(Canvas::Point { +// .origin = point.make(), +// .radius = 1, +// .color = kGreen, +// }); +// } +// visual.publish(-result->yaw_rate, "estimate_yaw"); +// visual.publish(-result->pitch_rate, "estimate_pitch"); +// } + +class RotationEstimator { + RMCS_PIMPL_DEFINITION(RotationEstimator) + +public: + struct Estimate { + TimePoint timestamp; + double dt; + + double yaw_rate; + double pitch_rate; + double roll_rate; + + double confidence; + }; + struct Addition { + TimePoint timestamp { }; + double dt { 0.0 }; + + bool valid { false }; + + int tracked_points { 0 }; + int inlier_points { 0 }; + int feature_points { 0 }; + + double scale { 1.0 }; + double confidence { 0.0 }; + + std::vector tracked_features; + }; + + auto configure(std::array camera_matrix) -> void; + auto configure(std::array distort_coeff) -> void; + + auto update(const Image& image) -> std::optional; + + auto update_mask(std::span armors) -> void; + auto update_mask(std::span lightbars) -> void; + + auto addition() const -> const Addition&; + + auto reset() -> void; +}; + +} diff --git a/src/utility/image/drawable.cpp b/src/utility/image/drawable.cpp index ef16ae0..350234e 100644 --- a/src/utility/image/drawable.cpp +++ b/src/utility/image/drawable.cpp @@ -99,4 +99,9 @@ auto Canvas::draw(const Text& text) -> void { cv::putText(mat, text.content, org, font, scale, font_color, line_thickness, cv::LINE_AA); } +auto Canvas::draw(const Point& point) -> void { + auto& mat = canvas.details().mat; + cv::circle(mat, point.origin, point.radius, point.color, -1, cv::LINE_AA); +} + } diff --git a/src/utility/image/drawable.hpp b/src/utility/image/drawable.hpp index 2d16e2e..6ece909 100644 --- a/src/utility/image/drawable.hpp +++ b/src/utility/image/drawable.hpp @@ -19,17 +19,23 @@ inline const auto kOrange = cv::Scalar { 000, 165, 255, 255 }; inline const auto kPurple = cv::Scalar { 128, 000, 128, 255 }; inline const auto kPink = cv::Scalar { 203, 192, 255, 255 }; -struct Text { - std::string content; - cv::Point2i top_left; - cv::Scalar color = kWhite; -}; -struct Area { - cv::Rect2i rect; - cv::Scalar color = kWhite; -}; - struct Canvas { + struct Text { + std::string content; + cv::Point2i top_left; + cv::Scalar color = kWhite; + }; + struct Area { + cv::Rect2i rect; + cv::Scalar color = kWhite; + }; + struct Point { + cv::Point2i origin; + int radius; + + cv::Scalar color = kWhite; + }; + Image& canvas; std::uint8_t transparency = 255; @@ -39,6 +45,7 @@ struct Canvas { auto draw(const Lightbar2d&) -> void; auto draw(const cv::Rect2i&) -> void; auto draw(const Text&) -> void; + auto draw(const Point&) -> void; }; template concept drawable_trait = requires(Canvas& canvas) { canvas.draw(std::declval()); }; diff --git a/src/utility/math/linear.hpp b/src/utility/math/linear.hpp index f767f33..ef9f81a 100644 --- a/src/utility/math/linear.hpp +++ b/src/utility/math/linear.hpp @@ -264,4 +264,6 @@ struct Transform { } }; +constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + } diff --git a/src/utility/rclcpp/visual/scalar.cpp b/src/utility/rclcpp/visual/scalar.cpp new file mode 100644 index 0000000..f4cbdc4 --- /dev/null +++ b/src/utility/rclcpp/visual/scalar.cpp @@ -0,0 +1,40 @@ +#include "scalar.hpp" +#include "utility/rclcpp/node.details.hpp" + +#include + +using namespace rmcs::util::visual; + +using Float64 = std_msgs::msg::Float64; + +struct Scalar::Impl { + static inline rclcpp::Clock rclcpp_clock { RCL_STEADY_TIME }; + + RclcppNode& node; + std::string name; + std::shared_ptr> publisher; + + explicit Impl(RclcppNode& node, std::string name) noexcept + : node { node } + , name { std::move(name) } { } + + auto publish(double value) -> void { + if (!publisher) { + auto topic_name = node.get_pub_topic_prefix() + name; + publisher = node.details->make_pub(topic_name, qos::debug); + } + auto msg = Float64 {}; + msg.data = value; + publisher->publish(msg); + } +}; + +Scalar::Scalar(RclcppNode& node, std::string name) + : pimpl { std::make_unique(node, std::move(name)) } { } + +Scalar::~Scalar() noexcept = default; + +Scalar::Scalar(Scalar&&) noexcept = default; +auto Scalar::operator=(Scalar&&) noexcept -> Scalar& = default; + +auto Scalar::publish(double value) -> void { pimpl->publish(value); } diff --git a/src/utility/rclcpp/visual/scalar.hpp b/src/utility/rclcpp/visual/scalar.hpp new file mode 100644 index 0000000..6edaeec --- /dev/null +++ b/src/utility/rclcpp/visual/scalar.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "utility/pimpl.hpp" +#include "utility/rclcpp/node.hpp" + +namespace rmcs::util::visual { + +struct Scalar { + RMCS_PIMPL_DEFINITION(Scalar) + +public: + Scalar(RclcppNode&, std::string name); + + Scalar(Scalar&&) noexcept; + Scalar& operator=(Scalar&&) noexcept; + + auto publish(double value) -> void; +}; + +} diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index ea3e2f3..6c27f67 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -8,14 +8,7 @@ namespace rmcs { -template -concept context_trait = std::is_trivially_copyable_v; - struct AutoAimState { - static constexpr auto kLabel = "/shm_autoaim_state"; - static constexpr auto kLength = 512; - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - TimePoint timestamp { }; bool should_control { false }; @@ -39,15 +32,10 @@ struct AutoAimState { }; } }; -static_assert(context_trait); struct SystemContext { using RobotId = rmcs_msgs::RobotId; - static constexpr auto kLabel = "/shm_control_state"; - static constexpr auto kLength = 512; - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - /// Dynamic Context /// TimePoint timestamp { }; @@ -79,6 +67,5 @@ struct SystemContext { }; } }; -static_assert(context_trait); } // namespace rmcs diff --git a/tool/cxx/CMakeLists.txt b/tool/cxx/CMakeLists.txt index 8d47af9..107fc37 100644 --- a/tool/cxx/CMakeLists.txt +++ b/tool/cxx/CMakeLists.txt @@ -119,6 +119,21 @@ if(HIKCAMERA_AVAILABLE) ${OpenCV_LIBS} ${hikcamera_LIBRARIES} ) + + # Rotation Estimator + add_executable( + rotation_estimator + ${TOOL_DIR}/rotation_estimator.cpp + ${RMCS_SRC_DIR}/module/perception/rotation_estimator.cpp + ${RMCS_SRC_DIR}/utility/image/image.cpp + ) + target_link_libraries( + rotation_estimator + rclcpp::rclcpp + ${geometry_msgs_LIBRARIES} + ${OpenCV_LIBS} + ${hikcamera_LIBRARIES} + ) else() message(STATUS "hikcamera 未检测到，跳过相关构建") endif() diff --git a/tool/cxx/rotation_estimator.cpp b/tool/cxx/rotation_estimator.cpp new file mode 100644 index 0000000..b8b2875 --- /dev/null +++ b/tool/cxx/rotation_estimator.cpp @@ -0,0 +1,219 @@ +#include "module/perception/rotation_estimator.hpp" +#include "utility/image/image.details.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace { + +using RotationMessage = geometry_msgs::msg::Vector3Stamped; + +struct LatestFrame { + std::mutex mutex; + std::condition_variable cv; + + std::unique_ptr image; + std::uint64_t sequence { 0 }; +}; + +struct Statistics { + std::atomic captured { 0 }; + std::atomic dropped { 0 }; + std::atomic read_time_sum_ms { 0.0 }; + + std::size_t processed { 0 }; + std::size_t estimated { 0 }; + double update_time_sum_ms { 0.0 }; + + int tracked_points { 0 }; + int inlier_points { 0 }; + double confidence { 0.0 }; + + auto add_read_time(std::chrono::steady_clock::duration duration) -> void { + read_time_sum_ms.fetch_add(std::chrono::duration(duration).count(), + std::memory_order::relaxed); + } + + auto add_update_time(std::chrono::steady_clock::duration duration) -> void { + update_time_sum_ms += std::chrono::duration(duration).count(); + } + + auto record_estimate(const rmcs::RotationEstimator::Addition& addition) -> void { + estimated += 1; + tracked_points = addition.tracked_points; + inlier_points = addition.inlier_points; + confidence = addition.confidence; + } + + auto record_failed_estimate(const rmcs::RotationEstimator::Addition& addition) -> void { + tracked_points = addition.tracked_points; + inlier_points = addition.inlier_points; + confidence = 0.0; + } + + auto print_and_reset() -> void { + const auto captured_count = captured.exchange(0, std::memory_order::relaxed); + const auto dropped_count = dropped.exchange(0, std::memory_order::relaxed); + const auto read_time_sum = read_time_sum_ms.exchange(0.0, std::memory_order::relaxed); + + const auto read_divisor = captured_count == 0 ? 1.0 : static_cast(captured_count); + const auto update_divisor = processed == 0 ? 1.0 : static_cast(processed); + + std::println("[rotation_estimator] captured={}, processed={}, estimates={}, dropped={}, " + "tracked={}, " + "inliers={}, confidence={:.2f}, read={:.2f}ms, update={:.2f}ms", + captured_count, processed, estimated, dropped_count, tracked_points, inlier_points, + confidence, read_time_sum / read_divisor, update_time_sum_ms / update_divisor); + + processed = 0; + estimated = 0; + update_time_sum_ms = 0.0; + } +}; + +constexpr auto kCameraMatrix = std::array { + 1.722231837421459e+03, + 0.0, + 7.013056440882832e+02, + 0.0, + 1.724876404292754e+03, + 5.645821718351237e+02, + 0.0, + 0.0, + 1.0, +}; + +constexpr auto kDistortCoeff = std::array { + -0.064232403853946, + -0.087667493884102, + 0.0, + 0.0, + 0.792381808294582, +}; + +} // namespace + +auto main(int argc, char** argv) -> int { + rclcpp::init(argc, argv); + + auto node = std::make_shared("rotation_estimator_tool"); + auto publisher = node->create_publisher( + "/rmcs/auto_aim/visual_rotation_rate", rclcpp::QoS { 10 }.best_effort().keep_last(10)); + auto clock = rclcpp::Clock { RCL_STEADY_TIME }; + + auto config = hikcamera::Config { + .timeout_ms = 2'000, + .exposure_us = 1'500, + .framerate = 120, + .fixed_framerate = true, + }; + config.framerate = 165; + + auto camera = hikcamera::Camera { }; + camera.configure(config); + if (auto result = camera.connect(); !result) { + std::println("[rotation_estimator] Failed to connect camera: {}", result.error()); + rclcpp::shutdown(); + return 1; + } + + auto estimator = rmcs::RotationEstimator { }; + estimator.configure(kCameraMatrix); + estimator.configure(kDistortCoeff); + + auto latest_frame = LatestFrame { }; + auto statistics = Statistics { }; + + auto capture_thread = std::jthread { [&](const std::stop_token& stop) { + while (!stop.stop_requested() && rclcpp::ok()) { + const auto read_begin = std::chrono::steady_clock::now(); + auto captured = camera.read_image_with_timestamp(); + const auto read_end = std::chrono::steady_clock::now(); + statistics.add_read_time(read_end - read_begin); + + if (!captured) { + std::println("[rotation_estimator] Failed to read image: {}", captured.error()); + continue; + } + + auto image = std::make_unique(); + image->details().set_mat(captured->mat.clone()); + image->set_timestamp(captured->timestamp); + + { + auto lock = std::lock_guard { latest_frame.mutex }; + if (latest_frame.image) statistics.dropped.fetch_add(1, std::memory_order::relaxed); + latest_frame.image = std::move(image); + latest_frame.sequence += 1; + } + statistics.captured.fetch_add(1, std::memory_order::relaxed); + latest_frame.cv.notify_one(); + } + } }; + + auto last_print = std::chrono::steady_clock::now(); + auto last_sequence = std::uint64_t { 0 }; + + std::println("[rotation_estimator] Publishing /rmcs/auto_aim/visual_rotation_rate"); + std::println("[rotation_estimator] Press Ctrl+C to quit"); + + while (rclcpp::ok()) { + rclcpp::spin_some(node); + + auto image = std::unique_ptr { }; + { + auto lock = std::unique_lock { latest_frame.mutex }; + latest_frame.cv.wait_for(lock, std::chrono::milliseconds { 5 }, + [&] { return latest_frame.sequence != last_sequence || !rclcpp::ok(); }); + if (latest_frame.sequence == last_sequence || !latest_frame.image) continue; + + last_sequence = latest_frame.sequence; + image = std::move(latest_frame.image); + } + + statistics.processed += 1; + + const auto update_begin = std::chrono::steady_clock::now(); + auto estimate = estimator.update(*image); + const auto update_end = std::chrono::steady_clock::now(); + statistics.add_update_time(update_end - update_begin); + const auto& addition = estimator.addition(); + + if (estimate) { + statistics.record_estimate(addition); + + auto message = RotationMessage { }; + message.header.stamp = clock.now(); + message.header.frame_id = "camera_link"; + message.vector.x = estimate->yaw_rate; + message.vector.y = estimate->pitch_rate; + message.vector.z = estimate->roll_rate; + publisher->publish(message); + } else { + statistics.record_failed_estimate(addition); + } + + const auto now = std::chrono::steady_clock::now(); + if (now - last_print >= std::chrono::seconds { 1 }) { + statistics.print_and_reset(); + last_print = now; + } + } + + capture_thread.request_stop(); + latest_frame.cv.notify_one(); + if (capture_thread.joinable()) capture_thread.join(); + std::ignore = camera.disconnect(); + rclcpp::shutdown(); + std::println("[rotation_estimator] Stopped"); + return 0; +}