From 2d0331296e5235d4913e4a5966625e28bcc9053d Mon Sep 17 00:00:00 2001 From: zlq040222 <1542498005@qq.com> Date: Tue, 7 Jul 2026 15:22:47 +0800 Subject: [PATCH 1/4] feat: add UiRobotCenterOverlay for flight referee UI --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 7 + rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../referee/app/ui/robot_center_overlay.cpp | 186 ++++++++++++++++++ 3 files changed, 194 insertions(+) create mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 2291e12a..4f83e999 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::referee::Status -> referee_status - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::Flight -> referee_ui_flight + - rmcs_core::referee::app::ui::UiRobotCenterOverlay -> referee_ui_robot_center # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster @@ -151,3 +152,9 @@ bullet_feeder_velocity_pid_controller: kp: 0.583 ki: 0.0 kd: 0.0 + +referee_ui_robot_center: + ros__parameters: + offset_x: 0.000 + offset_y: 0.000 + offset_z: 0.202 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 1f46763e..559ab114 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -56,4 +56,5 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp new file mode 100644 index 00000000..f8746718 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp @@ -0,0 +1,186 @@ +#include + +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::referee::app::ui { +using namespace rmcs_description; + +class UiRobotCenterOverlay + : public rmcs_executor::Component + , public rclcpp::Node { +public: + UiRobotCenterOverlay() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , center_ring_{Shape::Color::GREEN, 2, 0, 0, 5, 5, false} + , cross_top_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false} + , cross_bottom_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false} + , cross_left_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false} + , cross_right_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false} { + offset_x_ = get_parameter_or("offset_x", 0.000); + offset_y_ = get_parameter_or("offset_y", 0.000); + offset_z_ = get_parameter_or("offset_z", 0.202); + + register_input("/tf", tf_); + register_input("/auto_aim/robot_center", robot_center_, false); + register_input("/auto_aim/should_shoot", should_shoot_, false); + } + + void update() override { + using namespace Eigen; + + if (!robot_center_.ready()) { + hide_all(); + return; + } + const auto& center = *robot_center_; + if (!center.allFinite()) { + hide_all(); + return; + } + + const auto camera_pose = fast_tf::lookup_transform(*tf_); + const Quaterniond q_aa{camera_pose.rotation()}; + const Vector3d t_aa = camera_pose.translation(); + + const Vector3d t_ui = t_aa - q_aa * Vector3d{offset_x_, offset_y_, offset_z_}; + + const Vector3d P_ros = q_aa.inverse() * (center - t_ui); + + const double x_cv = -P_ros.y(); + const double y_cv = P_ros.z(); + const double z_cv = P_ros.x(); + + if (z_cv <= 1e-6) { + hide_all(); + return; + } + + const double x_norm = x_cv / z_cv; + const double y_norm = y_cv / z_cv; + + const double r2 = x_norm * x_norm + y_norm * y_norm; + const double r4 = r2 * r2; + const double r6 = r4 * r2; + + const double radial = 1.0 + k1_ * r2 + k2_ * r4 + k3_ * r6; + const double x_dist = + x_norm * radial + 2.0 * p1_ * x_norm * y_norm + p2_ * (r2 + 2.0 * x_norm * x_norm); + const double y_dist = + y_norm * radial + p1_ * (r2 + 2.0 * y_norm * y_norm) + 2.0 * p2_ * x_norm * y_norm; + + const double u = fx_ * x_dist + cx_; + const double v = fy_ * y_dist + cy_; + + if (u < 0.0 || u >= static_cast(screen_width) || v < 0.0 + || v >= static_cast(screen_height)) { + hide_all(); + return; + } + + const auto ui = static_cast(std::lround(u)); + const auto vi = static_cast(std::lround(v)); + + const bool should_shoot = should_shoot_.ready() && *should_shoot_; + const auto color = should_shoot ? Shape::Color::ORANGE : Shape::Color::GREEN; + const uint16_t radius = should_shoot ? 10 : 15; + + center_ring_.set_color(color); + center_ring_.set_x(ui); + center_ring_.set_y(vi); + center_ring_.set_r(radius); + center_ring_.set_visible(true); + + if (should_shoot) { + constexpr int cross_len = 16, cross_gap = 5; + const auto clamp_x = [](int x) { + return static_cast( + x < 0 ? 0 : (x >= screen_width ? screen_width - 1 : x)); + }; + const auto clamp_y = [](int y) { + return static_cast( + y < 0 ? 0 : (y >= screen_height ? screen_height - 1 : y)); + }; + + cross_top_.set_color(color); + cross_top_.set_x(ui); + cross_top_.set_y(clamp_y(vi - cross_len)); + cross_top_.set_x2(ui); + cross_top_.set_y2(clamp_y(vi - cross_gap)); + cross_top_.set_visible(true); + + cross_bottom_.set_color(color); + cross_bottom_.set_x(ui); + cross_bottom_.set_y(clamp_y(vi + cross_gap)); + cross_bottom_.set_x2(ui); + cross_bottom_.set_y2(clamp_y(vi + cross_len)); + cross_bottom_.set_visible(true); + + cross_left_.set_color(color); + cross_left_.set_x(clamp_x(ui - cross_len)); + cross_left_.set_y(vi); + cross_left_.set_x2(clamp_x(ui - cross_gap)); + cross_left_.set_y2(vi); + cross_left_.set_visible(true); + + cross_right_.set_color(color); + cross_right_.set_x(clamp_x(ui + cross_gap)); + cross_right_.set_y(vi); + cross_right_.set_x2(clamp_x(ui + cross_len)); + cross_right_.set_y2(vi); + cross_right_.set_visible(true); + } else { + cross_top_.set_visible(false); + cross_bottom_.set_visible(false); + cross_left_.set_visible(false); + cross_right_.set_visible(false); + } + } + +private: + void hide_all() { + center_ring_.set_visible(false); + cross_top_.set_visible(false); + cross_bottom_.set_visible(false); + cross_left_.set_visible(false); + cross_right_.set_visible(false); + } + + static constexpr uint16_t screen_width = 1920, screen_height = 1080; + + static constexpr double fx_ = 730.7267062695; + static constexpr double fy_ = 730.5886055073; + static constexpr double cx_ = 961.8345772991; + static constexpr double cy_ = 549.3357457590; + + static constexpr double k1_ = -0.1764932263; + static constexpr double k2_ = 0.1582678597; + static constexpr double k3_ = -0.1557993057; + static constexpr double p1_ = -0.0000433367; + static constexpr double p2_ = 0.0003937540; + + double offset_x_; + double offset_y_; + double offset_z_; + + InputInterface tf_; + InputInterface robot_center_; + InputInterface should_shoot_; + + Circle center_ring_; + Line cross_top_; + Line cross_bottom_; + Line cross_left_; + Line cross_right_; +}; + +} // namespace rmcs_core::referee::app::ui + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::UiRobotCenterOverlay, rmcs_executor::Component) \ No newline at end of file From 8a2b92a49db53d57bb852d21179d01d446668e3f Mon Sep 17 00:00:00 2001 From: zlq04222 <1542498005@qq.com> Date: Tue, 7 Jul 2026 19:55:06 +0800 Subject: [PATCH 2/4] Update rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> --- .../rmcs_core/src/referee/app/ui/robot_center_overlay.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp index f8746718..ea1a598f 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp @@ -83,8 +83,12 @@ class UiRobotCenterOverlay return; } - const auto ui = static_cast(std::lround(u)); - const auto vi = static_cast(std::lround(v)); + const auto clamp_coord = [](long value, uint16_t upper) { + return static_cast( + value < 0 ? 0 : (value >= upper ? upper - 1 : value)); + }; + const auto ui = clamp_coord(std::lround(u), screen_width); + const auto vi = clamp_coord(std::lround(v), screen_height); const bool should_shoot = should_shoot_.ready() && *should_shoot_; const auto color = should_shoot ? Shape::Color::ORANGE : Shape::Color::GREEN; From 6bc7bb5817543ae45fd06cefd741063361d34b19 Mon Sep 17 00:00:00 2001 From: zlq040222 <1542498005@qq.com> Date: Tue, 7 Jul 2026 23:33:00 +0800 Subject: [PATCH 3/4] refactor: address review comments for robot center overlay - Rename camera constants to kXxxxXxxx convention - Move shape construction to member definition site - Change offset_z default to 0 - Remove using namespace Eigen, use explicit Eigen:: prefix - Extract reproject() function for coordinate projection pipeline --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 2 +- .../referee/app/ui/robot_center_overlay.cpp | 110 +++++++++--------- 2 files changed, 53 insertions(+), 59 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 4f83e999..4efc3e51 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -157,4 +157,4 @@ referee_ui_robot_center: ros__parameters: offset_x: 0.000 offset_y: 0.000 - offset_z: 0.202 + offset_z: 0.000 diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp index ea1a598f..adf0eb52 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp @@ -16,15 +16,10 @@ class UiRobotCenterOverlay UiRobotCenterOverlay() : Node{ get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , center_ring_{Shape::Color::GREEN, 2, 0, 0, 5, 5, false} - , cross_top_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false} - , cross_bottom_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false} - , cross_left_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false} - , cross_right_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false} { + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { offset_x_ = get_parameter_or("offset_x", 0.000); offset_y_ = get_parameter_or("offset_y", 0.000); - offset_z_ = get_parameter_or("offset_z", 0.202); + offset_z_ = get_parameter_or("offset_z", 0.000); register_input("/tf", tf_); register_input("/auto_aim/robot_center", robot_center_, false); @@ -32,8 +27,6 @@ class UiRobotCenterOverlay } void update() override { - using namespace Eigen; - if (!robot_center_.ready()) { hide_all(); return; @@ -44,40 +37,8 @@ class UiRobotCenterOverlay return; } - const auto camera_pose = fast_tf::lookup_transform(*tf_); - const Quaterniond q_aa{camera_pose.rotation()}; - const Vector3d t_aa = camera_pose.translation(); - - const Vector3d t_ui = t_aa - q_aa * Vector3d{offset_x_, offset_y_, offset_z_}; - - const Vector3d P_ros = q_aa.inverse() * (center - t_ui); - - const double x_cv = -P_ros.y(); - const double y_cv = P_ros.z(); - const double z_cv = P_ros.x(); - - if (z_cv <= 1e-6) { - hide_all(); - return; - } - - const double x_norm = x_cv / z_cv; - const double y_norm = y_cv / z_cv; - - const double r2 = x_norm * x_norm + y_norm * y_norm; - const double r4 = r2 * r2; - const double r6 = r4 * r2; - - const double radial = 1.0 + k1_ * r2 + k2_ * r4 + k3_ * r6; - const double x_dist = - x_norm * radial + 2.0 * p1_ * x_norm * y_norm + p2_ * (r2 + 2.0 * x_norm * x_norm); - const double y_dist = - y_norm * radial + p1_ * (r2 + 2.0 * y_norm * y_norm) + 2.0 * p2_ * x_norm * y_norm; - - const double u = fx_ * x_dist + cx_; - const double v = fy_ * y_dist + cy_; - - if (u < 0.0 || u >= static_cast(screen_width) || v < 0.0 + const auto [u, v] = reproject(center); + if (std::isnan(u) || u >= static_cast(screen_width) || v < 0.0 || v >= static_cast(screen_height)) { hide_all(); return; @@ -147,6 +108,39 @@ class UiRobotCenterOverlay } private: + std::pair reproject(const Eigen::Vector3d& center) const { + const auto camera_pose = fast_tf::lookup_transform(*tf_); + const Eigen::Quaterniond q_aa{camera_pose.rotation()}; + const Eigen::Vector3d t_aa = camera_pose.translation(); + + const Eigen::Vector3d t_ui = + t_aa - q_aa * Eigen::Vector3d{offset_x_, offset_y_, offset_z_}; + + const Eigen::Vector3d P_ros = q_aa.inverse() * (center - t_ui); + + const double x_cv = -P_ros.y(); + const double y_cv = P_ros.z(); + const double z_cv = P_ros.x(); + + if (z_cv <= 1e-6) + return {NAN, NAN}; + + const double x_norm = x_cv / z_cv; + const double y_norm = y_cv / z_cv; + + const double r2 = x_norm * x_norm + y_norm * y_norm; + const double r4 = r2 * r2; + const double r6 = r4 * r2; + + const double radial = 1.0 + kK1_ * r2 + kK2_ * r4 + kK3_ * r6; + const double x_dist = + x_norm * radial + 2.0 * kP1_ * x_norm * y_norm + kP2_ * (r2 + 2.0 * x_norm * x_norm); + const double y_dist = + y_norm * radial + kP1_ * (r2 + 2.0 * y_norm * y_norm) + 2.0 * kP2_ * x_norm * y_norm; + + return {kFx_ * x_dist + kCx_, kFy_ * y_dist + kCy_}; + } + void hide_all() { center_ring_.set_visible(false); cross_top_.set_visible(false); @@ -157,16 +151,16 @@ class UiRobotCenterOverlay static constexpr uint16_t screen_width = 1920, screen_height = 1080; - static constexpr double fx_ = 730.7267062695; - static constexpr double fy_ = 730.5886055073; - static constexpr double cx_ = 961.8345772991; - static constexpr double cy_ = 549.3357457590; + static constexpr double kFx_ = 730.7267062695; + static constexpr double kFy_ = 730.5886055073; + static constexpr double kCx_ = 961.8345772991; + static constexpr double kCy_ = 549.3357457590; - static constexpr double k1_ = -0.1764932263; - static constexpr double k2_ = 0.1582678597; - static constexpr double k3_ = -0.1557993057; - static constexpr double p1_ = -0.0000433367; - static constexpr double p2_ = 0.0003937540; + static constexpr double kK1_ = -0.1764932263; + static constexpr double kK2_ = 0.1582678597; + static constexpr double kK3_ = -0.1557993057; + static constexpr double kP1_ = -0.0000433367; + static constexpr double kP2_ = 0.0003937540; double offset_x_; double offset_y_; @@ -176,15 +170,15 @@ class UiRobotCenterOverlay InputInterface robot_center_; InputInterface should_shoot_; - Circle center_ring_; - Line cross_top_; - Line cross_bottom_; - Line cross_left_; - Line cross_right_; + Circle center_ring_{Shape::Color::GREEN, 2, 0, 0, 5, 5, false}; + Line cross_top_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + Line cross_bottom_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + Line cross_left_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + Line cross_right_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; }; } // namespace rmcs_core::referee::app::ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::UiRobotCenterOverlay, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::UiRobotCenterOverlay, rmcs_executor::Component) From 2f6e7e062001b8b7b501625b6f7ab6c2b3847e43 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 8 Jul 2026 19:48:39 +0800 Subject: [PATCH 4/4] wip: Clean up and rename --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 4 +- rmcs_ws/src/rmcs_core/plugins.xml | 2 +- .../rmcs_core/src/referee/app/ui/auto_aim.cpp | 183 +++++++++++++++++ .../referee/app/ui/robot_center_overlay.cpp | 184 ------------------ 4 files changed, 186 insertions(+), 187 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 4efc3e51..d0082799 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -21,7 +21,7 @@ rmcs_executor: - rmcs_core::referee::Status -> referee_status - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::Flight -> referee_ui_flight - - rmcs_core::referee::app::ui::UiRobotCenterOverlay -> referee_ui_robot_center + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster @@ -153,7 +153,7 @@ bullet_feeder_velocity_pid_controller: ki: 0.0 kd: 0.0 -referee_ui_robot_center: +auto_aim_ui: ros__parameters: offset_x: 0.000 offset_y: 0.000 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 559ab114..3ac3bf0c 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -56,5 +56,5 @@ - + diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp new file mode 100644 index 00000000..b874a165 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp @@ -0,0 +1,183 @@ +#include +#include +#include + +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::referee::app::ui { +using namespace rmcs_description; + +class AutoAimUi + : public rmcs_executor::Component + , public rclcpp::Node { +public: + AutoAimUi() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + + offset_ = Eigen::Vector3d{ + get_parameter_or("offset_x", 0.), + get_parameter_or("offset_y", 0.), + get_parameter_or("offset_z", 0.), + }; + + // What's the point of an auto-aim UI if it doesn't have auto-aim? + register_input("/tf", tf_, true); + register_input("/auto_aim/robot_center", robot_center_, true); + register_input("/auto_aim/should_shoot", should_shoot_, true); + } + + void update() override { + if (!robot_center_->allFinite() || robot_center_->isZero()) { + hide_all(); + return; + } + + const auto point = reproject(*robot_center_); + if (!point.allFinite() // + || point.x() >= kScreenW || point.x() < 0 // + || point.y() >= kScreenH || point.y() < 0 // + ) { + hide_all(); + return; + } + + const auto x = std::clamp(std::lround(point.x()), 0, kScreenW - 1); + const auto y = std::clamp(std::lround(point.y()), 0, kScreenH - 1); + + const auto color = *should_shoot_ ? Shape::Color::ORANGE : Shape::Color::GREEN; + const auto radius = *should_shoot_ ? 10 : 15; + + center_ring_.set_color(color); + center_ring_.set_x(x); + center_ring_.set_y(y); + center_ring_.set_r(radius); + center_ring_.set_visible(true); + + if (*should_shoot_) { + constexpr int kCrossLen = 16; + constexpr int kCrossGap = 5; + + constexpr auto clamp_x = [](int x) { + return std::clamp(x, 0, kScreenW - 1); + }; + constexpr auto clamp_y = [](int y) { + return std::clamp(y, 0, kScreenH - 1); + }; + + cross_top_.set_color(color); + cross_top_.set_x(x); + cross_top_.set_y(clamp_y(y - kCrossLen)); + cross_top_.set_x2(x); + cross_top_.set_y2(clamp_y(y - kCrossGap)); + cross_top_.set_visible(true); + + cross_bottom_.set_color(color); + cross_bottom_.set_x(x); + cross_bottom_.set_y(clamp_y(y + kCrossGap)); + cross_bottom_.set_x2(x); + cross_bottom_.set_y2(clamp_y(y + kCrossLen)); + cross_bottom_.set_visible(true); + + cross_left_.set_color(color); + cross_left_.set_x(clamp_x(x - kCrossLen)); + cross_left_.set_y(y); + cross_left_.set_x2(clamp_x(x - kCrossGap)); + cross_left_.set_y2(y); + cross_left_.set_visible(true); + + cross_right_.set_color(color); + cross_right_.set_x(clamp_x(x + kCrossGap)); + cross_right_.set_y(y); + cross_right_.set_x2(clamp_x(x + kCrossLen)); + cross_right_.set_y2(y); + cross_right_.set_visible(true); + } else { + cross_top_.set_visible(false); + cross_bottom_.set_visible(false); + cross_left_.set_visible(false); + cross_right_.set_visible(false); + } + } + +private: + static constexpr std::uint16_t kScreenW = 1920; + static constexpr std::uint16_t kScreenH = 1080; + + static constexpr double kFx = 730.7267062695; + static constexpr double kFy = 730.5886055073; + static constexpr double kCx = 961.8345772991; + static constexpr double kCy = 549.3357457590; + + static constexpr double kK1 = -0.1764932263; + static constexpr double kK2 = +0.1582678597; + static constexpr double kK3 = -0.1557993057; + static constexpr double kP1 = -0.0000433367; + static constexpr double kP2 = +0.0003937540; + + Eigen::Vector3d offset_ = Eigen::Vector3d::Zero(); + + InputInterface tf_; + InputInterface robot_center_; + InputInterface should_shoot_; + + Circle center_ring_{Shape::Color::GREEN, 2, 0, 0, 5, 5, false}; + Line cross_top_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + Line cross_bottom_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + Line cross_left_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + Line cross_right_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + + void hide_all() { + center_ring_.set_visible(false); + cross_top_.set_visible(false); + cross_bottom_.set_visible(false); + cross_left_.set_visible(false); + cross_right_.set_visible(false); + } + + Eigen::Vector2d reproject(const Eigen::Vector3d& center) const { + const auto camera_pose = fast_tf::lookup_transform(*tf_); + + const auto q_aa = Eigen::Quaterniond{camera_pose.rotation()}; + const auto t_aa = Eigen::Vector3d{camera_pose.translation()}; + + const auto t_ui = Eigen::Vector3d{t_aa - q_aa * offset_}; + + const auto point_ros = Eigen::Vector3d{q_aa.inverse() * (center - t_ui)}; + + const double x_cv = -point_ros.y(); + const double y_cv = +point_ros.z(); + const double z_cv = +point_ros.x(); + + if (z_cv <= 1e-6) + return Eigen::Vector2d{ + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + }; + + const double x_norm = x_cv / z_cv; + const double y_norm = y_cv / z_cv; + + const double r2 = x_norm * x_norm + y_norm * y_norm; + const double r4 = r2 * r2; + const double r6 = r4 * r2; + + const double radial = 1.0 + kK1 * r2 + kK2 * r4 + kK3 * r6; + const double x_dist = + x_norm * radial + 2.0 * kP1 * x_norm * y_norm + kP2 * (r2 + 2.0 * x_norm * x_norm); + const double y_dist = + y_norm * radial + kP1 * (r2 + 2.0 * y_norm * y_norm) + 2.0 * kP2 * x_norm * y_norm; + + return Eigen::Vector2d{kFx * x_dist + kCx, kFy * y_dist + kCy}; + } +}; + +} // namespace rmcs_core::referee::app::ui + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::AutoAimUi, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp deleted file mode 100644 index adf0eb52..00000000 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp +++ /dev/null @@ -1,184 +0,0 @@ -#include - -#include -#include -#include - -#include "referee/app/ui/shape/shape.hpp" - -namespace rmcs_core::referee::app::ui { -using namespace rmcs_description; - -class UiRobotCenterOverlay - : public rmcs_executor::Component - , public rclcpp::Node { -public: - UiRobotCenterOverlay() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { - offset_x_ = get_parameter_or("offset_x", 0.000); - offset_y_ = get_parameter_or("offset_y", 0.000); - offset_z_ = get_parameter_or("offset_z", 0.000); - - register_input("/tf", tf_); - register_input("/auto_aim/robot_center", robot_center_, false); - register_input("/auto_aim/should_shoot", should_shoot_, false); - } - - void update() override { - if (!robot_center_.ready()) { - hide_all(); - return; - } - const auto& center = *robot_center_; - if (!center.allFinite()) { - hide_all(); - return; - } - - const auto [u, v] = reproject(center); - if (std::isnan(u) || u >= static_cast(screen_width) || v < 0.0 - || v >= static_cast(screen_height)) { - hide_all(); - return; - } - - const auto clamp_coord = [](long value, uint16_t upper) { - return static_cast( - value < 0 ? 0 : (value >= upper ? upper - 1 : value)); - }; - const auto ui = clamp_coord(std::lround(u), screen_width); - const auto vi = clamp_coord(std::lround(v), screen_height); - - const bool should_shoot = should_shoot_.ready() && *should_shoot_; - const auto color = should_shoot ? Shape::Color::ORANGE : Shape::Color::GREEN; - const uint16_t radius = should_shoot ? 10 : 15; - - center_ring_.set_color(color); - center_ring_.set_x(ui); - center_ring_.set_y(vi); - center_ring_.set_r(radius); - center_ring_.set_visible(true); - - if (should_shoot) { - constexpr int cross_len = 16, cross_gap = 5; - const auto clamp_x = [](int x) { - return static_cast( - x < 0 ? 0 : (x >= screen_width ? screen_width - 1 : x)); - }; - const auto clamp_y = [](int y) { - return static_cast( - y < 0 ? 0 : (y >= screen_height ? screen_height - 1 : y)); - }; - - cross_top_.set_color(color); - cross_top_.set_x(ui); - cross_top_.set_y(clamp_y(vi - cross_len)); - cross_top_.set_x2(ui); - cross_top_.set_y2(clamp_y(vi - cross_gap)); - cross_top_.set_visible(true); - - cross_bottom_.set_color(color); - cross_bottom_.set_x(ui); - cross_bottom_.set_y(clamp_y(vi + cross_gap)); - cross_bottom_.set_x2(ui); - cross_bottom_.set_y2(clamp_y(vi + cross_len)); - cross_bottom_.set_visible(true); - - cross_left_.set_color(color); - cross_left_.set_x(clamp_x(ui - cross_len)); - cross_left_.set_y(vi); - cross_left_.set_x2(clamp_x(ui - cross_gap)); - cross_left_.set_y2(vi); - cross_left_.set_visible(true); - - cross_right_.set_color(color); - cross_right_.set_x(clamp_x(ui + cross_gap)); - cross_right_.set_y(vi); - cross_right_.set_x2(clamp_x(ui + cross_len)); - cross_right_.set_y2(vi); - cross_right_.set_visible(true); - } else { - cross_top_.set_visible(false); - cross_bottom_.set_visible(false); - cross_left_.set_visible(false); - cross_right_.set_visible(false); - } - } - -private: - std::pair reproject(const Eigen::Vector3d& center) const { - const auto camera_pose = fast_tf::lookup_transform(*tf_); - const Eigen::Quaterniond q_aa{camera_pose.rotation()}; - const Eigen::Vector3d t_aa = camera_pose.translation(); - - const Eigen::Vector3d t_ui = - t_aa - q_aa * Eigen::Vector3d{offset_x_, offset_y_, offset_z_}; - - const Eigen::Vector3d P_ros = q_aa.inverse() * (center - t_ui); - - const double x_cv = -P_ros.y(); - const double y_cv = P_ros.z(); - const double z_cv = P_ros.x(); - - if (z_cv <= 1e-6) - return {NAN, NAN}; - - const double x_norm = x_cv / z_cv; - const double y_norm = y_cv / z_cv; - - const double r2 = x_norm * x_norm + y_norm * y_norm; - const double r4 = r2 * r2; - const double r6 = r4 * r2; - - const double radial = 1.0 + kK1_ * r2 + kK2_ * r4 + kK3_ * r6; - const double x_dist = - x_norm * radial + 2.0 * kP1_ * x_norm * y_norm + kP2_ * (r2 + 2.0 * x_norm * x_norm); - const double y_dist = - y_norm * radial + kP1_ * (r2 + 2.0 * y_norm * y_norm) + 2.0 * kP2_ * x_norm * y_norm; - - return {kFx_ * x_dist + kCx_, kFy_ * y_dist + kCy_}; - } - - void hide_all() { - center_ring_.set_visible(false); - cross_top_.set_visible(false); - cross_bottom_.set_visible(false); - cross_left_.set_visible(false); - cross_right_.set_visible(false); - } - - static constexpr uint16_t screen_width = 1920, screen_height = 1080; - - static constexpr double kFx_ = 730.7267062695; - static constexpr double kFy_ = 730.5886055073; - static constexpr double kCx_ = 961.8345772991; - static constexpr double kCy_ = 549.3357457590; - - static constexpr double kK1_ = -0.1764932263; - static constexpr double kK2_ = 0.1582678597; - static constexpr double kK3_ = -0.1557993057; - static constexpr double kP1_ = -0.0000433367; - static constexpr double kP2_ = 0.0003937540; - - double offset_x_; - double offset_y_; - double offset_z_; - - InputInterface tf_; - InputInterface robot_center_; - InputInterface should_shoot_; - - Circle center_ring_{Shape::Color::GREEN, 2, 0, 0, 5, 5, false}; - Line cross_top_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; - Line cross_bottom_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; - Line cross_left_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; - Line cross_right_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; -}; - -} // namespace rmcs_core::referee::app::ui - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::UiRobotCenterOverlay, rmcs_executor::Component)