Skip to content

feat: add UiRobotCenterOverlay for flight referee UI #87

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
zlq04222 wants to merge 5 commits into
base: main
Choose a base branch
from
Open

feat: add UiRobotCenterOverlay for flight referee UI #87

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
a431649
feat: add UiRobotCenterOverlay for flight referee UI
zlq04222 Jul 7, 2026
2953667
Update rmcs_ws/src/rmcs_core/src/referee/app/ui/robot_center_overlay.cpp
zlq04222 Jul 7, 2026
57eec77
refactor: address review comments for robot center overlay
zlq04222 Jul 7, 2026
207e930
feat: Add executor timing stats with thread config support (#88)
qzhhhi Jul 7, 2026
a45e3f9
wip: Clean up and rename
creeper5820 Jul 8, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions rmcs_ws/src/rmcs_bringup/config/flight.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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::AutoAimUi -> auto_aim_ui

# - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster
# - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster
Expand Down Expand Up @@ -151,3 +152,9 @@ bullet_feeder_velocity_pid_controller:
kp: 0.583
ki: 0.0
kd: 0.0

auto_aim_ui:
ros__parameters:
offset_x: 0.000
offset_y: 0.000
offset_z: 0.000
1 change: 1 addition & 0 deletions rmcs_ws/src/rmcs_core/plugins.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,4 +56,5 @@
<class type="rmcs_core::referee::app::ui::DeformableInfantry" base_class_type="rmcs_executor::Component" />
<class type="rmcs_core::referee::app::ui::Hero" base_class_type="rmcs_executor::Component" />
<class type="rmcs_core::referee::app::ui::Flight" base_class_type="rmcs_executor::Component" />
<class type="rmcs_core::referee::app::ui::AutoAimUi" base_class_type="rmcs_executor::Component" />
</library>
183 changes: 183 additions & 0 deletions rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,183 @@
#include <algorithm>
#include <cmath>
#include <limits>

#include <rclcpp/node.hpp>
#include <rmcs_description/tf_description.hpp>
#include <rmcs_executor/component.hpp>

#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::uint16_t>(std::lround(point.x()), 0, kScreenW - 1);
const auto y = std::clamp<std::uint16_t>(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<std::uint16_t>(x, 0, kScreenW - 1);
};
constexpr auto clamp_y = [](int y) {
return std::clamp<std::uint16_t>(y, 0, kScreenH - 1);
};
Comment on lines +66 to +71

@coderabbitai coderabbitai Bot Jul 8, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🎯 Functional Correctness | 🟠 Major | ⚡ Quick win

🧩 Analysis chain

🏁 Script executed:

#!/bin/bash
# 确认 clamp 辅助函数的调用方确实会产生负实参
sed -n '62,105p' rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp

Repository: Alliance-Algorithm/RMCS

Length of output: 1998

🏁 Script executed:

#!/bin/bash
set -euo pipefail

# Locate the file and inspect the relevant region with line numbers.
git ls-files 'rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp'
echo '---'
cat -n rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp | sed -n '62,105p'
echo '---'
# Show all clamp_x/clamp_y references in the file for call-site context.
rg -n 'clamp_[xy]\(' rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp

Repository: Alliance-Algorithm/RMCS

Length of output: 1969

先在 int 域内 clamp，避免负坐标回绕到屏幕最大边缘 std::clamp<std::uint16_t> 会先把 int 实参转成 uint16_t，所以靠近左上角时的负值会被夹到上界而不是 0。改成先 std::clamp(x, 0, ...)，再转换为 uint16_t。

🤖 Prompt for AI Agents 
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.

In `@rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp` around lines 66 - 71,
The clamp_x and clamp_y lambdas in auto_aim.cpp are clamping in uint16_t, which
can wrap negative int values to the screen maximum instead of 0. Update these
helpers to first call std::clamp on the int input using an int range, then cast
the clamped result to std::uint16_t so negative coordinates stay pinned to the
left/top edge.


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> tf_;
InputInterface<Eigen::Vector3d> robot_center_;
InputInterface<bool> 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<OdomImu, CameraLink>(*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<double>::quiet_NaN(),
std::numeric_limits<double>::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/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::AutoAimUi, rmcs_executor::Component)
3 changes: 2 additions & 1 deletion rmcs_ws/src/rmcs_executor/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,8 @@ project(rmcs_executor)

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20")
set(CMAKE_CXX_STANDARD 23)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
Comment thread
creeper5820 marked this conversation as resolved.

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-g1 -rdynamic -O2 -Wall -Wextra -Wpedantic)
Expand Down
Loading