cyy/yq_2026_duo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add wust typr mpc and mutipule x

Browse Source
This commit is contained in:
cyy_mac
2026-03-27 03:41:42 +08:00
parent 2c64655fae
commit 7dcb53bb77
192 changed files with 29571 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

198
wust_vision-main/tasks/auto_guidance/auto_guidance.cpp Normal file
View File

@@ -0,0 +1,198 @@
#include "auto_guidance.hpp"
#include "tasks/auto_guidance/guidance_detector/detector_base.hpp"
#include "tasks/auto_guidance/guidance_detector/detector_factory.hpp"
#include "tasks/auto_guidance/guidance_tracker/guidance_tracker.hpp"
#include "tasks/utils/utils.hpp"
#include "wust_vl/common/concurrency/queues.hpp"
#include "wust_vl/common/utils/logger.hpp"
#include "wust_vl/common/utils/timer.hpp"
namespace wust_vision {
namespace auto_guidance {
struct AutoGuidance::Impl {
~Impl() {
lights_queue_->stop();
if (processing_thread_) {
processing_thread_->stop();
wust_vl::common::concurrency::ThreadManager::instance().unregisterThread(
processing_thread_->getName()
);
}
}
void init(const YAML::Node& config, const std::pair<cv::Mat, cv::Mat>& camera_info) {
camera_info_ = camera_info;
std::string backend = config["backend"].as<std::string>();
std::cout << "backend: " << backend << std::endl;
auto detector_cfg = config["detector"];
detector_ = DetectorFactory::createDetector(backend, detector_cfg, debug_);
detector_->setCallback(std::bind(
&AutoGuidance::Impl::detectCallback,
this,
std::placeholders::_1,
std::placeholders::_2
));
tracker_ = GuidanceTracker::create(config["tracker"]);
lights_queue_ =
std::make_unique<wust_vl::common::concurrency::OrderedQueue<GreenLights>>(100, 500);
latency_averager_ =
std::make_unique<wust_vl::common::concurrency::Averager<double>>(100);
}
void pushInput(CommonFrame& frame) {
img_recv_count_++;
if (detector_) {
detector_->pushInput(frame);
}
}
void detectCallback(const std::vector<GreenLight>& objs, const CommonFrame& frame) {
detect_finish_count_++;
GreenLights lights;
lights.lights = objs;
lights.timestamp = frame.img_frame.timestamp;
lights.id = frame.id;
for (auto& light: lights.lights) {
light.solvePnP(camera_info_.first, camera_info_.second);
light.timestamp = frame.img_frame.timestamp;
light.image_size = frame.img_frame.src_img.size();
}
green_lights_ = lights;
lights_queue_->enqueue(lights);
if (debug_) {
std::lock_guard<std::mutex> lock(dbg_mutex_);
dbg_.lights = lights;
dbg_.img_frame = frame.img_frame;
}
}
void lightsCallback(const GreenLights& lights) {
if (lights.timestamp <= tracker_->getLastTime()) {
WUST_WARN(logger_) << "Received out-of-order armor data, discarded.";
return;
}
GuidanceTarget target;
target = tracker_->track(lights);
{
std::lock_guard<std::mutex> lock(target_mutex_);
guidance_target_ = target;
}
auto now = std::chrono::steady_clock::now();
auto latency_ms = wust_vl::common::utils::time_utils::durationMs(lights.timestamp, now);
latency_averager_->add(latency_ms);
dbg_.latency_ms = latency_averager_->average();
if (debug_) {
std::lock_guard<std::mutex> lock(dbg_mutex_);
dbg_.target = target;
}
}
void start() {
processing_thread_ = wust_vl::common::concurrency::MonitoredThread::create(
"AutoAimProcessingThread",
[this](wust_vl::common::concurrency::MonitoredThread::Ptr self) {
this->processingLoop(self);
}
);
wust_vl::common::concurrency::ThreadManager::instance().registerThread(
processing_thread_
);
run_flag_ = true;
}
void processingLoop(wust_vl::common::concurrency::MonitoredThread::Ptr self) {
while (!self->isAlive()) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
while (self->isAlive()) {
if (!self->waitPoint())
break;
self->heartbeat();
printStats();
GreenLights lights;
bool skip;
// if (lights_queue_->dequeue_wait(lights, skip)) {
// lightsCallback(lights);
// tracker_finish_count_++;
// if (skip) {
// WUST_DEBUG(logger_) << "OrderQueue skip";
// }
// }
if (!lights_queue_->try_dequeue(lights)) {
std::this_thread::sleep_for(std::chrono::milliseconds(3));
continue;
}
lightsCallback(lights);
tracker_finish_count_++;
}
}
GuidanceTarget getTarget() {
timer_count_++;
std::lock_guard<std::mutex> lock(target_mutex_);
return guidance_target_;
}
void printStats() {
utils::XSecOnce(
[&] {
WUST_INFO(logger_)
<< "Rec: " << img_recv_count_ << ", Det: " << detect_finish_count_
<< ", Fin: " << tracker_finish_count_ << ", Lat: " << dbg_.latency_ms
<< "ms"
<< ", Tc:" << timer_count_;
img_recv_count_ = 0;
detect_finish_count_ = 0;
tracker_finish_count_ = 0;
timer_count_ = 0;
},
1.0
);
}
std::unique_ptr<detector_base> detector_;
std::string logger_ = "auto_guidance";
std::chrono::steady_clock::time_point last_stat_time_steady_ =
std::chrono::steady_clock::now();
GuidanceTracker::Ptr tracker_;
bool run_flag_ = false;
int detect_finish_count_ = 0;
int img_recv_count_ = 0;
int tracker_finish_count_ = 0;
int timer_count_ = 0;
bool debug_ = false;
GuidanceTarget guidance_target_;
GreenLights green_lights_;
std::shared_ptr<wust_vl::common::concurrency::MonitoredThread> processing_thread_;
std::unique_ptr<wust_vl::common::concurrency::OrderedQueue<GreenLights>> lights_queue_;
std::unique_ptr<wust_vl::common::concurrency::Averager<double>> latency_averager_;
std::pair<cv::Mat, cv::Mat> camera_info_;
AutoGuidanceDebug dbg_;
std::mutex target_mutex_;
std::mutex dbg_mutex_;
};
AutoGuidance::AutoGuidance(): _impl(std::make_unique<Impl>()) {}
AutoGuidance::~AutoGuidance() {
_impl.reset();
}
void
AutoGuidance::init(const YAML::Node& config, const std::pair<cv::Mat, cv::Mat>& camera_info) {
_impl->init(config, camera_info);
}
void AutoGuidance::start() {
_impl->start();
}
void AutoGuidance::pushInput(CommonFrame& frame) {
_impl->pushInput(frame);
}
void AutoGuidance::setDebug(bool debug) {
_impl->debug_ = debug;
}
AutoGuidanceDebug AutoGuidance::getDebug() {
std::lock_guard<std::mutex> lock(_impl->dbg_mutex_);
return _impl->dbg_;
}
GuidanceTarget AutoGuidance::getTarget() {
return _impl->getTarget();
}
} // namespace auto_guidance
} // namespace wust_vision