From 94048da193b962912ed8cd6a7090f2c8085cf071 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 4 May 2026 13:10:37 +0800 Subject: [PATCH 1/2] feat: refine visualization granularity with multi-layered debug control --- config/config.yaml | 9 ++++++++- src/kernel/visualization.cpp | 38 ++++++++++++++++++++++++++---------- src/runtime.cpp | 15 +++++++++----- 3 files changed, 46 insertions(+), 16 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index 65b2d4f..1ab56fc 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -107,7 +107,14 @@ fire_control: visualization: framerate: 60 + # e.g. monitor_host: "192.168.3.125" monitor_host: "127.0.0.1" - # monitor_host: "192.168.3.125" monitor_port: "5000" stream_type: "RTP_JEPG" + + image_enabled: true + visible_armors_enabled: true + visible_robot_enabled: true + camera_pose_enabled: true + aiming_direction_enabled: true + mpc_plan_enabled: true diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 5f15b5d..31f2d42 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -33,18 +33,31 @@ struct Visualization::Impl { util::string_t monitor_port = "5000"; util::string_t stream_type = "RTP_JEPG"; + bool image_enabled = true; + bool visible_armors_enabled = true; + bool visible_robot_enabled = true; + bool camera_pose_enabled = true; + bool aiming_direction_enabled = true; + bool mpc_plan_enabled = true; + static constexpr auto metas = std::tuple { - &Config::framerate, - "framerate", - &Config::monitor_host, - "monitor_host", - &Config::monitor_port, - "monitor_port", - &Config::stream_type, - "stream_type", + // clang-format off + &Config::framerate, "framerate", + &Config::monitor_host, "monitor_host", + &Config::monitor_port, "monitor_port", + &Config::stream_type, "stream_type", + &Config::image_enabled, "image_enabled", + &Config::visible_armors_enabled, "visible_armors_enabled", + &Config::visible_robot_enabled, "visible_robot_enabled", + &Config::camera_pose_enabled, "camera_pose_enabled", + &Config::aiming_direction_enabled,"aiming_direction_enabled", + &Config::mpc_plan_enabled, "mpc_plan_enabled", + // clang-format on }; }; + Config config {}; + Printer log { "visual" }; std::unique_ptr session; @@ -68,7 +81,6 @@ struct Visualization::Impl { } auto initialize(const YAML::Node& yaml, RclcppNode& visual_node) noexcept -> NormalResult { - auto config = Config { }; auto result = config.serialize(yaml); if (!result.has_value()) { return std::unexpected { result.error() }; @@ -103,13 +115,14 @@ struct Visualization::Impl { }); is_initialized = true; - return { }; + return {}; } auto initialized() const noexcept { return is_initialized; } auto send_image(const Image& image) noexcept -> bool { if (!is_initialized) return false; + if (!config.image_enabled) return false; const auto& mat = image.details().mat; @@ -150,16 +163,19 @@ struct Visualization::Impl { auto update_visible_armors(std::span armors) const -> bool { if (!is_initialized) return false; + if (!config.visible_armors_enabled) return false; return armors_detect->visualize(armors, "visible_armors", kOdomLink); } auto update_visible_robot(std::span armors) const -> bool { if (!is_initialized) return false; + if (!config.visible_robot_enabled) return false; return armors_group->visualize(armors, "visible_robot", kOdomLink); } auto update_aiming_direction(double yaw, double pitch) const -> void { if (!is_initialized) return; + if (!config.aiming_direction_enabled) return; aiming_direction->move(Translation::kZero(), euler_to_quaternion(yaw, pitch, 0.0)); aiming_direction->update(); } @@ -167,12 +183,14 @@ struct Visualization::Impl { auto update_mpc_plan(double yaw, double pitch, double yaw_rate, double pitch_rate, double yaw_acc, double pitch_acc) const -> void { if (!is_initialized) return; + if (!config.mpc_plan_enabled) return; mpc_plan->publish_planned_yaw(yaw, yaw_rate, yaw_acc); mpc_plan->publish_planned_pitch(pitch, pitch_rate, pitch_acc); } auto update_camera_pose(const Orientation& orientation) const -> void { if (!is_initialized) return; + if (!config.camera_pose_enabled) return; camera_transform->move(Translation::kZero(), orientation); camera_transform->update(); } diff --git a/src/runtime.cpp b/src/runtime.cpp index f40e314..eeebaf3 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -172,7 +172,10 @@ auto main() -> int { auto target = tracker.decide(armors_3d, image->get_timestamp()); auto command = AutoAimState::kInvalid(); if (target.allow_control && target.snapshot) { - auto& snapshot = target.snapshot; + auto& snapshot = target.snapshot; + auto armors = snapshot->predicted_armors(Clock::now()); + visualization.update_visible_robot(armors); + const auto control = target.tracking_confirmed; const auto yaw = context.yaw; if (auto result = fire_control.solve(*snapshot, control, yaw)) { @@ -187,11 +190,13 @@ auto main() -> int { command.pitch_acc = result->pitch_acc; command.feedforward_valid = result->feedforward_valid; command.robot_center = Translation { result->center_position }; + + visualization.update_aiming_direction(command.yaw, command.pitch); + if (command.feedforward_valid) { + visualization.update_mpc_plan(command.yaw, command.pitch, command.yaw_rate, + command.pitch_rate, command.yaw_acc, command.pitch_acc); + } } - auto armors = snapshot->predicted_armors(Clock::now()); - visualization.update_visible_robot(armors); - visualization.update_mpc_plan(command.yaw, command.pitch, command.yaw_rate, - command.pitch_rate, command.yaw_acc, command.pitch_acc); } /// 4. Transmit State From 9fee19ab465cbf92946c6dc2bc7eb08d7f2a7bc2 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 4 May 2026 13:33:33 +0800 Subject: [PATCH 2/2] fix: resolve visualization residual bug in MPC trajectory --- src/runtime.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/runtime.cpp b/src/runtime.cpp index eeebaf3..434698f 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -192,10 +192,8 @@ auto main() -> int { command.robot_center = Translation { result->center_position }; visualization.update_aiming_direction(command.yaw, command.pitch); - if (command.feedforward_valid) { - visualization.update_mpc_plan(command.yaw, command.pitch, command.yaw_rate, - command.pitch_rate, command.yaw_acc, command.pitch_acc); - } + visualization.update_mpc_plan(command.yaw, command.pitch, command.yaw_rate, + command.pitch_rate, command.yaw_acc, command.pitch_acc); } }