From 373b1ce505590a8c502797eab41eb82fb7c76ecd Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 28 Feb 2026 15:25:24 +0800 Subject: [PATCH] change name space --- calibration/calibrate_handeye.cpp | 10 +- calibration/calibrate_robotworld_handeye.cpp | 12 +-- calibration/capture.cpp | 18 ++-- calibration/split_video.cpp | 2 +- src/component/crc.cpp | 4 +- src/component/crc.hpp | 10 +- src/component/exiter.cpp | 4 +- src/component/exiter.hpp | 10 +- src/component/extended_kalman_filter.cpp | 4 +- src/component/extended_kalman_filter.hpp | 10 +- src/component/img_tools.cpp | 4 +- src/component/img_tools.hpp | 10 +- src/component/logger.cpp | 4 +- src/component/logger.hpp | 10 +- src/component/math_tools.cpp | 4 +- src/component/math_tools.hpp | 10 +- src/component/pid.cpp | 4 +- src/component/pid.hpp | 10 +- src/component/plotter.cpp | 4 +- src/component/plotter.hpp | 10 +- src/component/ransac_sine_fitter.cpp | 4 +- src/component/ransac_sine_fitter.hpp | 4 +- src/component/recorder.cpp | 10 +- src/component/recorder.hpp | 12 +-- src/component/thread_pool.hpp | 28 +++--- src/component/thread_safe_queue.hpp | 10 +- src/component/trajectory.cpp | 4 +- src/component/trajectory.hpp | 10 +- src/component/yaml.hpp | 10 +- src/device/camera.cpp | 18 ++-- src/device/camera.hpp | 10 +- src/device/cboard.cpp | 24 ++--- src/device/cboard.hpp | 12 +-- src/device/command.hpp | 10 +- src/device/dm_imu/dm_imu.cpp | 22 ++--- src/device/dm_imu/dm_imu.hpp | 10 +- src/device/gimbal/gimbal.cpp | 46 ++++----- src/device/gimbal/gimbal.hpp | 14 +-- src/device/hikrobot/hikrobot.cpp | 50 +++++----- src/device/hikrobot/hikrobot.hpp | 12 +-- src/device/mindvision/mindvision.cpp | 24 ++--- src/device/mindvision/mindvision.hpp | 12 +-- src/device/ros2/publish2nav.cpp | 4 +- src/device/ros2/publish2nav.hpp | 8 +- src/device/ros2/ros2.cpp | 4 +- src/device/ros2/ros2.hpp | 8 +- src/device/ros2/subscribe2nav.cpp | 4 +- src/device/ros2/subscribe2nav.hpp | 14 +-- src/device/socketcan.hpp | 18 ++-- src/device/usbcamera/usbcamera.cpp | 48 +++++----- src/device/usbcamera/usbcamera.hpp | 10 +- src/module/auto_aim/aimer.cpp | 32 +++---- src/module/auto_aim/aimer.hpp | 6 +- src/module/auto_aim/detector.cpp | 24 ++--- .../auto_aim/multithread/commandgener.cpp | 12 +-- .../auto_aim/multithread/commandgener.hpp | 8 +- .../auto_aim/multithread/mt_detector.cpp | 2 +- .../auto_aim/multithread/mt_detector.hpp | 4 +- src/module/auto_aim/planner/planner.cpp | 46 ++++----- src/module/auto_aim/shooter.cpp | 6 +- src/module/auto_aim/shooter.hpp | 4 +- src/module/auto_aim/solver.cpp | 24 ++--- src/module/auto_aim/target.cpp | 46 ++++----- src/module/auto_aim/target.hpp | 4 +- src/module/auto_aim/tracker.cpp | 18 ++-- src/module/auto_aim/yolos/yolo11.cpp | 8 +- src/module/auto_aim/yolos/yolov5.cpp | 8 +- src/module/auto_aim/yolos/yolov8.cpp | 8 +- src/module/auto_aim/yolos/yolov8.hpp | 2 +- src/module/auto_buff/buff_aimer.cpp | 42 ++++---- src/module/auto_buff/buff_aimer.hpp | 4 +- src/module/auto_buff/buff_detector.cpp | 4 +- src/module/auto_buff/buff_predict.hpp | 16 ++-- src/module/auto_buff/buff_solver.cpp | 8 +- src/module/auto_buff/buff_target.cpp | 96 +++++++++---------- src/module/auto_buff/buff_target.hpp | 4 +- src/module/auto_buff/buff_type.cpp | 2 +- src/module/auto_buff/yolo11_buff.cpp | 4 +- src/module/omniperception/decider.cpp | 48 +++++----- src/module/omniperception/decider.hpp | 12 +-- src/module/omniperception/perceptron.cpp | 14 +-- src/module/omniperception/perceptron.hpp | 8 +- src/task/auto_aim_test.cpp | 26 ++--- src/task/auto_buff_test.cpp | 22 ++--- src/task/camera_detect_test.cpp | 8 +- src/task/camera_test.cpp | 8 +- src/task/camera_thread_test.cpp | 28 +++--- src/task/cboard_test.cpp | 10 +- src/task/detector_video_test.cpp | 4 +- src/task/dm_test.cpp | 8 +- src/task/fire_test.cpp | 14 +-- src/task/gimbal_response_test.cpp | 18 ++-- src/task/gimbal_test.cpp | 14 +-- src/task/handeye_test.cpp | 12 +-- src/task/minimum_vision_system.cpp | 20 ++-- src/task/multi_usbcamera_test.cpp | 12 +-- src/task/planner_test.cpp | 8 +- src/task/planner_test_offline.cpp | 6 +- src/task/publish_test.cpp | 4 +- src/task/subscribe_test.cpp | 8 +- src/task/topic_loop_test.cpp | 6 +- src/task/usbcamera_detect_test.cpp | 8 +- src/task/usbcamera_test.cpp | 8 +- 103 files changed, 702 insertions(+), 702 deletions(-) diff --git a/calibration/calibrate_handeye.cpp b/calibration/calibrate_handeye.cpp index 8436aba..ec88b9b 100644 --- a/calibration/calibrate_handeye.cpp +++ b/calibration/calibrate_handeye.cpp @@ -65,13 +65,13 @@ void load( Eigen::Matrix3d R_imubody2imuabs = q.toRotationMatrix(); Eigen::Matrix3d R_gimbal2world = R_gimbal2imubody.transpose() * R_imubody2imuabs * R_gimbal2imubody; - Eigen::Vector3d ypr = tools::eulers(R_gimbal2world, 2, 1, 0) * 57.3; // degree + Eigen::Vector3d ypr = component::eulers(R_gimbal2world, 2, 1, 0) * 57.3; // degree // 在图片上显示云台的欧拉角，用来检验R_gimbal2imubody是否正确 auto drawing = img.clone(); - tools::draw_text(drawing, fmt::format("yaw {:.2f}", ypr[0]), {40, 40}, {0, 0, 255}); - tools::draw_text(drawing, fmt::format("pitch {:.2f}", ypr[1]), {40, 80}, {0, 0, 255}); - tools::draw_text(drawing, fmt::format("roll {:.2f}", ypr[2]), {40, 120}, {0, 0, 255}); + component::draw_text(drawing, fmt::format("yaw {:.2f}", ypr[0]), {40, 40}, {0, 0, 255}); + component::draw_text(drawing, fmt::format("pitch {:.2f}", ypr[1]), {40, 80}, {0, 0, 255}); + component::draw_text(drawing, fmt::format("roll {:.2f}", ypr[2]), {40, 120}, {0, 0, 255}); // 识别标定板 std::vector centers_2d; @@ -161,7 +161,7 @@ int main(int argc, char * argv[]) cv::cv2eigen(R_camera2gimbal, R_camera2gimbal_eigen); Eigen::Matrix3d R_gimbal2ideal{{0, -1, 0}, {0, 0, -1}, {1, 0, 0}}; Eigen::Matrix3d R_camera2ideal = R_gimbal2ideal * R_camera2gimbal_eigen; - Eigen::Vector3d ypr = tools::eulers(R_camera2ideal, 1, 0, 2) * 57.3; // degree + Eigen::Vector3d ypr = component::eulers(R_camera2ideal, 1, 0, 2) * 57.3; // degree // 输出yaml print_yaml(R_gimbal2imubody_data, R_camera2gimbal, t_camera2gimbal, ypr); diff --git a/calibration/calibrate_robotworld_handeye.cpp b/calibration/calibrate_robotworld_handeye.cpp index 86d1b48..7c60e4c 100644 --- a/calibration/calibrate_robotworld_handeye.cpp +++ b/calibration/calibrate_robotworld_handeye.cpp @@ -70,13 +70,13 @@ void load( Eigen::Matrix3d R_imubody2imuabs = q.toRotationMatrix(); Eigen::Matrix3d R_gimbal2world = R_gimbal2imubody.transpose() * R_imubody2imuabs * R_gimbal2imubody; - Eigen::Vector3d ypr = tools::eulers(R_gimbal2world, 2, 1, 0) * 57.3; // degree + Eigen::Vector3d ypr = component::eulers(R_gimbal2world, 2, 1, 0) * 57.3; // degree // 在图片上显示云台的欧拉角，用来检验R_gimbal2imubody是否正确 auto drawing = img.clone(); - tools::draw_text(drawing, fmt::format("yaw {:.2f}", ypr[0]), {40, 40}, {0, 0, 255}); - tools::draw_text(drawing, fmt::format("pitch {:.2f}", ypr[1]), {40, 80}, {0, 0, 255}); - tools::draw_text(drawing, fmt::format("roll {:.2f}", ypr[2]), {40, 120}, {0, 0, 255}); + component::draw_text(drawing, fmt::format("yaw {:.2f}", ypr[0]), {40, 40}, {0, 0, 255}); + component::draw_text(drawing, fmt::format("pitch {:.2f}", ypr[1]), {40, 80}, {0, 0, 255}); + component::draw_text(drawing, fmt::format("roll {:.2f}", ypr[2]), {40, 120}, {0, 0, 255}); // 识别标定板 std::vector centers_2d; @@ -186,7 +186,7 @@ int main(int argc, char * argv[]) cv::cv2eigen(R_camera2gimbal, R_camera2gimbal_eigen); Eigen::Matrix3d R_gimbal2ideal{{0, -1, 0}, {0, 0, -1}, {1, 0, 0}}; Eigen::Matrix3d R_camera2ideal = R_gimbal2ideal * R_camera2gimbal_eigen; - Eigen::Vector3d camera_ypr = tools::eulers(R_camera2ideal, 1, 0, 2) * 57.3; // degree + Eigen::Vector3d camera_ypr = component::eulers(R_camera2ideal, 1, 0, 2) * 57.3; // degree // 计算标定板到世界坐标系原点的水平距离 auto x = t_board2world.at(0); @@ -196,7 +196,7 @@ int main(int argc, char * argv[]) // 计算标定板同竖直摆放时的偏角 Eigen::Matrix3d R_board2world_eigen; cv::cv2eigen(R_board2world, R_board2world_eigen); - Eigen::Vector3d board_ypr = tools::eulers(R_board2world_eigen, 2, 1, 0) * 57.3; // degree + Eigen::Vector3d board_ypr = component::eulers(R_board2world_eigen, 2, 1, 0) * 57.3; // degree // 输出yaml print_yaml( diff --git a/calibration/capture.cpp b/calibration/capture.cpp index a07174f..8f02b05 100644 --- a/calibration/capture.cpp +++ b/calibration/capture.cpp @@ -27,8 +27,8 @@ void write_q(const std::string q_path, const Eigen::Quaterniond & q) void capture_loop( const std::string & config_path, const std::string & can, const std::string & output_folder) { - io::CBoard cboard(config_path); - io::Camera camera(config_path); + device::CBoard cboard(config_path); + device::Camera camera(config_path); cv::Mat img; std::chrono::steady_clock::time_point timestamp; @@ -39,10 +39,10 @@ void capture_loop( // 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂 auto img_with_ypr = img.clone(); - Eigen::Vector3d zyx = tools::eulers(q, 2, 1, 0) * 57.3; // degree - tools::draw_text(img_with_ypr, fmt::format("Z {:.2f}", zyx[0]), {40, 40}, {0, 0, 255}); - tools::draw_text(img_with_ypr, fmt::format("Y {:.2f}", zyx[1]), {40, 80}, {0, 0, 255}); - tools::draw_text(img_with_ypr, fmt::format("X {:.2f}", zyx[2]), {40, 120}, {0, 0, 255}); + Eigen::Vector3d zyx = component::eulers(q, 2, 1, 0) * 57.3; // degree + component::draw_text(img_with_ypr, fmt::format("Z {:.2f}", zyx[0]), {40, 40}, {0, 0, 255}); + component::draw_text(img_with_ypr, fmt::format("Y {:.2f}", zyx[1]), {40, 80}, {0, 0, 255}); + component::draw_text(img_with_ypr, fmt::format("X {:.2f}", zyx[2]), {40, 120}, {0, 0, 255}); std::vector centers_2d; auto success = cv::findCirclesGrid(img, cv::Size(10, 7), centers_2d); // 默认是对称圆点图案 @@ -63,7 +63,7 @@ void capture_loop( auto q_path = fmt::format("{}/{}.txt", output_folder, count); cv::imwrite(img_path, img); write_q(q_path, q); - tools::logger()->info("[{}] Saved in {}", count, output_folder); + component::logger()->info("[{}] Saved in {}", count, output_folder); } // 离开该作用域时，camera和cboard会自动关闭 @@ -83,11 +83,11 @@ int main(int argc, char * argv[]) // 新建输出文件夹 std::filesystem::create_directory(output_folder); - tools::logger()->info("默认标定板尺寸为10列7行"); + component::logger()->info("默认标定板尺寸为10列7行"); // 主循环，保存图片和对应四元数 capture_loop(config_path, "can0", output_folder); - tools::logger()->warn("注意四元数输出顺序为wxyz"); + component::logger()->warn("注意四元数输出顺序为wxyz"); return 0; } diff --git a/calibration/split_video.cpp b/calibration/split_video.cpp index 76d769e..4fd96eb 100644 --- a/calibration/split_video.cpp +++ b/calibration/split_video.cpp @@ -34,7 +34,7 @@ int main(int argc, char * argv[]) auto end_index = cli.get("end-index"); // 初始化绘图器和退出器 - tools::Exiter exiter; + component::Exiter exiter; // 构造视频和文本文件路径 auto video_path = fmt::format("{}.avi", input_path); diff --git a/src/component/crc.cpp b/src/component/crc.cpp index bbea7ef..09e2932 100644 --- a/src/component/crc.cpp +++ b/src/component/crc.cpp @@ -45,7 +45,7 @@ const uint16_t CRC16_TABLE[256] = { 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78}; -namespace tools +namespace component { uint8_t get_crc8(const uint8_t * data, uint16_t len) { @@ -88,4 +88,4 @@ bool check_crc16(const uint8_t * data, uint32_t len) return get_crc16(data, len - 2) == crc16; } -} // namespace tools \ No newline at end of file +} // namespace component \ No newline at end of file diff --git a/src/component/crc.hpp b/src/component/crc.hpp index 2d71cf8..b81ce39 100644 --- a/src/component/crc.hpp +++ b/src/component/crc.hpp @@ -1,9 +1,9 @@ -#ifndef TOOLS__CRC_HPP -#define TOOLS__CRC_HPP +#ifndef COMPONENT__CRC_HPP +#define COMPONENT__CRC_HPP #include -namespace tools +namespace component { // len不包括crc8 uint8_t get_crc8(const uint8_t * data, uint16_t len); @@ -17,6 +17,6 @@ uint16_t get_crc16(const uint8_t * data, uint32_t len); // len包括crc16 bool check_crc16(const uint8_t * data, uint32_t len); -} // namespace tools +} // namespace component -#endif // TOOLS__CRC_HPP +#endif // COMPONENT__CRC_HPP diff --git a/src/component/exiter.cpp b/src/component/exiter.cpp index a4d1d5d..9c49314 100644 --- a/src/component/exiter.cpp +++ b/src/component/exiter.cpp @@ -3,7 +3,7 @@ #include #include -namespace tools +namespace component { bool exit_ = false; bool exiter_inited_ = false; @@ -17,4 +17,4 @@ Exiter::Exiter() bool Exiter::exit() const { return exit_; } -} // namespace tools \ No newline at end of file +} // namespace component \ No newline at end of file diff --git a/src/component/exiter.hpp b/src/component/exiter.hpp index 1b0de77..47a0912 100644 --- a/src/component/exiter.hpp +++ b/src/component/exiter.hpp @@ -1,7 +1,7 @@ -#ifndef TOOLS__EXITER_HPP -#define TOOLS__EXITER_HPP +#ifndef COMPONENT__EXITER_HPP +#define COMPONENT__EXITER_HPP -namespace tools +namespace component { class Exiter { @@ -11,6 +11,6 @@ public: bool exit() const; }; -} // namespace tools +} // namespace component -#endif // TOOLS__EXITER_HPP \ No newline at end of file +#endif // COMPONENT__EXITER_HPP \ No newline at end of file diff --git a/src/component/extended_kalman_filter.cpp b/src/component/extended_kalman_filter.cpp index b73f40f..9e8d0ba 100644 --- a/src/component/extended_kalman_filter.cpp +++ b/src/component/extended_kalman_filter.cpp @@ -2,7 +2,7 @@ #include -namespace tools +namespace component { ExtendedKalmanFilter::ExtendedKalmanFilter( const Eigen::VectorXd & x0, const Eigen::MatrixXd & P0, @@ -91,4 +91,4 @@ Eigen::VectorXd ExtendedKalmanFilter::update( return x; } -} // namespace tools \ No newline at end of file +} // namespace component \ No newline at end of file diff --git a/src/component/extended_kalman_filter.hpp b/src/component/extended_kalman_filter.hpp index 2fed923..996e08b 100644 --- a/src/component/extended_kalman_filter.hpp +++ b/src/component/extended_kalman_filter.hpp @@ -1,12 +1,12 @@ -#ifndef TOOLS__EXTENDED_KALMAN_FILTER_HPP -#define TOOLS__EXTENDED_KALMAN_FILTER_HPP +#ifndef COMPONENT__EXTENDED_KALMAN_FILTER_HPP +#define COMPONENT__EXTENDED_KALMAN_FILTER_HPP #include #include #include #include -namespace tools +namespace component { class ExtendedKalmanFilter { @@ -52,6 +52,6 @@ private: int total_count_ = 0; }; -} // namespace tools +} // namespace component -#endif // TOOLS__EXTENDED_KALMAN_FILTER_HPP \ No newline at end of file +#endif // COMPONENT__EXTENDED_KALMAN_FILTER_HPP \ No newline at end of file diff --git a/src/component/img_tools.cpp b/src/component/img_tools.cpp index 81545f9..e106af7 100644 --- a/src/component/img_tools.cpp +++ b/src/component/img_tools.cpp @@ -1,6 +1,6 @@ #include "img_tools.hpp" -namespace tools +namespace component { void draw_point(cv::Mat & img, const cv::Point & point, const cv::Scalar & color, int radius) { @@ -28,4 +28,4 @@ void draw_text( cv::putText(img, text, point, cv::FONT_HERSHEY_SIMPLEX, font_scale, color, thickness); } -} // namespace tools \ No newline at end of file +} // namespace component \ No newline at end of file diff --git a/src/component/img_tools.hpp b/src/component/img_tools.hpp index 13a3e8d..40b038e 100644 --- a/src/component/img_tools.hpp +++ b/src/component/img_tools.hpp @@ -1,11 +1,11 @@ -#ifndef TOOLS__IMG_TOOLS_HPP -#define TOOLS__IMG_TOOLS_HPP +#ifndef COMPONENT__IMG_TOOLS_HPP +#define COMPONENT__IMG_TOOLS_HPP #include #include #include -namespace tools +namespace component { void draw_point( cv::Mat & img, const cv::Point & point, const cv::Scalar & color = {0, 0, 255}, int radius = 3); @@ -22,6 +22,6 @@ void draw_text( cv::Mat & img, const std::string & text, const cv::Point & point, const cv::Scalar & color = {0, 255, 255}, double font_scale = 1.0, int thickness = 2); -} // namespace tools +} // namespace component -#endif // TOOLS__IMG_TOOLS_HPP \ No newline at end of file +#endif // COMPONENT__IMG_TOOLS_HPP \ No newline at end of file diff --git a/src/component/logger.cpp b/src/component/logger.cpp index 2cb9c79..688d773 100644 --- a/src/component/logger.cpp +++ b/src/component/logger.cpp @@ -8,7 +8,7 @@ #include #include -namespace tools +namespace component { std::shared_ptr logger_ = nullptr; @@ -32,4 +32,4 @@ std::shared_ptr logger() return logger_; } -} // namespace tools +} // namespace component diff --git a/src/component/logger.hpp b/src/component/logger.hpp index 5c87aee..674a490 100644 --- a/src/component/logger.hpp +++ b/src/component/logger.hpp @@ -1,12 +1,12 @@ -#ifndef TOOLS__LOGGER_HPP -#define TOOLS__LOGGER_HPP +#ifndef COMPONENT__LOGGER_HPP +#define COMPONENT__LOGGER_HPP #include -namespace tools +namespace component { std::shared_ptr logger(); -} // namespace tools +} // namespace component -#endif // TOOLS__LOGGER_HPP \ No newline at end of file +#endif // COMPONENT__LOGGER_HPP \ No newline at end of file diff --git a/src/component/math_tools.cpp b/src/component/math_tools.cpp index 992c0aa..b350fe1 100644 --- a/src/component/math_tools.cpp +++ b/src/component/math_tools.cpp @@ -3,7 +3,7 @@ #include #include // CV_PI -namespace tools +namespace component { double limit_rad(double angle) { @@ -199,4 +199,4 @@ double limit_min_max(double input, double min, double max) return min; return input; } -} // namespace tools \ No newline at end of file +} // namespace component \ No newline at end of file diff --git a/src/component/math_tools.hpp b/src/component/math_tools.hpp index ffb9b9f..ef259ca 100644 --- a/src/component/math_tools.hpp +++ b/src/component/math_tools.hpp @@ -1,10 +1,10 @@ -#ifndef TOOLS__MATH_TOOLS_HPP -#define TOOLS__MATH_TOOLS_HPP +#ifndef COMPONENT__MATH_TOOLS_HPP +#define COMPONENT__MATH_TOOLS_HPP #include #include -namespace tools +namespace component { // 将弧度值限制在(-pi, pi] double limit_rad(double angle); @@ -53,6 +53,6 @@ T square(T const & a) }; double limit_min_max(double input, double min, double max); -} // namespace tools +} // namespace component -#endif // TOOLS__MATH_TOOLS_HPP \ No newline at end of file +#endif // COMPONENT__MATH_TOOLS_HPP \ No newline at end of file diff --git a/src/component/pid.cpp b/src/component/pid.cpp index 7ad4ddf..bcbc038 100644 --- a/src/component/pid.cpp +++ b/src/component/pid.cpp @@ -4,7 +4,7 @@ float clip(float value, float min, float max) { return std::max(min, std::min(max, value)); } -namespace tools +namespace component { PID::PID(float dt, float kp, float ki, float kd, float max_out, float max_iout, bool angular) : dt_(dt), kp_(kp), ki_(ki), kd_(kd), max_out_(max_out), max_iout_(max_iout), angular_(angular) @@ -24,4 +24,4 @@ float PID::calc(float set, float fdb) return clip(this->pout + this->iout + this->dout, -max_out_, max_out_); } -} // namespace tools +} // namespace component diff --git a/src/component/pid.hpp b/src/component/pid.hpp index a534e52..d64af7b 100644 --- a/src/component/pid.hpp +++ b/src/component/pid.hpp @@ -1,7 +1,7 @@ -#ifndef TOOLS__PID_HPP -#define TOOLS__PID_HPP +#ifndef COMPONENT__PID_HPP +#define COMPONENT__PID_HPP -namespace tools +namespace component { class PID { @@ -32,6 +32,6 @@ private: float last_fdb_ = 0.0f; // 上次反馈值 }; -} // namespace tools +} // namespace component -#endif // TOOLS__PID_HPP \ No newline at end of file +#endif // COMPONENT__PID_HPP \ No newline at end of file diff --git a/src/component/plotter.cpp b/src/component/plotter.cpp index ce85468..6f486e2 100644 --- a/src/component/plotter.cpp +++ b/src/component/plotter.cpp @@ -4,7 +4,7 @@ #include // socket, sendto #include // close -namespace tools +namespace component { Plotter::Plotter(std::string host, uint16_t port) { @@ -26,4 +26,4 @@ void Plotter::plot(const nlohmann::json & json) sizeof(destination_)); } -} // namespace tools \ No newline at end of file +} // namespace component \ No newline at end of file diff --git a/src/component/plotter.hpp b/src/component/plotter.hpp index 9e3ca50..3194391 100644 --- a/src/component/plotter.hpp +++ b/src/component/plotter.hpp @@ -1,5 +1,5 @@ -#ifndef TOOLS__PLOTTER_HPP -#define TOOLS__PLOTTER_HPP +#ifndef COMPONENT__PLOTTER_HPP +#define COMPONENT__PLOTTER_HPP #include // sockaddr_in @@ -7,7 +7,7 @@ #include #include -namespace tools +namespace component { class Plotter { @@ -24,6 +24,6 @@ private: std::mutex mutex_; }; -} // namespace tools +} // namespace component -#endif // TOOLS__PLOTTER_HPP \ No newline at end of file +#endif // COMPONENT__PLOTTER_HPP \ No newline at end of file diff --git a/src/component/ransac_sine_fitter.cpp b/src/component/ransac_sine_fitter.cpp index 00f1cb7..b18ce5a 100644 --- a/src/component/ransac_sine_fitter.cpp +++ b/src/component/ransac_sine_fitter.cpp @@ -5,7 +5,7 @@ #include #include -namespace tools +namespace component { RansacSineFitter::RansacSineFitter( @@ -102,4 +102,4 @@ int RansacSineFitter::evaluate_inliers(double A, double omega, double phi, doubl return count; } -} // namespace tools +} // namespace component diff --git a/src/component/ransac_sine_fitter.hpp b/src/component/ransac_sine_fitter.hpp index c7ced21..067d139 100644 --- a/src/component/ransac_sine_fitter.hpp +++ b/src/component/ransac_sine_fitter.hpp @@ -6,7 +6,7 @@ #include #include -namespace tools +namespace component { class RansacSineFitter @@ -47,4 +47,4 @@ private: int evaluate_inliers(double A, double omega, double phi, double C); }; -} // namespace tools +} // namespace component diff --git a/src/component/recorder.cpp b/src/component/recorder.cpp index d4076a1..3bdc2d8 100644 --- a/src/component/recorder.cpp +++ b/src/component/recorder.cpp @@ -8,7 +8,7 @@ #include "math_tools.hpp" #include "src/component/logger.hpp" -namespace tools +namespace component { Recorder::Recorder(double fps) : init_(false), fps_(fps), queue_(1), stop_thread_(false) { @@ -41,7 +41,7 @@ void Recorder::save_to_file() FrameData frame; queue_.pop(frame); // 从队列中取出帧数据 if (frame.img.empty()) { - tools::logger()->debug("Recorder received empty img. Skip this frame."); + component::logger()->debug("Recorder received empty img. Skip this frame."); continue; } // 写入视频文件 @@ -49,7 +49,7 @@ void Recorder::save_to_file() // 写入文本文件（输出顺序为wxyz） Eigen::Vector4d xyzw = frame.q.coeffs(); - auto since_begin = tools::delta_time(frame.timestamp, start_time_); + auto since_begin = component::delta_time(frame.timestamp, start_time_); text_writer_ << fmt::format( "{} {} {} {} {}\n", since_begin, xyzw[3], xyzw[0], xyzw[1], xyzw[2]); } @@ -62,7 +62,7 @@ void Recorder::record( if (img.empty()) return; if (!init_) init(img); - auto since_last = tools::delta_time(timestamp, last_time_); + auto since_last = component::delta_time(timestamp, last_time_); if (since_last < 1.0 / fps_) return; last_time_ = timestamp; @@ -78,4 +78,4 @@ void Recorder::init(const cv::Mat & img) init_ = true; } -} // namespace tools +} // namespace component diff --git a/src/component/recorder.hpp b/src/component/recorder.hpp index f0374d1..ea42d4a 100644 --- a/src/component/recorder.hpp +++ b/src/component/recorder.hpp @@ -1,5 +1,5 @@ -#ifndef TOOLS__RECORDER_HPP -#define TOOLS__RECORDER_HPP +#ifndef COMPONENT__RECORDER_HPP +#define COMPONENT__RECORDER_HPP #include #include @@ -8,7 +8,7 @@ #include #include "src/component/thread_safe_queue.hpp" -namespace tools +namespace component { class Recorder { @@ -35,12 +35,12 @@ private: cv::VideoWriter video_writer_; std::chrono::steady_clock::time_point start_time_; std::chrono::steady_clock::time_point last_time_; - tools::ThreadSafeQueue queue_; + component::ThreadSafeQueue queue_; std::thread saving_thread_; // 负责保存帧数据的线程 void init(const cv::Mat & img); void save_to_file(); }; -} // namespace tools +} // namespace component -#endif // TOOLS__RECORDER_HPP \ No newline at end of file +#endif // COMPONENT__RECORDER_HPP \ No newline at end of file diff --git a/src/component/thread_pool.hpp b/src/component/thread_pool.hpp index e04db61..da76d92 100644 --- a/src/component/thread_pool.hpp +++ b/src/component/thread_pool.hpp @@ -1,5 +1,5 @@ -#ifndef TOOLS__THREAD_POOL_HPP -#define TOOLS__THREAD_POOL_HPP +#ifndef COMPONENT__THREAD_POOL_HPP +#define COMPONENT__THREAD_POOL_HPP #include #include @@ -11,7 +11,7 @@ #include "src/module/auto_aim/yolo.hpp" #include "src/component/logger.hpp" -namespace tools +namespace component { struct Frame { @@ -51,19 +51,19 @@ public: { std::lock_guard lock(mutex_); - main_queue_ = std::queue(); + main_queue_ = std::queue(); buffer_.clear(); current_id_ = 0; } - tools::logger()->info("OrderedQueue destroyed, queue and buffer cleared."); + component::logger()->info("OrderedQueue destroyed, queue and buffer cleared."); } - void enqueue(const tools::Frame & item) + void enqueue(const component::Frame & item) { std::lock_guard lock(mutex_); if (item.id < current_id_) { - tools::logger()->warn("small id"); + component::logger()->warn("small id"); return; } @@ -87,19 +87,19 @@ public: } } - tools::Frame dequeue() + component::Frame dequeue() { std::unique_lock lock(mutex_); cond_var_.wait(lock, [this]() { return !main_queue_.empty(); }); - tools::Frame item = main_queue_.front(); + component::Frame item = main_queue_.front(); main_queue_.pop(); return item; } // 不会阻塞队列 - bool try_dequeue(tools::Frame & item) + bool try_dequeue(component::Frame & item) { std::lock_guard lock(mutex_); if (main_queue_.empty()) { @@ -113,8 +113,8 @@ public: size_t get_size() { return main_queue_.size() + buffer_.size(); } private: - std::queue main_queue_; - std::unordered_map buffer_; + std::queue main_queue_; + std::unordered_map buffer_; int current_id_; std::mutex mutex_; std::condition_variable cond_var_; @@ -180,6 +180,6 @@ private: std::condition_variable condition; // 条件变量，用于等待任务 bool stop; // 是否停止线程池 }; -} // namespace tools +} // namespace component -#endif // TOOLS__THREAD_POOL_HPP +#endif // COMPONENT__THREAD_POOL_HPP diff --git a/src/component/thread_safe_queue.hpp b/src/component/thread_safe_queue.hpp index e9bb50b..ff23253 100644 --- a/src/component/thread_safe_queue.hpp +++ b/src/component/thread_safe_queue.hpp @@ -1,5 +1,5 @@ -#ifndef TOOLS__THREAD_SAFE_QUEUE_HPP -#define TOOLS__THREAD_SAFE_QUEUE_HPP +#ifndef COMPONENT__THREAD_SAFE_QUEUE_HPP +#define COMPONENT__THREAD_SAFE_QUEUE_HPP #include #include @@ -7,7 +7,7 @@ #include #include -namespace tools +namespace component { template class ThreadSafeQueue @@ -106,6 +106,6 @@ private: std::function full_handler_; }; -} // namespace tools +} // namespace component -#endif // TOOLS__THREAD_SAFE_QUEUE_HPP \ No newline at end of file +#endif // COMPONENT__THREAD_SAFE_QUEUE_HPP \ No newline at end of file diff --git a/src/component/trajectory.cpp b/src/component/trajectory.cpp index 2e28956..05e6c1a 100644 --- a/src/component/trajectory.cpp +++ b/src/component/trajectory.cpp @@ -2,7 +2,7 @@ #include -namespace tools +namespace component { constexpr double g = 9.7833; @@ -30,4 +30,4 @@ Trajectory::Trajectory(const double v0, const double d, const double h) fly_time = (t_1 < t_2) ? t_1 : t_2; } -} // namespace tools \ No newline at end of file +} // namespace component \ No newline at end of file diff --git a/src/component/trajectory.hpp b/src/component/trajectory.hpp index ed817cc..8330ef9 100644 --- a/src/component/trajectory.hpp +++ b/src/component/trajectory.hpp @@ -1,7 +1,7 @@ -#ifndef TOOLS__TRAJECTORY_HPP -#define TOOLS__TRAJECTORY_HPP +#ifndef COMPONENT__TRAJECTORY_HPP +#define COMPONENT__TRAJECTORY_HPP -namespace tools +namespace component { struct Trajectory { @@ -16,6 +16,6 @@ struct Trajectory Trajectory(const double v0, const double d, const double h); }; -} // namespace tools +} // namespace component -#endif // TOOLS__TRAJECTORY_HPP \ No newline at end of file +#endif // COMPONENT__TRAJECTORY_HPP \ No newline at end of file diff --git a/src/component/yaml.hpp b/src/component/yaml.hpp index 2a5c4bc..5de73da 100644 --- a/src/component/yaml.hpp +++ b/src/component/yaml.hpp @@ -1,11 +1,11 @@ -#ifndef TOOLS__YAML_HPP -#define TOOLS__YAML_HPP +#ifndef COMPONENT__YAML_HPP +#define COMPONENT__YAML_HPP #include #include "src/component/logger.hpp" -namespace tools +namespace component { inline YAML::Node load(const std::string & path) { @@ -28,6 +28,6 @@ inline T read(const YAML::Node & yaml, const std::string & key) exit(1); } -} // namespace tools +} // namespace component -#endif // TOOLS__YAML_HPP \ No newline at end of file +#endif // COMPONENT__YAML_HPP \ No newline at end of file diff --git a/src/device/camera.cpp b/src/device/camera.cpp index a843ecf..71de154 100644 --- a/src/device/camera.cpp +++ b/src/device/camera.cpp @@ -6,23 +6,23 @@ #include "mindvision/mindvision.hpp" #include "src/component/yaml.hpp" -namespace io +namespace device { Camera::Camera(const std::string & config_path) { - auto yaml = tools::load(config_path); - auto camera_name = tools::read(yaml, "camera_name"); - auto exposure_ms = tools::read(yaml, "exposure_ms"); + auto yaml = component::load(config_path); + auto camera_name = component::read(yaml, "camera_name"); + auto exposure_ms = component::read(yaml, "exposure_ms"); if (camera_name == "mindvision") { - auto gamma = tools::read(yaml, "gamma"); - auto vid_pid = tools::read(yaml, "vid_pid"); + auto gamma = component::read(yaml, "gamma"); + auto vid_pid = component::read(yaml, "vid_pid"); camera_ = std::make_unique(exposure_ms, gamma, vid_pid); } else if (camera_name == "hikrobot") { - auto gain = tools::read(yaml, "gain"); - auto vid_pid = tools::read(yaml, "vid_pid"); + auto gain = component::read(yaml, "gain"); + auto vid_pid = component::read(yaml, "vid_pid"); camera_ = std::make_unique(exposure_ms, gain, vid_pid); } @@ -36,4 +36,4 @@ void Camera::read(cv::Mat & img, std::chrono::steady_clock::time_point & timesta camera_->read(img, timestamp); } -} // namespace io \ No newline at end of file +} // namespace device \ No newline at end of file diff --git a/src/device/camera.hpp b/src/device/camera.hpp index b509b9a..e0bb37b 100644 --- a/src/device/camera.hpp +++ b/src/device/camera.hpp @@ -1,12 +1,12 @@ -#ifndef IO__CAMERA_HPP -#define IO__CAMERA_HPP +#ifndef DEVICE__CAMERA_HPP +#define DEVICE__CAMERA_HPP #include #include #include #include -namespace io +namespace device { class CameraBase { @@ -25,6 +25,6 @@ private: std::unique_ptr camera_; }; -} // namespace io +} // namespace device -#endif // IO__CAMERA_HPP \ No newline at end of file +#endif // DEVICE__CAMERA_HPP \ No newline at end of file diff --git a/src/device/cboard.cpp b/src/device/cboard.cpp index 895a437..6d18a63 100644 --- a/src/device/cboard.cpp +++ b/src/device/cboard.cpp @@ -3,7 +3,7 @@ #include "src/component/math_tools.hpp" #include "src/component/yaml.hpp" -namespace io +namespace device { CBoard::CBoard(const std::string & config_path) : mode(Mode::idle), @@ -13,10 +13,10 @@ CBoard::CBoard(const std::string & config_path) can_(read_yaml(config_path), std::bind(&CBoard::callback, this, std::placeholders::_1)) // 注意: callback的运行会早于Cboard构造函数的完成 { - tools::logger()->info("[Cboard] Waiting for q..."); + component::logger()->info("[Cboard] Waiting for q..."); queue_.pop(data_ahead_); queue_.pop(data_behind_); - tools::logger()->info("[Cboard] Opened."); + component::logger()->info("[Cboard] Opened."); } Eigen::Quaterniond CBoard::imu_at(std::chrono::steady_clock::time_point timestamp) @@ -61,7 +61,7 @@ void CBoard::send(Command command) const try { can_.write(&frame); } catch (const std::exception & e) { - tools::logger()->warn("{}", e.what()); + component::logger()->warn("{}", e.what()); } } @@ -76,7 +76,7 @@ void CBoard::callback(const can_frame & frame) auto w = (int16_t)(frame.data[6] << 8 | frame.data[7]) / 1e4; if (std::abs(x * x + y * y + z * z + w * w - 1) > 1e-2) { - tools::logger()->warn("Invalid q: {} {} {} {}", w, x, y, z); + component::logger()->warn("Invalid q: {} {} {} {}", w, x, y, z); return; } @@ -93,8 +93,8 @@ void CBoard::callback(const can_frame & frame) static auto last_log_time = std::chrono::steady_clock::time_point::min(); auto now = std::chrono::steady_clock::now(); - if (bullet_speed > 0 && tools::delta_time(now, last_log_time) >= 1.0) { - tools::logger()->info( + if (bullet_speed > 0 && component::delta_time(now, last_log_time) >= 1.0) { + component::logger()->info( "[CBoard] Bullet speed: {:.2f} m/s, Mode: {}, Shoot mode: {}, FT angle: {:.2f} rad", bullet_speed, MODES[mode], SHOOT_MODES[shoot_mode], ft_angle); last_log_time = now; @@ -105,11 +105,11 @@ void CBoard::callback(const can_frame & frame) // 实现方式有待改进 std::string CBoard::read_yaml(const std::string & config_path) { - auto yaml = tools::load(config_path); + auto yaml = component::load(config_path); - quaternion_canid_ = tools::read(yaml, "quaternion_canid"); - bullet_speed_canid_ = tools::read(yaml, "bullet_speed_canid"); - send_canid_ = tools::read(yaml, "send_canid"); + quaternion_canid_ = component::read(yaml, "quaternion_canid"); + bullet_speed_canid_ = component::read(yaml, "bullet_speed_canid"); + send_canid_ = component::read(yaml, "send_canid"); if (!yaml["can_interface"]) { throw std::runtime_error("Missing 'can_interface' in YAML configuration."); @@ -118,4 +118,4 @@ std::string CBoard::read_yaml(const std::string & config_path) return yaml["can_interface"].as(); } -} // namespace io \ No newline at end of file +} // namespace device \ No newline at end of file diff --git a/src/device/cboard.hpp b/src/device/cboard.hpp index 88a2000..60f5164 100644 --- a/src/device/cboard.hpp +++ b/src/device/cboard.hpp @@ -1,5 +1,5 @@ -#ifndef IO__CBOARD_HPP -#define IO__CBOARD_HPP +#ifndef DEVICE__CBOARD_HPP +#define DEVICE__CBOARD_HPP #include #include @@ -13,7 +13,7 @@ #include "src/component/logger.hpp" #include "src/component/thread_safe_queue.hpp" -namespace io +namespace device { enum Mode { @@ -55,7 +55,7 @@ private: std::chrono::steady_clock::time_point timestamp; }; - tools::ThreadSafeQueue queue_; // 必须在can_之前初始化，否则存在死锁的可能 + component::ThreadSafeQueue queue_; // 必须在can_之前初始化，否则存在死锁的可能 SocketCAN can_; IMUData data_ahead_; IMUData data_behind_; @@ -67,6 +67,6 @@ private: std::string read_yaml(const std::string & config_path); }; -} // namespace io +} // namespace device -#endif // IO__CBOARD_HPP \ No newline at end of file +#endif // DEVICE__CBOARD_HPP \ No newline at end of file diff --git a/src/device/command.hpp b/src/device/command.hpp index 797ba29..d336e02 100644 --- a/src/device/command.hpp +++ b/src/device/command.hpp @@ -1,7 +1,7 @@ -#ifndef IO__COMMAND_HPP -#define IO__COMMAND_HPP +#ifndef DEVICE__COMMAND_HPP +#define DEVICE__COMMAND_HPP -namespace io +namespace device { struct Command { @@ -12,6 +12,6 @@ struct Command double horizon_distance = 0; //无人机专有 }; -} // namespace io +} // namespace device -#endif // IO__COMMAND_HPP \ No newline at end of file +#endif // DEVICE__COMMAND_HPP \ No newline at end of file diff --git a/src/device/dm_imu/dm_imu.cpp b/src/device/dm_imu/dm_imu.cpp index 979bb15..40c7c7d 100644 --- a/src/device/dm_imu/dm_imu.cpp +++ b/src/device/dm_imu/dm_imu.cpp @@ -12,7 +12,7 @@ #include "src/component/logger.hpp" #include "src/component/math_tools.hpp" -namespace io +namespace device { DM_IMU::DM_IMU() : queue_(5000) { @@ -20,7 +20,7 @@ DM_IMU::DM_IMU() : queue_(5000) rec_thread_ = std::thread(&DM_IMU::get_imu_data_thread, this); queue_.pop(data_ahead_); queue_.pop(data_behind_); - tools::logger()->info("[DM_IMU] initialized"); + component::logger()->info("[DM_IMU] initialized"); } DM_IMU::~DM_IMU() @@ -48,11 +48,11 @@ void DM_IMU::init_serial() serial_.open(); usleep(1000000); //1s - tools::logger()->info("[DM_IMU] serial port opened"); + component::logger()->info("[DM_IMU] serial port opened"); } catch (serial::IOException & e) { - tools::logger()->warn("[DM_IMU] failed to open serial port "); + component::logger()->warn("[DM_IMU] failed to open serial port "); exit(0); } } @@ -61,7 +61,7 @@ void DM_IMU::get_imu_data_thread() { while (!stop_thread_) { if (!serial_.isOpen()) { - tools::logger()->warn("In get_imu_data_thread,imu serial port unopen"); + component::logger()->warn("In get_imu_data_thread,imu serial port unopen"); } serial_.read((uint8_t *)(&receive_data.FrameHeader1), 4); @@ -73,21 +73,21 @@ void DM_IMU::get_imu_data_thread() { serial_.read((uint8_t *)(&receive_data.accx_u32), 57 - 4); - if (tools::get_crc16((uint8_t *)(&receive_data.FrameHeader1), 16) == receive_data.crc1) { + if (component::get_crc16((uint8_t *)(&receive_data.FrameHeader1), 16) == receive_data.crc1) { data.accx = *((float *)(&receive_data.accx_u32)); data.accy = *((float *)(&receive_data.accy_u32)); data.accz = *((float *)(&receive_data.accz_u32)); } - if (tools::get_crc16((uint8_t *)(&receive_data.FrameHeader2), 16) == receive_data.crc2) { + if (component::get_crc16((uint8_t *)(&receive_data.FrameHeader2), 16) == receive_data.crc2) { data.gyrox = *((float *)(&receive_data.gyrox_u32)); data.gyroy = *((float *)(&receive_data.gyroy_u32)); data.gyroz = *((float *)(&receive_data.gyroz_u32)); } - if (tools::get_crc16((uint8_t *)(&receive_data.FrameHeader3), 16) == receive_data.crc3) { + if (component::get_crc16((uint8_t *)(&receive_data.FrameHeader3), 16) == receive_data.crc3) { data.roll = *((float *)(&receive_data.roll_u32)); data.pitch = *((float *)(&receive_data.pitch_u32)); data.yaw = *((float *)(&receive_data.yaw_u32)); - // tools::logger()->debug( + // component::logger()->debug( // "yaw: {:.2f}, pitch: {:.2f}, roll: {:.2f}", static_cast(data.yaw), // static_cast(data.pitch), static_cast(data.roll)); } @@ -98,7 +98,7 @@ void DM_IMU::get_imu_data_thread() q.normalize(); queue_.push({q, timestamp}); } else { - tools::logger()->info("[DM_IMU] failed to get correct data"); + component::logger()->info("[DM_IMU] failed to get correct data"); } } } @@ -128,4 +128,4 @@ Eigen::Quaterniond DM_IMU::imu_at(std::chrono::steady_clock::time_point timestam return q_c; } -} // namespace io +} // namespace device diff --git a/src/device/dm_imu/dm_imu.hpp b/src/device/dm_imu/dm_imu.hpp index 9732087..80f8f2f 100644 --- a/src/device/dm_imu/dm_imu.hpp +++ b/src/device/dm_imu/dm_imu.hpp @@ -1,5 +1,5 @@ -#ifndef IO__Dm_Imu_HPP -#define IO__Dm_Imu_HPP +#ifndef DEVICE__Dm_Imu_HPP +#define DEVICE__Dm_Imu_HPP #include #include @@ -13,7 +13,7 @@ #include "src/component/thread_safe_queue.hpp" -namespace io +namespace device { struct __attribute__((packed)) IMU_Receive_Frame @@ -83,7 +83,7 @@ private: serial::Serial serial_; std::thread rec_thread_; - tools::ThreadSafeQueue queue_; + component::ThreadSafeQueue queue_; IMUData data_ahead_, data_behind_; std::atomic stop_thread_{false}; @@ -91,6 +91,6 @@ private: IMU_Data data{}; }; -} // namespace io +} // namespace device #endif diff --git a/src/device/gimbal/gimbal.cpp b/src/device/gimbal/gimbal.cpp index 16f1cb5..972e49e 100644 --- a/src/device/gimbal/gimbal.cpp +++ b/src/device/gimbal/gimbal.cpp @@ -5,25 +5,25 @@ #include "src/component/math_tools.hpp" #include "src/component/yaml.hpp" -namespace io +namespace device { Gimbal::Gimbal(const std::string & config_path) { - auto yaml = tools::load(config_path); - auto com_port = tools::read(yaml, "com_port"); + auto yaml = component::load(config_path); + auto com_port = component::read(yaml, "com_port"); try { serial_.setPort(com_port); serial_.open(); } catch (const std::exception & e) { - tools::logger()->error("[Gimbal] Failed to open serial: {}", e.what()); + component::logger()->error("[Gimbal] Failed to open serial: {}", e.what()); exit(1); } thread_ = std::thread(&Gimbal::read_thread, this); queue_.pop(); - tools::logger()->info("[Gimbal] First q received."); + component::logger()->info("[Gimbal] First q received."); } Gimbal::~Gimbal() @@ -66,8 +66,8 @@ Eigen::Quaterniond Gimbal::q(std::chrono::steady_clock::time_point t) while (true) { auto [q_a, t_a] = queue_.pop(); auto [q_b, t_b] = queue_.front(); - auto t_ab = tools::delta_time(t_a, t_b); - auto t_ac = tools::delta_time(t_a, t); + auto t_ab = component::delta_time(t_a, t_b); + auto t_ac = component::delta_time(t_a, t); auto k = t_ac / t_ab; Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized(); if (t < t_a) return q_c; @@ -77,7 +77,7 @@ Eigen::Quaterniond Gimbal::q(std::chrono::steady_clock::time_point t) } } -void Gimbal::send(io::VisionToGimbal VisionToGimbal) +void Gimbal::send(device::VisionToGimbal VisionToGimbal) { tx_data_.mode = VisionToGimbal.mode; tx_data_.yaw = VisionToGimbal.yaw; @@ -86,13 +86,13 @@ void Gimbal::send(io::VisionToGimbal VisionToGimbal) tx_data_.pitch = VisionToGimbal.pitch; tx_data_.pitch_vel = VisionToGimbal.pitch_vel; tx_data_.pitch_acc = VisionToGimbal.pitch_acc; - tx_data_.crc16 = tools::get_crc16( + tx_data_.crc16 = component::get_crc16( reinterpret_cast(&tx_data_), sizeof(tx_data_) - sizeof(tx_data_.crc16)); try { serial_.write(reinterpret_cast(&tx_data_), sizeof(tx_data_)); } catch (const std::exception & e) { - tools::logger()->warn("[Gimbal] Failed to write serial: {}", e.what()); + component::logger()->warn("[Gimbal] Failed to write serial: {}", e.what()); } } @@ -107,13 +107,13 @@ void Gimbal::send( tx_data_.pitch = pitch; tx_data_.pitch_vel = pitch_vel; tx_data_.pitch_acc = pitch_acc; - tx_data_.crc16 = tools::get_crc16( + tx_data_.crc16 = component::get_crc16( reinterpret_cast(&tx_data_), sizeof(tx_data_) - sizeof(tx_data_.crc16)); try { serial_.write(reinterpret_cast(&tx_data_), sizeof(tx_data_)); } catch (const std::exception & e) { - tools::logger()->warn("[Gimbal] Failed to write serial: {}", e.what()); + component::logger()->warn("[Gimbal] Failed to write serial: {}", e.what()); } } @@ -122,20 +122,20 @@ bool Gimbal::read(uint8_t * buffer, size_t size) try { return serial_.read(buffer, size) == size; } catch (const std::exception & e) { - // tools::logger()->warn("[Gimbal] Failed to read serial: {}", e.what()); + // component::logger()->warn("[Gimbal] Failed to read serial: {}", e.what()); return false; } } void Gimbal::read_thread() { - tools::logger()->info("[Gimbal] read_thread started."); + component::logger()->info("[Gimbal] read_thread started."); int error_count = 0; while (!quit_) { if (error_count > 5000) { error_count = 0; - tools::logger()->warn("[Gimbal] Too many errors, attempting to reconnect..."); + component::logger()->warn("[Gimbal] Too many errors, attempting to reconnect..."); reconnect(); continue; } @@ -156,8 +156,8 @@ void Gimbal::read_thread() continue; } - if (!tools::check_crc16(reinterpret_cast(&rx_data_), sizeof(rx_data_))) { - tools::logger()->debug("[Gimbal] CRC16 check failed."); + if (!component::check_crc16(reinterpret_cast(&rx_data_), sizeof(rx_data_))) { + component::logger()->debug("[Gimbal] CRC16 check failed."); continue; } @@ -189,19 +189,19 @@ void Gimbal::read_thread() break; default: mode_ = GimbalMode::IDLE; - tools::logger()->warn("[Gimbal] Invalid mode: {}", rx_data_.mode); + component::logger()->warn("[Gimbal] Invalid mode: {}", rx_data_.mode); break; } } - tools::logger()->info("[Gimbal] read_thread stopped."); + component::logger()->info("[Gimbal] read_thread stopped."); } void Gimbal::reconnect() { int max_retry_count = 10; for (int i = 0; i < max_retry_count && !quit_; ++i) { - tools::logger()->warn("[Gimbal] Reconnecting serial, attempt {}/{}...", i + 1, max_retry_count); + component::logger()->warn("[Gimbal] Reconnecting serial, attempt {}/{}...", i + 1, max_retry_count); try { serial_.close(); std::this_thread::sleep_for(std::chrono::seconds(1)); @@ -211,13 +211,13 @@ void Gimbal::reconnect() try { serial_.open(); // 尝试重新打开 queue_.clear(); - tools::logger()->info("[Gimbal] Reconnected serial successfully."); + component::logger()->info("[Gimbal] Reconnected serial successfully."); break; } catch (const std::exception & e) { - tools::logger()->warn("[Gimbal] Reconnect failed: {}", e.what()); + component::logger()->warn("[Gimbal] Reconnect failed: {}", e.what()); std::this_thread::sleep_for(std::chrono::seconds(1)); } } } -} // namespace io \ No newline at end of file +} // namespace device \ No newline at end of file diff --git a/src/device/gimbal/gimbal.hpp b/src/device/gimbal/gimbal.hpp index e47bce6..735a5c7 100644 --- a/src/device/gimbal/gimbal.hpp +++ b/src/device/gimbal/gimbal.hpp @@ -1,5 +1,5 @@ -#ifndef IO__GIMBAL_HPP -#define IO__GIMBAL_HPP +#ifndef DEVICE__GIMBAL_HPP +#define DEVICE__GIMBAL_HPP #include #include @@ -12,7 +12,7 @@ #include "serial/serial.h" #include "src/component/thread_safe_queue.hpp" -namespace io +namespace device { struct __attribute__((packed)) GimbalToVision { @@ -79,7 +79,7 @@ public: bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel, float pitch_acc); - void send(io::VisionToGimbal VisionToGimbal); + void send(device::VisionToGimbal VisionToGimbal); private: serial::Serial serial_; @@ -93,7 +93,7 @@ private: GimbalMode mode_ = GimbalMode::IDLE; GimbalState state_; - tools::ThreadSafeQueue> + component::ThreadSafeQueue> queue_{1000}; bool read(uint8_t * buffer, size_t size); @@ -101,6 +101,6 @@ private: void reconnect(); }; -} // namespace io +} // namespace device -#endif // IO__GIMBAL_HPP \ No newline at end of file +#endif // DEVICE__GIMBAL_HPP \ No newline at end of file diff --git a/src/device/hikrobot/hikrobot.cpp b/src/device/hikrobot/hikrobot.cpp index 8f0eaa8..bcce4f4 100644 --- a/src/device/hikrobot/hikrobot.cpp +++ b/src/device/hikrobot/hikrobot.cpp @@ -6,16 +6,16 @@ using namespace std::chrono_literals; -namespace io +namespace device { HikRobot::HikRobot(double exposure_ms, double gain, const std::string & vid_pid) : exposure_us_(exposure_ms * 1e3), gain_(gain), queue_(1), daemon_quit_(false), vid_(-1), pid_(-1) { set_vid_pid(vid_pid); - if (libusb_init(NULL)) tools::logger()->warn("Unable to init libusb!"); + if (libusb_init(NULL)) component::logger()->warn("Unable to init libusb!"); daemon_thread_ = std::thread{[this] { - tools::logger()->info("HikRobot's daemon thread started."); + component::logger()->info("HikRobot's daemon thread started."); capture_start(); @@ -31,7 +31,7 @@ HikRobot::HikRobot(double exposure_ms, double gain, const std::string & vid_pid) capture_stop(); - tools::logger()->info("HikRobot's daemon thread stopped."); + component::logger()->info("HikRobot's daemon thread stopped."); }}; } @@ -39,7 +39,7 @@ HikRobot::~HikRobot() { daemon_quit_ = true; if (daemon_thread_.joinable()) daemon_thread_.join(); - tools::logger()->info("HikRobot destructed."); + component::logger()->info("HikRobot destructed."); } void HikRobot::read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp) @@ -61,24 +61,24 @@ void HikRobot::capture_start() MV_CC_DEVICE_INFO_LIST device_list; ret = MV_CC_EnumDevices(MV_USB_DEVICE, &device_list); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_EnumDevices failed: {:#x}", ret); + component::logger()->warn("MV_CC_EnumDevices failed: {:#x}", ret); return; } if (device_list.nDeviceNum == 0) { - tools::logger()->warn("Not found camera!"); + component::logger()->warn("Not found camera!"); return; } ret = MV_CC_CreateHandle(&handle_, device_list.pDeviceInfo[0]); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_CreateHandle failed: {:#x}", ret); + component::logger()->warn("MV_CC_CreateHandle failed: {:#x}", ret); return; } ret = MV_CC_OpenDevice(handle_); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_OpenDevice failed: {:#x}", ret); + component::logger()->warn("MV_CC_OpenDevice failed: {:#x}", ret); return; } @@ -91,12 +91,12 @@ void HikRobot::capture_start() ret = MV_CC_StartGrabbing(handle_); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_StartGrabbing failed: {:#x}", ret); + component::logger()->warn("MV_CC_StartGrabbing failed: {:#x}", ret); return; } capture_thread_ = std::thread{[this] { - tools::logger()->info("HikRobot's capture thread started."); + component::logger()->info("HikRobot's capture thread started."); capturing_ = true; @@ -111,7 +111,7 @@ void HikRobot::capture_start() ret = MV_CC_GetImageBuffer(handle_, &raw, nMsec); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_GetImageBuffer failed: {:#x}", ret); + component::logger()->warn("MV_CC_GetImageBuffer failed: {:#x}", ret); break; } @@ -145,13 +145,13 @@ void HikRobot::capture_start() ret = MV_CC_FreeImageBuffer(handle_, &raw); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_FreeImageBuffer failed: {:#x}", ret); + component::logger()->warn("MV_CC_FreeImageBuffer failed: {:#x}", ret); break; } } capturing_ = false; - tools::logger()->info("HikRobot's capture thread stopped."); + component::logger()->info("HikRobot's capture thread stopped."); }}; } @@ -164,19 +164,19 @@ void HikRobot::capture_stop() ret = MV_CC_StopGrabbing(handle_); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_StopGrabbing failed: {:#x}", ret); + component::logger()->warn("MV_CC_StopGrabbing failed: {:#x}", ret); return; } ret = MV_CC_CloseDevice(handle_); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_CloseDevice failed: {:#x}", ret); + component::logger()->warn("MV_CC_CloseDevice failed: {:#x}", ret); return; } ret = MV_CC_DestroyHandle(handle_); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_DestroyHandle failed: {:#x}", ret); + component::logger()->warn("MV_CC_DestroyHandle failed: {:#x}", ret); return; } } @@ -188,7 +188,7 @@ void HikRobot::set_float_value(const std::string & name, double value) ret = MV_CC_SetFloatValue(handle_, name.c_str(), value); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_SetFloatValue(\"{}\", {}) failed: {:#x}", name, value, ret); + component::logger()->warn("MV_CC_SetFloatValue(\"{}\", {}) failed: {:#x}", name, value, ret); return; } } @@ -200,7 +200,7 @@ void HikRobot::set_enum_value(const std::string & name, unsigned int value) ret = MV_CC_SetEnumValue(handle_, name.c_str(), value); if (ret != MV_OK) { - tools::logger()->warn("MV_CC_SetEnumValue(\"{}\", {}) failed: {:#x}", name, value, ret); + component::logger()->warn("MV_CC_SetEnumValue(\"{}\", {}) failed: {:#x}", name, value, ret); return; } } @@ -209,7 +209,7 @@ void HikRobot::set_vid_pid(const std::string & vid_pid) { auto index = vid_pid.find(':'); if (index == std::string::npos) { - tools::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid); + component::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid); return; } @@ -220,7 +220,7 @@ void HikRobot::set_vid_pid(const std::string & vid_pid) vid_ = std::stoi(vid_str, 0, 16); pid_ = std::stoi(pid_str, 0, 16); } catch (const std::exception &) { - tools::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid); + component::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid); } } @@ -231,16 +231,16 @@ void HikRobot::reset_usb() const // https://github.com/ralight/usb-reset/blob/master/usb-reset.c auto handle = libusb_open_device_with_vid_pid(NULL, vid_, pid_); if (!handle) { - tools::logger()->warn("Unable to open usb!"); + component::logger()->warn("Unable to open usb!"); return; } if (libusb_reset_device(handle)) - tools::logger()->warn("Unable to reset usb!"); + component::logger()->warn("Unable to reset usb!"); else - tools::logger()->info("Reset usb successfully :)"); + component::logger()->info("Reset usb successfully :)"); libusb_close(handle); } -} // namespace io \ No newline at end of file +} // namespace device \ No newline at end of file diff --git a/src/device/hikrobot/hikrobot.hpp b/src/device/hikrobot/hikrobot.hpp index 31a49ed..56ef672 100644 --- a/src/device/hikrobot/hikrobot.hpp +++ b/src/device/hikrobot/hikrobot.hpp @@ -1,5 +1,5 @@ -#ifndef IO__HIKROBOT_HPP -#define IO__HIKROBOT_HPP +#ifndef DEVICE__HIKROBOT_HPP +#define DEVICE__HIKROBOT_HPP #include #include @@ -11,7 +11,7 @@ #include "src/device/camera.hpp" #include "src/component/thread_safe_queue.hpp" -namespace io +namespace device { class HikRobot : public CameraBase { @@ -37,7 +37,7 @@ private: std::thread capture_thread_; std::atomic capturing_; std::atomic capture_quit_; - tools::ThreadSafeQueue queue_; + component::ThreadSafeQueue queue_; int vid_, pid_; @@ -51,6 +51,6 @@ private: void reset_usb() const; }; -} // namespace io +} // namespace device -#endif // IO__HIKROBOT_HPP \ No newline at end of file +#endif // DEVICE__HIKROBOT_HPP \ No newline at end of file diff --git a/src/device/mindvision/mindvision.cpp b/src/device/mindvision/mindvision.cpp index 6b7967e..e190409 100644 --- a/src/device/mindvision/mindvision.cpp +++ b/src/device/mindvision/mindvision.cpp @@ -8,7 +8,7 @@ using namespace std::chrono_literals; -namespace io +namespace device { MindVision::MindVision(double exposure_ms, double gamma, const std::string & vid_pid) : exposure_ms_(exposure_ms), @@ -21,7 +21,7 @@ MindVision::MindVision(double exposure_ms, double gamma, const std::string & vid pid_(-1) { set_vid_pid(vid_pid); - if (libusb_init(NULL)) tools::logger()->warn("Unable to init libusb!"); + if (libusb_init(NULL)) component::logger()->warn("Unable to init libusb!"); try_open(); @@ -47,7 +47,7 @@ MindVision::~MindVision() if (daemon_thread_.joinable()) daemon_thread_.join(); if (capture_thread_.joinable()) capture_thread_.join(); close(); - tools::logger()->info("Mindvision destructed."); + component::logger()->info("Mindvision destructed."); } void MindVision::read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp) @@ -100,7 +100,7 @@ void MindVision::open() auto timestamp = std::chrono::steady_clock::now(); if (status != CAMERA_STATUS_SUCCESS) { - tools::logger()->warn("Camera dropped!"); + component::logger()->warn("Camera dropped!"); ok_ = false; break; } @@ -112,7 +112,7 @@ void MindVision::open() } }}; - tools::logger()->info("Mindvision opened."); + component::logger()->info("Mindvision opened."); } void MindVision::try_open() @@ -120,7 +120,7 @@ void MindVision::try_open() try { open(); } catch (const std::exception & e) { - tools::logger()->warn("{}", e.what()); + component::logger()->warn("{}", e.what()); } } @@ -134,7 +134,7 @@ void MindVision::set_vid_pid(const std::string & vid_pid) { auto index = vid_pid.find(':'); if (index == std::string::npos) { - tools::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid); + component::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid); return; } @@ -145,7 +145,7 @@ void MindVision::set_vid_pid(const std::string & vid_pid) vid_ = std::stoi(vid_str, 0, 16); pid_ = std::stoi(pid_str, 0, 16); } catch (const std::exception &) { - tools::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid); + component::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid); } } @@ -156,16 +156,16 @@ void MindVision::reset_usb() const // https://github.com/ralight/usb-reset/blob/master/usb-reset.c auto handle = libusb_open_device_with_vid_pid(NULL, vid_, pid_); if (!handle) { - tools::logger()->warn("Unable to open usb!"); + component::logger()->warn("Unable to open usb!"); return; } if (libusb_reset_device(handle)) - tools::logger()->warn("Unable to reset usb!"); + component::logger()->warn("Unable to reset usb!"); else - tools::logger()->info("Reset usb successfully :)"); + component::logger()->info("Reset usb successfully :)"); libusb_close(handle); } -} // namespace io +} // namespace device diff --git a/src/device/mindvision/mindvision.hpp b/src/device/mindvision/mindvision.hpp index 124cbce..4db378c 100644 --- a/src/device/mindvision/mindvision.hpp +++ b/src/device/mindvision/mindvision.hpp @@ -1,5 +1,5 @@ -#ifndef IO__MINDVISION_HPP -#define IO__MINDVISION_HPP +#ifndef DEVICE__MINDVISION_HPP +#define DEVICE__MINDVISION_HPP #include #include @@ -9,7 +9,7 @@ #include "src/device/camera.hpp" #include "src/component/thread_safe_queue.hpp" -namespace io +namespace device { class MindVision : public CameraBase { @@ -31,7 +31,7 @@ private: bool quit_, ok_; std::thread capture_thread_; std::thread daemon_thread_; - tools::ThreadSafeQueue queue_; + component::ThreadSafeQueue queue_; int vid_, pid_; void open(); @@ -41,6 +41,6 @@ private: void reset_usb() const; }; -} // namespace io +} // namespace device -#endif // IO__MINDVISION_HPP \ No newline at end of file +#endif // DEVICE__MINDVISION_HPP \ No newline at end of file diff --git a/src/device/ros2/publish2nav.cpp b/src/device/ros2/publish2nav.cpp index 761b865..1fd82c5 100644 --- a/src/device/ros2/publish2nav.cpp +++ b/src/device/ros2/publish2nav.cpp @@ -7,7 +7,7 @@ #include "src/component/logger.hpp" -namespace io +namespace device { Publish2Nav::Publish2Nav() : Node("auto_aim_target_pos_publisher") @@ -45,4 +45,4 @@ void Publish2Nav::start() rclcpp::spin(this->shared_from_this()); } -} // namespace io +} // namespace device diff --git a/src/device/ros2/publish2nav.hpp b/src/device/ros2/publish2nav.hpp index b738e73..12afff7 100644 --- a/src/device/ros2/publish2nav.hpp +++ b/src/device/ros2/publish2nav.hpp @@ -1,5 +1,5 @@ -#ifndef IO__PBLISH2NAV_HPP -#define IO__PBLISH2NAV_HPP +#ifndef DEVICE__PBLISH2NAV_HPP +#define DEVICE__PBLISH2NAV_HPP #include // For Eigen::Vector3d #include @@ -12,7 +12,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -namespace io +namespace device { class Publish2Nav : public rclcpp::Node { @@ -30,6 +30,6 @@ private: rclcpp::Publisher::SharedPtr publisher_; }; -} // namespace io +} // namespace device #endif // Publish2Nav_HPP_ diff --git a/src/device/ros2/ros2.cpp b/src/device/ros2/ros2.cpp index 4a25bfd..4397553 100644 --- a/src/device/ros2/ros2.cpp +++ b/src/device/ros2/ros2.cpp @@ -1,5 +1,5 @@ #include "ros2.hpp" -namespace io +namespace device { ROS2::ROS2() { @@ -33,4 +33,4 @@ std::vector ROS2::subscribe_autoaim_target() return subscribe2nav_->subscribe_autoaim_target(); } -} // namespace io +} // namespace device diff --git a/src/device/ros2/ros2.hpp b/src/device/ros2/ros2.hpp index 848fa40..ef3a469 100644 --- a/src/device/ros2/ros2.hpp +++ b/src/device/ros2/ros2.hpp @@ -1,10 +1,10 @@ -#ifndef IO__ROS2_HPP -#define IO__ROS2_HPP +#ifndef DEVICE__ROS2_HPP +#define DEVICE__ROS2_HPP #include "publish2nav.hpp" #include "subscribe2nav.hpp" -namespace io +namespace device { class ROS2 { @@ -41,5 +41,5 @@ private: std::unique_ptr subscribe_spin_thread_; }; -} // namespace io +} // namespace device #endif \ No newline at end of file diff --git a/src/device/ros2/subscribe2nav.cpp b/src/device/ros2/subscribe2nav.cpp index bc5934e..5fb2804 100644 --- a/src/device/ros2/subscribe2nav.cpp +++ b/src/device/ros2/subscribe2nav.cpp @@ -3,7 +3,7 @@ #include #include -namespace io +namespace device { Subscribe2Nav::Subscribe2Nav() @@ -105,4 +105,4 @@ std::vector Subscribe2Nav::subscribe_autoaim_target() return msg.target_ids; } -} // namespace io \ No newline at end of file +} // namespace device \ No newline at end of file diff --git a/src/device/ros2/subscribe2nav.hpp b/src/device/ros2/subscribe2nav.hpp index 16b3a20..0dbad68 100644 --- a/src/device/ros2/subscribe2nav.hpp +++ b/src/device/ros2/subscribe2nav.hpp @@ -1,5 +1,5 @@ -#ifndef IO__SUBSCRIBE2NAV_HPP -#define IO__SUBSCRIBE2NAV_HPP +#ifndef DEVICE__SUBSCRIBE2NAV_HPP +#define DEVICE__SUBSCRIBE2NAV_HPP #include #include @@ -10,7 +10,7 @@ #include "sp_msgs/msg/enemy_status_msg.hpp" #include "src/component/thread_safe_queue.hpp" -namespace io +namespace device { class Subscribe2Nav : public rclcpp::Node { @@ -37,9 +37,9 @@ private: rclcpp::Subscription::SharedPtr enemy_status_subscription_; rclcpp::Subscription::SharedPtr autoaim_target_subscription_; - tools::ThreadSafeQueue enemy_statue_queue_; - tools::ThreadSafeQueue autoaim_target_queue_; + component::ThreadSafeQueue enemy_statue_queue_; + component::ThreadSafeQueue autoaim_target_queue_; }; -} // namespace io +} // namespace device -#endif // IO__SUBSCRIBE2NAV_HPP +#endif // DEVICE__SUBSCRIBE2NAV_HPP diff --git a/src/device/socketcan.hpp b/src/device/socketcan.hpp index 31fe85f..785d5cd 100644 --- a/src/device/socketcan.hpp +++ b/src/device/socketcan.hpp @@ -1,5 +1,5 @@ -#ifndef IO__SOCKETCAN_HPP -#define IO__SOCKETCAN_HPP +#ifndef DEVICE__SOCKETCAN_HPP +#define DEVICE__SOCKETCAN_HPP #include #include @@ -19,7 +19,7 @@ using namespace std::chrono_literals; constexpr int MAX_EVENTS = 10; -namespace io +namespace device { class SocketCAN { @@ -55,7 +55,7 @@ public: if (daemon_thread_.joinable()) daemon_thread_.join(); if (read_thread_.joinable()) read_thread_.join(); close(); - tools::logger()->info("SocketCAN destructed."); + component::logger()->info("SocketCAN destructed."); } void write(can_frame * frame) const @@ -113,14 +113,14 @@ private: try { read(); } catch (const std::exception & e) { - tools::logger()->warn("SocketCAN::read() failed: {}", e.what()); + component::logger()->warn("SocketCAN::read() failed: {}", e.what()); ok_ = false; break; } } }); - tools::logger()->info("SocketCAN opened."); + component::logger()->info("SocketCAN opened."); } void try_open() @@ -128,7 +128,7 @@ private: try { open(); } catch (const std::exception & e) { - tools::logger()->warn("SocketCAN::open() failed: {}", e.what()); + component::logger()->warn("SocketCAN::open() failed: {}", e.what()); } } @@ -154,6 +154,6 @@ private: } }; -} // namespace io +} // namespace device -#endif // IO__SOCKETCAN_HPP \ No newline at end of file +#endif // DEVICE__SOCKETCAN_HPP \ No newline at end of file diff --git a/src/device/usbcamera/usbcamera.cpp b/src/device/usbcamera/usbcamera.cpp index e998936..eae734f 100644 --- a/src/device/usbcamera/usbcamera.cpp +++ b/src/device/usbcamera/usbcamera.cpp @@ -7,30 +7,30 @@ using namespace std::chrono_literals; -namespace io +namespace device { USBCamera::USBCamera(const std::string & open_name, const std::string & config_path) : open_name_(open_name), quit_(false), ok_(false), queue_(1), open_count_(0) { - auto yaml = tools::load(config_path); - image_width_ = tools::read(yaml, "image_width"); - image_height_ = tools::read(yaml, "image_height"); - usb_exposure_ = tools::read(yaml, "usb_exposure"); - usb_frame_rate_ = tools::read(yaml, "usb_frame_rate"); - usb_gamma_ = tools::read(yaml, "usb_gamma"); - usb_gain_ = tools::read(yaml, "usb_gain"); + auto yaml = component::load(config_path); + image_width_ = component::read(yaml, "image_width"); + image_height_ = component::read(yaml, "image_height"); + usb_exposure_ = component::read(yaml, "usb_exposure"); + usb_frame_rate_ = component::read(yaml, "usb_frame_rate"); + usb_gamma_ = component::read(yaml, "usb_gamma"); + usb_gain_ = component::read(yaml, "usb_gain"); try_open(); // 守护线程 daemon_thread_ = std::thread{[this] { - // tools::logger()->info("daemon thread start"); + // component::logger()->info("daemon thread start"); while (!quit_) { std::this_thread::sleep_for(100ms); if (ok_) continue; if (open_count_ > 20) { - tools::logger()->warn("Give up to open {} USB camera", this->device_name); + component::logger()->warn("Give up to open {} USB camera", this->device_name); quit_ = true; { @@ -39,7 +39,7 @@ USBCamera::USBCamera(const std::string & open_name, const std::string & config_p } if (capture_thread_.joinable()) { - tools::logger()->warn("Stopping capture thread"); + component::logger()->warn("Stopping capture thread"); capture_thread_.join(); } @@ -54,7 +54,7 @@ USBCamera::USBCamera(const std::string & open_name, const std::string & config_p } try_open(); } - // tools::logger()->info("daemon thread exit"); + // component::logger()->info("daemon thread exit"); }}; } @@ -67,14 +67,14 @@ USBCamera::~USBCamera() } if (daemon_thread_.joinable()) daemon_thread_.join(); if (capture_thread_.joinable()) capture_thread_.join(); - tools::logger()->info("USBCamera destructed."); + component::logger()->info("USBCamera destructed."); } cv::Mat USBCamera::read() { std::lock_guard lock(cap_mutex_); if (!cap_.isOpened()) { - tools::logger()->warn("Failed to read {} USB camera", this->device_name); + component::logger()->warn("Failed to read {} USB camera", this->device_name); return cv::Mat(); } cap_ >> img_; @@ -96,7 +96,7 @@ void USBCamera::open() std::string true_device_name = "/dev/" + open_name_; cap_.open(true_device_name, cv::CAP_V4L); if (!cap_.isOpened()) { - tools::logger()->warn("Failed to open USB camera"); + component::logger()->warn("Failed to open USB camera"); return; } sharpness_ = cap_.get(cv::CAP_PROP_SHARPNESS); @@ -118,16 +118,16 @@ void USBCamera::open() cap_.set(cv::CAP_PROP_EXPOSURE, usb_exposure_); } - tools::logger()->info("{} USBCamera opened", device_name); - // tools::logger()->info("USBCamera exposure time:{}", cap_.get(cv::CAP_PROP_EXPOSURE)); - tools::logger()->info("USBCamera fps:{}", cap_.get(cv::CAP_PROP_FPS)); - // tools::logger()->info("USBCamera gamma:{}", cap_.get(cv::CAP_PROP_GAMMA)); + component::logger()->info("{} USBCamera opened", device_name); + // component::logger()->info("USBCamera exposure time:{}", cap_.get(cv::CAP_PROP_EXPOSURE)); + component::logger()->info("USBCamera fps:{}", cap_.get(cv::CAP_PROP_FPS)); + // component::logger()->info("USBCamera gamma:{}", cap_.get(cv::CAP_PROP_GAMMA)); // 取图线程 capture_thread_ = std::thread{[this] { ok_ = true; std::this_thread::sleep_for(50ms); - tools::logger()->info("[{} USB camera] capture thread started ", this->device_name); + component::logger()->info("[{} USB camera] capture thread started ", this->device_name); while (!quit_) { std::this_thread::sleep_for(1ms); @@ -142,7 +142,7 @@ void USBCamera::open() } if (!success) { - tools::logger()->warn("Failed to read frame, exiting capture thread"); + component::logger()->warn("Failed to read frame, exiting capture thread"); break; } @@ -159,7 +159,7 @@ void USBCamera::try_open() open(); open_count_++; } catch (const std::exception & e) { - tools::logger()->warn("{}", e.what()); + component::logger()->warn("{}", e.what()); } } @@ -167,8 +167,8 @@ void USBCamera::close() { if (cap_.isOpened()) { cap_.release(); - tools::logger()->info("USB camera released."); + component::logger()->info("USB camera released."); } } -} // namespace io \ No newline at end of file +} // namespace device \ No newline at end of file diff --git a/src/device/usbcamera/usbcamera.hpp b/src/device/usbcamera/usbcamera.hpp index c18d5e0..a504f0a 100644 --- a/src/device/usbcamera/usbcamera.hpp +++ b/src/device/usbcamera/usbcamera.hpp @@ -1,5 +1,5 @@ -#ifndef IO__USBCamera_HPP -#define IO__USBCamera_HPP +#ifndef DEVICE__USBCamera_HPP +#define DEVICE__USBCamera_HPP #include #include @@ -8,7 +8,7 @@ #include "src/component/thread_safe_queue.hpp" -namespace io +namespace device { class USBCamera { @@ -37,13 +37,13 @@ private: bool quit_, ok_; std::thread capture_thread_; std::thread daemon_thread_; - tools::ThreadSafeQueue queue_; + component::ThreadSafeQueue queue_; void try_open(); void open(); void close(); }; -} // namespace io +} // namespace device #endif \ No newline at end of file diff --git a/src/module/auto_aim/aimer.cpp b/src/module/auto_aim/aimer.cpp index 933ce1f..dc1d1f6 100644 --- a/src/module/auto_aim/aimer.cpp +++ b/src/module/auto_aim/aimer.cpp @@ -25,11 +25,11 @@ Aimer::Aimer(const std::string & config_path) if (yaml["left_yaw_offset"].IsDefined() && yaml["right_yaw_offset"].IsDefined()) { left_yaw_offset_ = yaml["left_yaw_offset"].as() / 57.3; // degree to rad right_yaw_offset_ = yaml["right_yaw_offset"].as() / 57.3; // degree to rad - tools::logger()->info("[Aimer] successfully loading shootmode"); + component::logger()->info("[Aimer] successfully loading shootmode"); } } -io::Command Aimer::aim( +device::Command Aimer::aim( std::list targets, std::chrono::steady_clock::time_point timestamp, double bullet_speed, bool to_now) { @@ -46,14 +46,14 @@ io::Command Aimer::aim( auto future = timestamp; if (to_now) { double dt; - dt = tools::delta_time(std::chrono::steady_clock::now(), timestamp) + delay_time; + dt = component::delta_time(std::chrono::steady_clock::now(), timestamp) + delay_time; future += std::chrono::microseconds(int(dt * 1e6)); target.predict(future); } else { auto dt = 0.005 + delay_time; //detector-aimer耗时0.005+发弹延时0.1 - // tools::logger()->info("dt is {:.4f} second", dt); + // component::logger()->info("dt is {:.4f} second", dt); future += std::chrono::microseconds(int(dt * 1e6)); target.predict(future); } @@ -61,15 +61,15 @@ io::Command Aimer::aim( auto aim_point0 = choose_aim_point(target); debug_aim_point = aim_point0; if (!aim_point0.valid) { - // tools::logger()->debug("Invalid aim_point0."); + // component::logger()->debug("Invalid aim_point0."); return {false, false, 0, 0}; } Eigen::Vector3d xyz0 = aim_point0.xyza.head(3); auto d0 = std::sqrt(xyz0[0] * xyz0[0] + xyz0[1] * xyz0[1]); - tools::Trajectory trajectory0(bullet_speed, d0, xyz0[2]); + component::Trajectory trajectory0(bullet_speed, d0, xyz0[2]); if (trajectory0.unsolvable) { - tools::logger()->debug( + component::logger()->debug( "[Aimer] Unsolvable trajectory0: {:.2f} {:.2f} {:.2f}", bullet_speed, d0, xyz0[2]); debug_aim_point.valid = false; return {false, false, 0, 0}; @@ -78,7 +78,7 @@ io::Command Aimer::aim( // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) bool converged = false; double prev_fly_time = trajectory0.fly_time; - tools::Trajectory current_traj = trajectory0; + component::Trajectory current_traj = trajectory0; std::vector iteration_target(10, target); // 创建10个目标副本用于迭代预测 for (int iter = 0; iter < 10; ++iter) { @@ -96,11 +96,11 @@ io::Command Aimer::aim( // 计算新弹道 Eigen::Vector3d xyz = aim_point.xyza.head(3); double d = std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y()); - current_traj = tools::Trajectory(bullet_speed, d, xyz.z()); + current_traj = component::Trajectory(bullet_speed, d, xyz.z()); // 检查弹道是否可解 if (current_traj.unsolvable) { - tools::logger()->debug( + component::logger()->debug( "[Aimer] Unsolvable trajectory in iter {}: speed={:.2f}, d={:.2f}, z={:.2f}", iter + 1, bullet_speed, d, xyz.z()); debug_aim_point.valid = false; @@ -122,14 +122,14 @@ io::Command Aimer::aim( return {true, false, yaw, pitch}; } -io::Command Aimer::aim( +device::Command Aimer::aim( std::list targets, std::chrono::steady_clock::time_point timestamp, double bullet_speed, - io::ShootMode shoot_mode, bool to_now) + device::ShootMode shoot_mode, bool to_now) { double yaw_offset; - if (shoot_mode == io::left_shoot && left_yaw_offset_.has_value()) { + if (shoot_mode == device::left_shoot && left_yaw_offset_.has_value()) { yaw_offset = left_yaw_offset_.value(); - } else if (shoot_mode == io::right_shoot && right_yaw_offset_.has_value()) { + } else if (shoot_mode == device::right_shoot && right_yaw_offset_.has_value()) { yaw_offset = right_yaw_offset_.value(); } else { yaw_offset = yaw_offset_; @@ -155,7 +155,7 @@ AimPoint Aimer::choose_aim_point(const Target & target) // 如果delta_angle为0，则该装甲板中心和整车中心的连线在世界坐标系的xy平面过原点 std::vector delta_angle_list; for (int i = 0; i < armor_num; i++) { - auto delta_angle = tools::limit_rad(armor_xyza_list[i][3] - center_yaw); + auto delta_angle = component::limit_rad(armor_xyza_list[i][3] - center_yaw); delta_angle_list.emplace_back(delta_angle); } @@ -169,7 +169,7 @@ AimPoint Aimer::choose_aim_point(const Target & target) } // 绝无可能 if (id_list.empty()) { - tools::logger()->warn("Empty id list!"); + component::logger()->warn("Empty id list!"); return {false, armor_xyza_list[0]}; } diff --git a/src/module/auto_aim/aimer.hpp b/src/module/auto_aim/aimer.hpp index df04873..b3878ad 100644 --- a/src/module/auto_aim/aimer.hpp +++ b/src/module/auto_aim/aimer.hpp @@ -23,13 +23,13 @@ class Aimer public: AimPoint debug_aim_point; explicit Aimer(const std::string & config_path); - io::Command aim( + device::Command aim( std::list targets, std::chrono::steady_clock::time_point timestamp, double bullet_speed, bool to_now = true); - io::Command aim( + device::Command aim( std::list targets, std::chrono::steady_clock::time_point timestamp, double bullet_speed, - io::ShootMode shoot_mode, bool to_now = true); + device::ShootMode shoot_mode, bool to_now = true); private: double yaw_offset_; diff --git a/src/module/auto_aim/detector.cpp b/src/module/auto_aim/detector.cpp index 75b55ae..13b6b92 100644 --- a/src/module/auto_aim/detector.cpp +++ b/src/module/auto_aim/detector.cpp @@ -208,12 +208,12 @@ bool Detector::detect(Armor & armor, const cv::Mat & bgr_img) } } - // tools::logger()->debug( + // component::logger()->debug( // "min_distance_br_tr + min_distance_tl_bl is {}", min_distance_br_tr + min_distance_tl_bl); // std::vector points2f{ // closest_left_lightbar->top, closest_left_lightbar->bottom, closest_right_lightbar->bottom, // closest_right_lightbar->top}; - // tools::draw_points(armor_roi, points2f, {0, 0, 255}, 2); + // component::draw_points(armor_roi, points2f, {0, 0, 255}, 2); // cv::imshow("armor_roi", armor_roi); if ( @@ -255,7 +255,7 @@ bool Detector::check_name(const Armor & armor) const if (name_ok && !confidence_ok) save(armor); // 出现 5号 则显示 debug 信息。但不过滤。 - if (armor.name == ArmorName::five) tools::logger()->debug("See pattern 5"); + if (armor.name == ArmorName::five) component::logger()->debug("See pattern 5"); return name_ok && confidence_ok; } @@ -268,7 +268,7 @@ bool Detector::check_type(const Armor & armor) const // 保存异常的图案，用于分类器的迭代 if (!name_ok) { - tools::logger()->debug( + component::logger()->debug( "see strange armor: {} {}", ARMOR_TYPES[armor.type], ARMOR_NAMES[armor.name]); save(armor); } @@ -314,18 +314,18 @@ ArmorType Detector::get_type(const Armor & armor) /// TODO: 25赛季是否还需要根据比例判断大小装甲？能否根据图案直接判断？ if (armor.ratio > 3.0) { - // tools::logger()->debug( + // component::logger()->debug( // "[Detector] get armor type by ratio: BIG {} {:.2f}", ARMOR_NAMES[armor.name], armor.ratio); return ArmorType::big; } if (armor.ratio < 2.5) { - // tools::logger()->debug( + // component::logger()->debug( // "[Detector] get armor type by ratio: SMALL {} {:.2f}", ARMOR_NAMES[armor.name], armor.ratio); return ArmorType::small; } - // tools::logger()->debug("[Detector] get armor type by name: {}", ARMOR_NAMES[armor.name]); + // component::logger()->debug("[Detector] get armor type by name: {}", ARMOR_NAMES[armor.name]); // 英雄、基地只能是大装甲板 if (armor.name == ArmorName::one || armor.name == ArmorName::base) { @@ -356,14 +356,14 @@ void Detector::show_result( const std::list & armors, int frame_count) const { auto detection = bgr_img.clone(); - tools::draw_text(detection, fmt::format("[{}]", frame_count), {10, 30}, {255, 255, 255}); + component::draw_text(detection, fmt::format("[{}]", frame_count), {10, 30}, {255, 255, 255}); for (const auto & lightbar : lightbars) { auto info = fmt::format( "{:.1f} {:.1f} {:.1f} {}", lightbar.angle_error * 57.3, lightbar.ratio, lightbar.length, COLORS[lightbar.color]); - tools::draw_text(detection, info, lightbar.top, {0, 255, 255}); - tools::draw_points(detection, lightbar.points, {0, 255, 255}, 3); + component::draw_text(detection, info, lightbar.top, {0, 255, 255}); + component::draw_points(detection, lightbar.points, {0, 255, 255}, 3); } for (const auto & armor : armors) { @@ -371,8 +371,8 @@ void Detector::show_result( "{:.2f} {:.2f} {:.1f} {:.2f} {} {}", armor.ratio, armor.side_ratio, armor.rectangular_error * 57.3, armor.confidence, ARMOR_NAMES[armor.name], ARMOR_TYPES[armor.type]); - tools::draw_points(detection, armor.points, {0, 255, 0}); - tools::draw_text(detection, info, armor.left.bottom, {0, 255, 0}); + component::draw_points(detection, armor.points, {0, 255, 0}); + component::draw_text(detection, info, armor.left.bottom, {0, 255, 0}); } cv::Mat binary_img2; diff --git a/src/module/auto_aim/multithread/commandgener.cpp b/src/module/auto_aim/multithread/commandgener.cpp index a10d84f..19784bd 100644 --- a/src/module/auto_aim/multithread/commandgener.cpp +++ b/src/module/auto_aim/multithread/commandgener.cpp @@ -8,8 +8,8 @@ namespace multithread { CommandGener::CommandGener( - auto_aim::Shooter & shooter, auto_aim::Aimer & aimer, io::CBoard & cboard, - tools::Plotter & plotter, bool debug) + auto_aim::Shooter & shooter, auto_aim::Aimer & aimer, device::CBoard & cboard, + component::Plotter & plotter, bool debug) : shooter_(shooter), aimer_(aimer), cboard_(cboard), plotter_(plotter), stop_(false), debug_(debug) { thread_ = std::thread(&CommandGener::generate_command, this); @@ -41,7 +41,7 @@ void CommandGener::generate_command() std::optional input; { std::lock_guard lock(mtx_); - if (latest_ && tools::delta_time(std::chrono::steady_clock::now(), latest_->t) < 0.2) { + if (latest_ && component::delta_time(std::chrono::steady_clock::now(), latest_->t) < 0.2) { input = latest_; } else input = std::nullopt; @@ -52,12 +52,12 @@ void CommandGener::generate_command() command.horizon_distance = input->targets_.empty() ? 0 : std::sqrt( - tools::square(input->targets_.front().ekf_x()[0]) + - tools::square(input->targets_.front().ekf_x()[2])); + component::square(input->targets_.front().ekf_x()[0]) + + component::square(input->targets_.front().ekf_x()[2])); cboard_.send(command); if (debug_) { nlohmann::json data; - data["t"] = tools::delta_time(std::chrono::steady_clock::now(), t0); + data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0); data["cmd_yaw"] = command.yaw * 57.3; data["cmd_pitch"] = command.pitch * 57.3; data["shoot"] = command.shoot; diff --git a/src/module/auto_aim/multithread/commandgener.hpp b/src/module/auto_aim/multithread/commandgener.hpp index 886c9ea..a338fae 100644 --- a/src/module/auto_aim/multithread/commandgener.hpp +++ b/src/module/auto_aim/multithread/commandgener.hpp @@ -18,8 +18,8 @@ class CommandGener { public: CommandGener( - auto_aim::Shooter & shooter, auto_aim::Aimer & aimer, io::CBoard & cboard, - tools::Plotter & plotter, bool debug = false); + auto_aim::Shooter & shooter, auto_aim::Aimer & aimer, device::CBoard & cboard, + component::Plotter & plotter, bool debug = false); ~CommandGener(); @@ -37,10 +37,10 @@ private: Eigen::Vector3d gimbal_pos; }; - io::CBoard & cboard_; + device::CBoard & cboard_; auto_aim::Shooter & shooter_; auto_aim::Aimer & aimer_; - tools::Plotter & plotter_; + component::Plotter & plotter_; std::optional latest_; std::mutex mtx_; diff --git a/src/module/auto_aim/multithread/mt_detector.cpp b/src/module/auto_aim/multithread/mt_detector.cpp index bf42928..a0b9b2b 100644 --- a/src/module/auto_aim/multithread/mt_detector.cpp +++ b/src/module/auto_aim/multithread/mt_detector.cpp @@ -37,7 +37,7 @@ MultiThreadDetector::MultiThreadDetector(const std::string & config_path, bool d compiled_model_ = core_.compile_model( model, device_, ov::hint::performance_mode(ov::hint::PerformanceMode::THROUGHPUT)); - tools::logger()->info("[MultiThreadDetector] initialized !"); + component::logger()->info("[MultiThreadDetector] initialized !"); } void MultiThreadDetector::push(cv::Mat img, std::chrono::steady_clock::time_point t) diff --git a/src/module/auto_aim/multithread/mt_detector.hpp b/src/module/auto_aim/multithread/mt_detector.hpp index a2d2b3c..7e241ef 100644 --- a/src/module/auto_aim/multithread/mt_detector.hpp +++ b/src/module/auto_aim/multithread/mt_detector.hpp @@ -32,9 +32,9 @@ private: std::string device_; YOLO yolo_; - tools::ThreadSafeQueue< + component::ThreadSafeQueue< std::tuple> - queue_{16, [] { tools::logger()->debug("[MultiThreadDetector] queue is full!"); }}; + queue_{16, [] { component::logger()->debug("[MultiThreadDetector] queue is full!"); }}; }; } // namespace multithread diff --git a/src/module/auto_aim/planner/planner.cpp b/src/module/auto_aim/planner/planner.cpp index 079b1bb..9ae590f 100644 --- a/src/module/auto_aim/planner/planner.cpp +++ b/src/module/auto_aim/planner/planner.cpp @@ -12,13 +12,13 @@ namespace auto_aim { Planner::Planner(const std::string & config_path) { - auto yaml = tools::load(config_path); - yaw_offset_ = tools::read(yaml, "yaw_offset") / 57.3; - pitch_offset_ = tools::read(yaml, "pitch_offset") / 57.3; - fire_thresh_ = tools::read(yaml, "fire_thresh"); - decision_speed_ = tools::read(yaml, "decision_speed"); - high_speed_delay_time_ = tools::read(yaml, "high_speed_delay_time"); - low_speed_delay_time_ = tools::read(yaml, "low_speed_delay_time"); + auto yaml = component::load(config_path); + yaw_offset_ = component::read(yaml, "yaw_offset") / 57.3; + pitch_offset_ = component::read(yaml, "pitch_offset") / 57.3; + fire_thresh_ = component::read(yaml, "fire_thresh"); + decision_speed_ = component::read(yaml, "decision_speed"); + high_speed_delay_time_ = component::read(yaml, "high_speed_delay_time"); + low_speed_delay_time_ = component::read(yaml, "low_speed_delay_time"); setup_yaw_solver(config_path); setup_pitch_solver(config_path); @@ -41,7 +41,7 @@ Plan Planner::plan(Target target, double bullet_speed) xyz = xyza.head<3>(); } } - auto bullet_traj = tools::Trajectory(bullet_speed, min_dist, xyz.z()); + auto bullet_traj = component::Trajectory(bullet_speed, min_dist, xyz.z()); target.predict(bullet_traj.fly_time); // 2. Get trajectory @@ -51,7 +51,7 @@ Plan Planner::plan(Target target, double bullet_speed) yaw0 = aim(target, bullet_speed)(0); traj = get_trajectory(target, yaw0, bullet_speed); } catch (const std::exception & e) { - tools::logger()->warn("Unsolvable target {:.2f}", bullet_speed); + component::logger()->warn("Unsolvable target {:.2f}", bullet_speed); return {false}; } @@ -73,10 +73,10 @@ Plan Planner::plan(Target target, double bullet_speed) Plan plan; plan.control = true; - plan.target_yaw = tools::limit_rad(traj(0, HALF_HORIZON) + yaw0); + plan.target_yaw = component::limit_rad(traj(0, HALF_HORIZON) + yaw0); plan.target_pitch = traj(2, HALF_HORIZON); - plan.yaw = tools::limit_rad(yaw_solver_->work->x(0, HALF_HORIZON) + yaw0); + plan.yaw = component::limit_rad(yaw_solver_->work->x(0, HALF_HORIZON) + yaw0); plan.yaw_vel = yaw_solver_->work->x(1, HALF_HORIZON); plan.yaw_acc = yaw_solver_->work->u(0, HALF_HORIZON); @@ -109,10 +109,10 @@ Plan Planner::plan(std::optional target, double bullet_speed) void Planner::setup_yaw_solver(const std::string & config_path) { - auto yaml = tools::load(config_path); - auto max_yaw_acc = tools::read(yaml, "max_yaw_acc"); - auto Q_yaw = tools::read>(yaml, "Q_yaw"); - auto R_yaw = tools::read>(yaml, "R_yaw"); + auto yaml = component::load(config_path); + auto max_yaw_acc = component::read(yaml, "max_yaw_acc"); + auto Q_yaw = component::read>(yaml, "Q_yaw"); + auto R_yaw = component::read>(yaml, "R_yaw"); Eigen::MatrixXd A{{1, DT}, {0, 1}}; Eigen::MatrixXd B{{0}, {DT}}; @@ -132,10 +132,10 @@ void Planner::setup_yaw_solver(const std::string & config_path) void Planner::setup_pitch_solver(const std::string & config_path) { - auto yaml = tools::load(config_path); - auto max_pitch_acc = tools::read(yaml, "max_pitch_acc"); - auto Q_pitch = tools::read>(yaml, "Q_pitch"); - auto R_pitch = tools::read>(yaml, "R_pitch"); + auto yaml = component::load(config_path); + auto max_pitch_acc = component::read(yaml, "max_pitch_acc"); + auto Q_pitch = component::read>(yaml, "Q_pitch"); + auto R_pitch = component::read>(yaml, "R_pitch"); Eigen::MatrixXd A{{1, DT}, {0, 1}}; Eigen::MatrixXd B{{0}, {DT}}; @@ -170,10 +170,10 @@ Eigen::Matrix Planner::aim(const Target & target, double bullet_sp debug_xyza = Eigen::Vector4d(xyz.x(), xyz.y(), xyz.z(), yaw); auto azim = std::atan2(xyz.y(), xyz.x()); - auto bullet_traj = tools::Trajectory(bullet_speed, min_dist, xyz.z()); + auto bullet_traj = component::Trajectory(bullet_speed, min_dist, xyz.z()); if (bullet_traj.unsolvable) throw std::runtime_error("Unsolvable bullet trajectory!"); - return {tools::limit_rad(azim + yaw_offset_), -bullet_traj.pitch - pitch_offset_}; + return {component::limit_rad(azim + yaw_offset_), -bullet_traj.pitch - pitch_offset_}; } Trajectory Planner::get_trajectory(Target & target, double yaw0, double bullet_speed) @@ -190,10 +190,10 @@ Trajectory Planner::get_trajectory(Target & target, double yaw0, double bullet_s target.predict(DT); auto yaw_pitch_next = aim(target, bullet_speed); - auto yaw_vel = tools::limit_rad(yaw_pitch_next(0) - yaw_pitch_last(0)) / (2 * DT); + auto yaw_vel = component::limit_rad(yaw_pitch_next(0) - yaw_pitch_last(0)) / (2 * DT); auto pitch_vel = (yaw_pitch_next(1) - yaw_pitch_last(1)) / (2 * DT); - traj.col(i) << tools::limit_rad(yaw_pitch(0) - yaw0), yaw_vel, yaw_pitch(1), pitch_vel; + traj.col(i) << component::limit_rad(yaw_pitch(0) - yaw0), yaw_vel, yaw_pitch(1), pitch_vel; yaw_pitch_last = yaw_pitch; yaw_pitch = yaw_pitch_next; diff --git a/src/module/auto_aim/shooter.cpp b/src/module/auto_aim/shooter.cpp index c6388cf..39cd122 100644 --- a/src/module/auto_aim/shooter.cpp +++ b/src/module/auto_aim/shooter.cpp @@ -17,17 +17,17 @@ Shooter::Shooter(const std::string & config_path) : last_command_{false, false, } bool Shooter::shoot( - const io::Command & command, const auto_aim::Aimer & aimer, + const device::Command & command, const auto_aim::Aimer & aimer, const std::list & targets, const Eigen::Vector3d & gimbal_pos) { if (!command.control || targets.empty() || !auto_fire_) return false; auto target_x = targets.front().ekf_x()[0]; auto target_y = targets.front().ekf_x()[2]; - auto tolerance = std::sqrt(tools::square(target_x) + tools::square(target_y)) > judge_distance_ + auto tolerance = std::sqrt(component::square(target_x) + component::square(target_y)) > judge_distance_ ? second_tolerance_ : first_tolerance_; - // tools::logger()->debug("d(command.yaw) is {:.4f}", std::abs(last_command_.yaw - command.yaw)); + // component::logger()->debug("d(command.yaw) is {:.4f}", std::abs(last_command_.yaw - command.yaw)); if ( std::abs(last_command_.yaw - command.yaw) < tolerance * 2 && //此时认为command突变不应该射击 std::abs(gimbal_pos[0] - last_command_.yaw) < tolerance && //应该减去上一次command的yaw值 diff --git a/src/module/auto_aim/shooter.hpp b/src/module/auto_aim/shooter.hpp index 02bb4a9..e9fc1cd 100644 --- a/src/module/auto_aim/shooter.hpp +++ b/src/module/auto_aim/shooter.hpp @@ -14,11 +14,11 @@ public: Shooter(const std::string & config_path); bool shoot( - const io::Command & command, const auto_aim::Aimer & aimer, + const device::Command & command, const auto_aim::Aimer & aimer, const std::list & targets, const Eigen::Vector3d & gimbal_pos); private: - io::Command last_command_; + device::Command last_command_; double judge_distance_; double first_tolerance_; double second_tolerance_; diff --git a/src/module/auto_aim/solver.cpp b/src/module/auto_aim/solver.cpp index 7b2783e..05f75a9 100644 --- a/src/module/auto_aim/solver.cpp +++ b/src/module/auto_aim/solver.cpp @@ -73,10 +73,10 @@ void Solver::solve(Armor & armor) const cv::cv2eigen(rmat, R_armor2camera); Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera; Eigen::Matrix3d R_armor2world = R_gimbal2world_ * R_armor2gimbal; - armor.ypr_in_gimbal = tools::eulers(R_armor2gimbal, 2, 1, 0); - armor.ypr_in_world = tools::eulers(R_armor2world, 2, 1, 0); + armor.ypr_in_gimbal = component::eulers(R_armor2gimbal, 2, 1, 0); + armor.ypr_in_world = component::eulers(R_armor2world, 2, 1, 0); - armor.ypd_in_world = tools::xyz2ypd(armor.xyz_in_world); + armor.ypd_in_world = component::xyz2ypd(armor.xyz_in_world); // 平衡不做yaw优化，因为pitch假设不成立 auto is_balance = (armor.type == ArmorType::big) && @@ -148,10 +148,10 @@ double Solver::oupost_reprojection_error(Armor armor, const double & pitch) cv::cv2eigen(rmat, R_armor2camera); Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera; Eigen::Matrix3d R_armor2world = R_gimbal2world_ * R_armor2gimbal; - armor.ypr_in_gimbal = tools::eulers(R_armor2gimbal, 2, 1, 0); - armor.ypr_in_world = tools::eulers(R_armor2world, 2, 1, 0); + armor.ypr_in_gimbal = component::eulers(R_armor2gimbal, 2, 1, 0); + armor.ypr_in_world = component::eulers(R_armor2world, 2, 1, 0); - armor.ypd_in_world = tools::xyz2ypd(armor.xyz_in_world); + armor.ypd_in_world = component::xyz2ypd(armor.xyz_in_world); auto yaw = armor.ypr_in_world[0]; auto xyz_in_world = armor.xyz_in_world; @@ -195,16 +195,16 @@ double Solver::oupost_reprojection_error(Armor armor, const double & pitch) void Solver::optimize_yaw(Armor & armor) const { - Eigen::Vector3d gimbal_ypr = tools::eulers(R_gimbal2world_, 2, 1, 0); + Eigen::Vector3d gimbal_ypr = component::eulers(R_gimbal2world_, 2, 1, 0); constexpr double SEARCH_RANGE = 140; // degree - auto yaw0 = tools::limit_rad(gimbal_ypr[0] - SEARCH_RANGE / 2 * CV_PI / 180.0); + auto yaw0 = component::limit_rad(gimbal_ypr[0] - SEARCH_RANGE / 2 * CV_PI / 180.0); auto min_error = 1e10; auto best_yaw = armor.ypr_in_world[0]; for (int i = 0; i < SEARCH_RANGE; i++) { - double yaw = tools::limit_rad(yaw0 + i * CV_PI / 180.0); + double yaw = component::limit_rad(yaw0 + i * CV_PI / 180.0); auto error = armor_reprojection_error(armor, yaw, (i - SEARCH_RANGE / 2) * CV_PI / 180.0); if (error < min_error) { @@ -239,12 +239,12 @@ double Solver::SJTU_cost( (0.5 * ((refs[i] - pts[i]).norm() + (refs[p] - pts[p]).norm()) + std::fabs(ref_d.norm() - pt_d.norm())) / ref_d.norm(); - double angular_dis = ref_d.norm() * tools::get_abs_angle(ref_d, pt_d) / ref_d.norm(); + double angular_dis = ref_d.norm() * component::get_abs_angle(ref_d, pt_d) / ref_d.norm(); // 平方可能是为了配合 sin 和 cos // 弧度差代价（0 度左右占比应该大） double cost_i = - tools::square(pixel_dis * std::sin(inclined)) + - tools::square(angular_dis * std::cos(inclined)) * 2.0; // DETECTOR_ERROR_PIXEL_BY_SLOPE + component::square(pixel_dis * std::sin(inclined)) + + component::square(angular_dis * std::cos(inclined)) * 2.0; // DETECTOR_ERROR_PIXEL_BY_SLOPE // 重投影像素误差越大，越相信斜率 cost += std::sqrt(cost_i); } diff --git a/src/module/auto_aim/target.cpp b/src/module/auto_aim/target.cpp index 3debb80..f74c2dc 100644 --- a/src/module/auto_aim/target.cpp +++ b/src/module/auto_aim/target.cpp @@ -42,11 +42,11 @@ Target::Target( // 防止夹角求和出现异常值 auto x_add = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd { Eigen::VectorXd c = a + b; - c[6] = tools::limit_rad(c[6]); + c[6] = component::limit_rad(c[6]); return c; }; - ekf_ = tools::ExtendedKalmanFilter(x0, P0, x_add); //初始化滤波器（预测量、预测量协方差） + ekf_ = component::ExtendedKalmanFilter(x0, P0, x_add); //初始化滤波器（预测量、预测量协方差） } Target::Target(double x, double vyaw, double radius, double h) : armor_num_(4) @@ -58,16 +58,16 @@ Target::Target(double x, double vyaw, double radius, double h) : armor_num_(4) // 防止夹角求和出现异常值 auto x_add = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd { Eigen::VectorXd c = a + b; - c[6] = tools::limit_rad(c[6]); + c[6] = component::limit_rad(c[6]); return c; }; - ekf_ = tools::ExtendedKalmanFilter(x0, P0, x_add); //初始化滤波器（预测量、预测量协方差） + ekf_ = component::ExtendedKalmanFilter(x0, P0, x_add); //初始化滤波器（预测量、预测量协方差） } void Target::predict(std::chrono::steady_clock::time_point t) { - auto dt = tools::delta_time(t, t_); + auto dt = component::delta_time(t, t_); predict(dt); t_ = t; } @@ -124,7 +124,7 @@ void Target::predict(double dt) // 防止夹角求和出现异常值 auto f = [&](const Eigen::VectorXd & x) -> Eigen::VectorXd { Eigen::VectorXd x_prior = F * x; - x_prior[6] = tools::limit_rad(x_prior[6]); + x_prior[6] = component::limit_rad(x_prior[6]); return x_prior; }; @@ -150,17 +150,17 @@ void Target::update(const Armor & armor) std::sort( xyza_i_list.begin(), xyza_i_list.end(), [](const std::pair & a, const std::pair & b) { - Eigen::Vector3d ypd1 = tools::xyz2ypd(a.first.head(3)); - Eigen::Vector3d ypd2 = tools::xyz2ypd(b.first.head(3)); + Eigen::Vector3d ypd1 = component::xyz2ypd(a.first.head(3)); + Eigen::Vector3d ypd2 = component::xyz2ypd(b.first.head(3)); return ypd1[2] < ypd2[2]; }); // 取前3个distance最小的装甲板 for (int i = 0; i < 3; i++) { const auto & xyza = xyza_i_list[i].first; - Eigen::Vector3d ypd = tools::xyz2ypd(xyza.head(3)); - auto angle_error = std::abs(tools::limit_rad(armor.ypr_in_world[0] - xyza[3])) + - std::abs(tools::limit_rad(armor.ypd_in_world[0] - ypd[0])); + Eigen::Vector3d ypd = component::xyz2ypd(xyza.head(3)); + auto angle_error = std::abs(component::limit_rad(armor.ypr_in_world[0] - xyza[3])) + + std::abs(component::limit_rad(armor.ypd_in_world[0] - ypd[0])); if (std::abs(angle_error) < std::abs(min_angle_error)) { id = xyza_i_list[i].second; @@ -190,7 +190,7 @@ void Target::update_ypda(const Armor & armor, int id) Eigen::MatrixXd H = h_jacobian(ekf_.x, id); // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; auto center_yaw = std::atan2(armor.xyz_in_world[1], armor.xyz_in_world[0]); - auto delta_angle = tools::limit_rad(armor.ypr_in_world[0] - center_yaw); + auto delta_angle = component::limit_rad(armor.ypr_in_world[0] - center_yaw); Eigen::VectorXd R_dig{ {4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, log(std::abs(armor.ypd_in_world[2]) + 1) / 200 + 9e-2}}; @@ -201,17 +201,17 @@ void Target::update_ypda(const Armor & armor, int id) // 定义非线性转换函数h: x -> z auto h = [&](const Eigen::VectorXd & x) -> Eigen::Vector4d { Eigen::VectorXd xyz = h_armor_xyz(x, id); - Eigen::VectorXd ypd = tools::xyz2ypd(xyz); - auto angle = tools::limit_rad(x[6] + id * 2 * CV_PI / armor_num_); + Eigen::VectorXd ypd = component::xyz2ypd(xyz); + auto angle = component::limit_rad(x[6] + id * 2 * CV_PI / armor_num_); return {ypd[0], ypd[1], ypd[2], angle}; }; // 防止夹角求差出现异常值 auto z_subtract = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd { Eigen::VectorXd c = a - b; - c[0] = tools::limit_rad(c[0]); - c[1] = tools::limit_rad(c[1]); - c[3] = tools::limit_rad(c[3]); + c[0] = component::limit_rad(c[0]); + c[1] = component::limit_rad(c[1]); + c[3] = component::limit_rad(c[3]); return c; }; @@ -224,14 +224,14 @@ void Target::update_ypda(const Armor & armor, int id) Eigen::VectorXd Target::ekf_x() const { return ekf_.x; } -const tools::ExtendedKalmanFilter & Target::ekf() const { return ekf_; } +const component::ExtendedKalmanFilter & Target::ekf() const { return ekf_; } std::vector Target::armor_xyza_list() const { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { - auto angle = tools::limit_rad(ekf_.x[6] + i * 2 * CV_PI / armor_num_); + auto angle = component::limit_rad(ekf_.x[6] + i * 2 * CV_PI / armor_num_); Eigen::Vector3d xyz = h_armor_xyz(ekf_.x, i); _armor_xyza_list.push_back({xyz[0], xyz[1], xyz[2], angle}); } @@ -245,7 +245,7 @@ bool Target::diverged() const if (r_ok && l_ok) return false; - tools::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); + component::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); return true; } @@ -266,7 +266,7 @@ bool Target::convergened() // 计算出装甲板中心的坐标（考虑长短轴） Eigen::Vector3d Target::h_armor_xyz(const Eigen::VectorXd & x, int id) const { - auto angle = tools::limit_rad(x[6] + id * 2 * CV_PI / armor_num_); + auto angle = component::limit_rad(x[6] + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); auto r = (use_l_h) ? x[8] + x[9] : x[8]; @@ -279,7 +279,7 @@ Eigen::Vector3d Target::h_armor_xyz(const Eigen::VectorXd & x, int id) const Eigen::MatrixXd Target::h_jacobian(const Eigen::VectorXd & x, int id) const { - auto angle = tools::limit_rad(x[6] + id * 2 * CV_PI / armor_num_); + auto angle = component::limit_rad(x[6] + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); auto r = (use_l_h) ? x[8] + x[9] : x[8]; @@ -303,7 +303,7 @@ Eigen::MatrixXd Target::h_jacobian(const Eigen::VectorXd & x, int id) const // clang-format on Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); - Eigen::MatrixXd H_armor_ypd = tools::xyz2ypd_jacobian(armor_xyz); + Eigen::MatrixXd H_armor_ypd = component::xyz2ypd_jacobian(armor_xyz); // clang-format off Eigen::MatrixXd H_armor_ypda{ {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, diff --git a/src/module/auto_aim/target.hpp b/src/module/auto_aim/target.hpp index f3014cc..c7b4a44 100644 --- a/src/module/auto_aim/target.hpp +++ b/src/module/auto_aim/target.hpp @@ -34,7 +34,7 @@ public: void update(const Armor & armor); Eigen::VectorXd ekf_x() const; - const tools::ExtendedKalmanFilter & ekf() const; + const component::ExtendedKalmanFilter & ekf() const; std::vector armor_xyza_list() const; bool diverged() const; @@ -52,7 +52,7 @@ private: bool is_switch_, is_converged_; - tools::ExtendedKalmanFilter ekf_; + component::ExtendedKalmanFilter ekf_; std::chrono::steady_clock::time_point t_; void update_ypda(const Armor & armor, int id); // yaw pitch distance angle diff --git a/src/module/auto_aim/tracker.cpp b/src/module/auto_aim/tracker.cpp index 73b15df..5771819 100644 --- a/src/module/auto_aim/tracker.cpp +++ b/src/module/auto_aim/tracker.cpp @@ -31,12 +31,12 @@ std::string Tracker::state() const { return state_; } std::list Tracker::track( std::list & armors, std::chrono::steady_clock::time_point t, bool use_enemy_color) { - auto dt = tools::delta_time(t, last_timestamp_); + auto dt = component::delta_time(t, last_timestamp_); last_timestamp_ = t; // 时间间隔过长，说明可能发生了相机离线 if (state_ != "lost" && dt > 0.1) { - tools::logger()->warn("[Tracker] Large dt: {:.3f}s", dt); + component::logger()->warn("[Tracker] Large dt: {:.3f}s", dt); state_ = "lost"; } // 过滤掉非我方装甲板 @@ -74,7 +74,7 @@ std::list Tracker::track( // 发散检测 if (state_ != "lost" && target_.diverged()) { - tools::logger()->debug("[Tracker] Target diverged!"); + component::logger()->debug("[Tracker] Target diverged!"); state_ = "lost"; return {}; } @@ -84,7 +84,7 @@ std::list Tracker::track( std::accumulate( target_.ekf().recent_nis_failures.begin(), target_.ekf().recent_nis_failures.end(), 0) >= (0.4 * target_.ekf().window_size)) { - tools::logger()->debug("[Target] Bad Converge Found!"); + component::logger()->debug("[Target] Bad Converge Found!"); state_ = "lost"; return {}; } @@ -105,12 +105,12 @@ std::tuple> Tracker::track( temp_target = detection_queue.front(); } - auto dt = tools::delta_time(t, last_timestamp_); + auto dt = component::delta_time(t, last_timestamp_); last_timestamp_ = t; // 时间间隔过长，说明可能发生了相机离线 if (state_ != "lost" && dt > 0.1) { - tools::logger()->warn("[Tracker] Large dt: {:.3f}s", dt); + component::logger()->warn("[Tracker] Large dt: {:.3f}s", dt); state_ = "lost"; } @@ -133,7 +133,7 @@ std::tuple> Tracker::track( // 此时主相机画面中出现了优先级更高的装甲板，切换目标 else if (state_ == "tracking" && !armors.empty() && armors.front().priority < target_.priority) { found = set_target(armors, t); - tools::logger()->debug("auto_aim switch target to {}", ARMOR_NAMES[armors.front().name]); + component::logger()->debug("auto_aim switch target to {}", ARMOR_NAMES[armors.front().name]); } // 此时全向感知相机画面中出现了优先级更高的装甲板，切换目标 @@ -145,7 +145,7 @@ std::tuple> Tracker::track( temp_target.armors, t, temp_target.delta_yaw, temp_target.delta_pitch}; omni_target_priority_ = temp_target.armors.front().priority; found = false; - tools::logger()->debug("omniperception find higher priority target"); + component::logger()->debug("omniperception find higher priority target"); } else if (state_ == "switching") { @@ -166,7 +166,7 @@ std::tuple> Tracker::track( // 发散检测 if (state_ != "lost" && target_.diverged()) { - tools::logger()->debug("[Tracker] Target diverged!"); + component::logger()->debug("[Tracker] Target diverged!"); state_ = "lost"; return {switch_target, {}}; // 返回switch_target和空的targets } diff --git a/src/module/auto_aim/yolos/yolo11.cpp b/src/module/auto_aim/yolos/yolo11.cpp index 9412e3f..66b49b3 100644 --- a/src/module/auto_aim/yolos/yolo11.cpp +++ b/src/module/auto_aim/yolos/yolo11.cpp @@ -56,7 +56,7 @@ YOLO11::YOLO11(const std::string & config_path, bool debug) std::list YOLO11::detect(const cv::Mat & raw_img, int frame_count) { if (raw_img.empty()) { - tools::logger()->warn("Empty img!, camera drop!"); + component::logger()->warn("Empty img!, camera drop!"); return std::list(); } @@ -238,13 +238,13 @@ void YOLO11::draw_detections( const cv::Mat & img, const std::list & armors, int frame_count) const { auto detection = img.clone(); - tools::draw_text(detection, fmt::format("[{}]", frame_count), {10, 30}, {255, 255, 255}); + component::draw_text(detection, fmt::format("[{}]", frame_count), {10, 30}, {255, 255, 255}); for (const auto & armor : armors) { auto info = fmt::format( "{:.2f} {} {} {}", armor.confidence, COLORS[armor.color], ARMOR_NAMES[armor.name], ARMOR_TYPES[armor.type]); - tools::draw_points(detection, armor.points, {0, 255, 0}); - tools::draw_text(detection, info, armor.center, {0, 255, 0}); + component::draw_points(detection, armor.points, {0, 255, 0}); + component::draw_text(detection, info, armor.center, {0, 255, 0}); } if (use_roi_) { diff --git a/src/module/auto_aim/yolos/yolov5.cpp b/src/module/auto_aim/yolos/yolov5.cpp index 60b3316..74c3c22 100644 --- a/src/module/auto_aim/yolos/yolov5.cpp +++ b/src/module/auto_aim/yolos/yolov5.cpp @@ -57,7 +57,7 @@ YOLOV5::YOLOV5(const std::string & config_path, bool debug) std::list YOLOV5::detect(const cv::Mat & raw_img, int frame_count) { if (raw_img.empty()) { - tools::logger()->warn("Empty img!, camera drop!"); + component::logger()->warn("Empty img!, camera drop!"); return std::list(); } @@ -227,13 +227,13 @@ void YOLOV5::draw_detections( const cv::Mat & img, const std::list & armors, int frame_count) const { auto detection = img.clone(); - tools::draw_text(detection, fmt::format("[{}]", frame_count), {10, 30}, {255, 255, 255}); + component::draw_text(detection, fmt::format("[{}]", frame_count), {10, 30}, {255, 255, 255}); for (const auto & armor : armors) { auto info = fmt::format( "{:.2f} {} {} {}", armor.confidence, COLORS[armor.color], ARMOR_NAMES[armor.name], ARMOR_TYPES[armor.type]); - tools::draw_points(detection, armor.points, {0, 255, 0}); - tools::draw_text(detection, info, armor.center, {0, 255, 0}); + component::draw_points(detection, armor.points, {0, 255, 0}); + component::draw_text(detection, info, armor.center, {0, 255, 0}); } if (use_roi_) { diff --git a/src/module/auto_aim/yolos/yolov8.cpp b/src/module/auto_aim/yolos/yolov8.cpp index c321159..ac3f225 100644 --- a/src/module/auto_aim/yolos/yolov8.cpp +++ b/src/module/auto_aim/yolos/yolov8.cpp @@ -61,7 +61,7 @@ YOLOV8::YOLOV8(const std::string & config_path, bool debug) std::list YOLOV8::detect(const cv::Mat & raw_img, int frame_count) { if (raw_img.empty()) { - tools::logger()->warn("Empty img!, camera drop!"); + component::logger()->warn("Empty img!, camera drop!"); return std::list(); } @@ -277,12 +277,12 @@ void YOLOV8::draw_detections( const cv::Mat & img, const std::list & armors, int frame_count) const { auto detection = img.clone(); - tools::draw_text(detection, fmt::format("[{}]", frame_count), {10, 30}, {255, 255, 255}); + component::draw_text(detection, fmt::format("[{}]", frame_count), {10, 30}, {255, 255, 255}); for (const auto & armor : armors) { auto info = fmt::format( "{:.2f} {} {}", armor.confidence, ARMOR_NAMES[armor.name], ARMOR_TYPES[armor.type]); - tools::draw_points(detection, armor.points, {0, 255, 0}); - tools::draw_text(detection, info, armor.center, {0, 255, 0}); + component::draw_points(detection, armor.points, {0, 255, 0}); + component::draw_text(detection, info, armor.center, {0, 255, 0}); } if (use_roi_) { diff --git a/src/module/auto_aim/yolos/yolov8.hpp b/src/module/auto_aim/yolos/yolov8.hpp index df64a51..50605c1 100644 --- a/src/module/auto_aim/yolos/yolov8.hpp +++ b/src/module/auto_aim/yolos/yolov8.hpp @@ -60,4 +60,4 @@ private: } // namespace auto_aim -#endif // TOOLS__YOLOV8_HPP \ No newline at end of file +#endif // COMPONENT__YOLOV8_HPP \ No newline at end of file diff --git a/src/module/auto_buff/buff_aimer.cpp b/src/module/auto_buff/buff_aimer.cpp index eacd267..6da0130 100644 --- a/src/module/auto_buff/buff_aimer.cpp +++ b/src/module/auto_buff/buff_aimer.cpp @@ -24,7 +24,7 @@ Aimer::Aimer(const std::string & config_path) fire_gap_time_ = buff_cfg["command_fire_gap"].as(); } else { fire_gap_time_ = 0.08; - tools::logger()->warn("[Buff Aimer] fire_gap_time missing, fallback to {}", fire_gap_time_); + component::logger()->warn("[Buff Aimer] fire_gap_time missing, fallback to {}", fire_gap_time_); } if (yaml["predict_time"]) { @@ -33,17 +33,17 @@ Aimer::Aimer(const std::string & config_path) predict_time_ = buff_cfg["predict_time"].as(); } else { predict_time_ = 0.15; - tools::logger()->warn("[Buff Aimer] predict_time missing, fallback to {}", predict_time_); + component::logger()->warn("[Buff Aimer] predict_time missing, fallback to {}", predict_time_); } last_fire_t_ = std::chrono::steady_clock::now(); } -io::Command Aimer::aim( +device::Command Aimer::aim( auto_buff::Target & target, std::chrono::steady_clock::time_point & timestamp, double bullet_speed, bool to_now) { - io::Command command = {false, false, 0, 0}; + device::Command command = {false, false, 0, 0}; if (target.is_unsolve()) return command; // 如果子弹速度小于10，将其设为24 @@ -51,7 +51,7 @@ io::Command Aimer::aim( auto now = std::chrono::steady_clock::now(); - auto detect_now_gap = tools::delta_time(now, timestamp); + auto detect_now_gap = component::delta_time(now, timestamp); auto future = to_now ? (detect_now_gap + predict_time_) : 0.1 + predict_time_; double yaw, pitch; @@ -80,7 +80,7 @@ io::Command Aimer::aim( if (switch_fanblade_) { command.shoot = false; last_fire_t_ = now; - } else if (!switch_fanblade_ && tools::delta_time(now, last_fire_t_) > fire_gap_time_) { + } else if (!switch_fanblade_ && component::delta_time(now, last_fire_t_) > fire_gap_time_) { command.shoot = true; last_fire_t_ = now; } @@ -89,7 +89,7 @@ io::Command Aimer::aim( } auto_aim::Plan Aimer::mpc_aim( - auto_buff::Target & target, std::chrono::steady_clock::time_point & timestamp, io::GimbalState gs, + auto_buff::Target & target, std::chrono::steady_clock::time_point & timestamp, device::GimbalState gs, bool to_now) { auto_aim::Plan plan = {false, false, 0, 0, 0, 0, 0, 0, 0, 0}; @@ -104,7 +104,7 @@ auto_aim::Plan Aimer::mpc_aim( auto now = std::chrono::steady_clock::now(); - auto detect_now_gap = tools::delta_time(now, timestamp); + auto detect_now_gap = component::delta_time(now, timestamp); auto future = to_now ? (detect_now_gap + predict_time_) : 0.1 + predict_time_; double yaw, pitch; @@ -144,16 +144,16 @@ auto_aim::Plan Aimer::mpc_aim( double last_yaw_mpc, last_pitch_mpc; get_send_angle( target, predict_time_ * -1, bullet_speed, to_now, last_yaw_mpc, last_pitch_mpc); - plan.yaw_vel = tools::limit_rad(yaw - last_yaw_mpc) / (2 * dt); - // plan.yaw_vel = tools::limit_min_max(plan.yaw_vel, -6.28, 6.28); - plan.yaw_acc = (tools::limit_rad(yaw - gs.yaw) - tools::limit_rad(gs.yaw - last_yaw_mpc)) / + plan.yaw_vel = component::limit_rad(yaw - last_yaw_mpc) / (2 * dt); + // plan.yaw_vel = component::limit_min_max(plan.yaw_vel, -6.28, 6.28); + plan.yaw_acc = (component::limit_rad(yaw - gs.yaw) - component::limit_rad(gs.yaw - last_yaw_mpc)) / std::pow(dt, 2); - // plan.yaw_acc = tools::limit_min_max(plan.yaw_acc, -50, 50); + // plan.yaw_acc = component::limit_min_max(plan.yaw_acc, -50, 50); - plan.pitch_vel = tools::limit_rad(-pitch + last_pitch_mpc) / (2 * dt); - // plan.pitch_vel = tools::limit_min_max(plan.pitch_vel, -6.28, 6.28); + plan.pitch_vel = component::limit_rad(-pitch + last_pitch_mpc) / (2 * dt); + // plan.pitch_vel = component::limit_min_max(plan.pitch_vel, -6.28, 6.28); plan.pitch_acc = (-pitch - gs.pitch - (gs.pitch + last_pitch_mpc)) / std::pow(dt, 2); - // plan.pitch_acc = tools::limit_min_max(plan.pitch_acc, -100, 100); + // plan.pitch_acc = component::limit_min_max(plan.pitch_acc, -100, 100); } } } @@ -161,7 +161,7 @@ auto_aim::Plan Aimer::mpc_aim( if (switch_fanblade_) { plan.fire = false; last_fire_t_ = now; - } else if (!switch_fanblade_ && tools::delta_time(now, last_fire_t_) > fire_gap_time_) { + } else if (!switch_fanblade_ && component::delta_time(now, last_fire_t_) > fire_gap_time_) { plan.fire = true; last_fire_t_ = now; } @@ -185,9 +185,9 @@ bool Aimer::get_send_angle( double h = aim_in_world[2]; // 创建弹道对象 - tools::Trajectory trajectory0(bullet_speed, d, h); + component::Trajectory trajectory0(bullet_speed, d, h); if (trajectory0.unsolvable) { // 如果弹道无法解算，返回未命中结果 - tools::logger()->debug( + component::logger()->debug( "[Aimer] Unsolvable trajectory0: {:.2f} {:.2f} {:.2f}", bullet_speed, d, h); return false; } @@ -200,9 +200,9 @@ bool Aimer::get_send_angle( aim_in_world = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.7)); d = std::sqrt(aim_in_world[0] * aim_in_world[0] + aim_in_world[1] * aim_in_world[1]); h = aim_in_world[2]; - tools::Trajectory trajectory1(bullet_speed, d, h); + component::Trajectory trajectory1(bullet_speed, d, h); if (trajectory1.unsolvable) { // 如果弹道无法解算，返回未命中结果 - tools::logger()->debug( + component::logger()->debug( "[Aimer] Unsolvable trajectory1: {:.2f} {:.2f} {:.2f}", bullet_speed, d, h); return false; } @@ -210,7 +210,7 @@ bool Aimer::get_send_angle( // 计算时间误差 auto time_error = trajectory1.fly_time - trajectory0.fly_time; if (std::abs(time_error) > 0.01) { // 如果时间误差过大，返回未命中结果 - tools::logger()->debug("[Aimer] Large time error: {:.3f}", time_error); + component::logger()->debug("[Aimer] Large time error: {:.3f}", time_error); return false; } diff --git a/src/module/auto_buff/buff_aimer.hpp b/src/module/auto_buff/buff_aimer.hpp index eac484d..358a3c9 100644 --- a/src/module/auto_buff/buff_aimer.hpp +++ b/src/module/auto_buff/buff_aimer.hpp @@ -21,12 +21,12 @@ class Aimer public: Aimer(const std::string & config_path); - io::Command aim( + device::Command aim( Target & target, std::chrono::steady_clock::time_point & timestamp, double bullet_speed, bool to_now = true); auto_aim::Plan mpc_aim( - Target & target, std::chrono::steady_clock::time_point & timestamp, io::GimbalState gs, + Target & target, std::chrono::steady_clock::time_point & timestamp, device::GimbalState gs, bool to_now = true); double angle; /// diff --git a/src/module/auto_buff/buff_detector.cpp b/src/module/auto_buff/buff_detector.cpp index a8f771f..0416eea 100644 --- a/src/module/auto_buff/buff_detector.cpp +++ b/src/module/auto_buff/buff_detector.cpp @@ -29,7 +29,7 @@ cv::Point2f Buff_Detector::get_r_center(std::vector & fanblades, cv::M /// error if (fanblades.empty()) { - tools::logger()->debug("[Buff_Detector] 无法计算r_center!"); + component::logger()->debug("[Buff_Detector] 无法计算r_center!"); return {0, 0}; } @@ -52,7 +52,7 @@ cv::Point2f Buff_Detector::get_r_center(std::vector & fanblades, cv::M cv::Mat mask = cv::Mat::zeros(dilated_img.size(), CV_8U); // mask circle(mask, r_center_t, radius, cv::Scalar(255), -1); bitwise_and(dilated_img, mask, dilated_img); // 将遮罩应用于二值化图像 - tools::draw_point(bgr_img, r_center_t, {255, 255, 0}, 5); // 调试用 + component::draw_point(bgr_img, r_center_t, {255, 255, 0}, 5); // 调试用 // cv::imshow("Dilated Image", dilated_img); // 调试用 /// 获取轮廓点,矩阵框筛选 TODO diff --git a/src/module/auto_buff/buff_predict.hpp b/src/module/auto_buff/buff_predict.hpp index fd3aea2..4e7514f 100644 --- a/src/module/auto_buff/buff_predict.hpp +++ b/src/module/auto_buff/buff_predict.hpp @@ -28,7 +28,7 @@ protected: Eigen::MatrixXd Q; Eigen::MatrixXd H; Eigen::MatrixXd R; - tools::ExtendedKalmanFilter ekf; + component::ExtendedKalmanFilter ekf; Eigen::VectorXd X_best; double lastangle = 0; double lasttime = 0; @@ -63,7 +63,7 @@ public: // 测量噪声协方差矩阵 //// 调整 R << 0.05; // 创建扩展卡尔曼滤波器对象 - ekf = tools::ExtendedKalmanFilter(x0, P0); + ekf = component::ExtendedKalmanFilter(x0, P0); } virtual void update(double angle, double nowtime) override @@ -118,7 +118,7 @@ public: #ifdef PLOTJUGGLER nlohmann::json json_obj; json_obj["angle"] = X_best[0] * 180 / CV_PI; - tools::Plotter().plot(json_obj); + component::Plotter().plot(json_obj); #endif unsolvable = false; return; @@ -185,7 +185,7 @@ public: // 测量噪声协方差矩阵 //// 调整 R << 0.05; // 创建扩展卡尔曼滤波器对象 - ekf = tools::ExtendedKalmanFilter(x0, P0); + ekf = component::ExtendedKalmanFilter(x0, P0); } virtual void update(double angle, double nowtime) override @@ -257,7 +257,7 @@ public: json_obj["a"] = X_best[2]; json_obj["w"] = X_best[3]; json_obj["theta"] = X_best[4]; - tools::Plotter().plot(json_obj); + component::Plotter().plot(json_obj); #endif } @@ -285,7 +285,7 @@ public: Eigen::MatrixXd Q; Eigen::MatrixXd H; Eigen::MatrixXd R; - tools::ExtendedKalmanFilter ekf; + component::ExtendedKalmanFilter ekf; Eigen::VectorXd X_best; XYZ_predictor() : x0(Eigen::VectorXd(3)), @@ -308,7 +308,7 @@ public: // 测量噪声协方差矩阵 //// 调整 R << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; // 创建扩展卡尔曼滤波器对象 - ekf = tools::ExtendedKalmanFilter(x0, P0); + ekf = component::ExtendedKalmanFilter(x0, P0); } void kalman(Eigen::Vector3d & XYZ) @@ -333,7 +333,7 @@ public: json_obj["x"] = X_best[0]; json_obj["y"] = X_best[1]; json_obj["z"] = X_best[2]; - tools::Plotter().plot(json_obj); + component::Plotter().plot(json_obj); #endif XYZ = X_best; } diff --git a/src/module/auto_buff/buff_solver.cpp b/src/module/auto_buff/buff_solver.cpp index 13c2268..a5baae5 100644 --- a/src/module/auto_buff/buff_solver.cpp +++ b/src/module/auto_buff/buff_solver.cpp @@ -101,12 +101,12 @@ void Solver::solve(std::optional & ps) const Eigen::Matrix3d R_buff2world = R_gimbal2world_ * R_buff2gimbal; p.xyz_in_world = R_gimbal2world_ * xyz_in_gimbal; - p.ypd_in_world = tools::xyz2ypd(p.xyz_in_world); + p.ypd_in_world = component::xyz2ypd(p.xyz_in_world); p.blade_xyz_in_world = R_gimbal2world_ * blade_xyz_in_gimbal; - p.blade_ypd_in_world = tools::xyz2ypd(p.blade_xyz_in_world); + p.blade_ypd_in_world = component::xyz2ypd(p.blade_xyz_in_world); - p.ypr_in_world = tools::eulers(R_buff2world, 2, 1, 0); + p.ypr_in_world = component::eulers(R_buff2world, 2, 1, 0); } // 调试用 @@ -124,7 +124,7 @@ cv::Point2f Solver::point_buff2pixel(cv::Point3f x) std::vector Solver::reproject_buff( const Eigen::Vector3d & xyz_in_world, double yaw, double row) const { - auto R_buff2world = tools::rotation_matrix(Eigen::Vector3d(yaw, 0.0, row)); + auto R_buff2world = component::rotation_matrix(Eigen::Vector3d(yaw, 0.0, row)); // clang-format on // get R_buff2camera t_buff2camera diff --git a/src/module/auto_buff/buff_target.cpp b/src/module/auto_buff/buff_target.cpp index 9da8230..7053940 100644 --- a/src/module/auto_buff/buff_target.cpp +++ b/src/module/auto_buff/buff_target.cpp @@ -25,7 +25,7 @@ Eigen::Vector3d Target::point_buff2world(const Eigen::Vector3d & point_in_buff) { if (unsolvable_) return Eigen::Vector3d(0, 0, 0); Eigen::Matrix3d R_buff2world = - tools::rotation_matrix(Eigen::Vector3d(ekf_.x[4], 0.0, ekf_.x[5])); // pitch = 0 + component::rotation_matrix(Eigen::Vector3d(ekf_.x[4], 0.0, ekf_.x[5])); // pitch = 0 auto R_yaw = ekf_.x[0]; auto R_pitch = ekf_.x[2]; @@ -58,7 +58,7 @@ void SmallTarget::get_target( } static std::chrono::steady_clock::time_point start_timestamp = timestamp; - auto time_gap = tools::delta_time(timestamp, start_timestamp); + auto time_gap = component::delta_time(timestamp, start_timestamp); // init if (first_in_) { @@ -70,7 +70,7 @@ void SmallTarget::get_target( // 处理识别时间间隔过大 if (lost_cn > 6) { unsolvable_ = true; - tools::logger()->debug("[Target] 丢失buff"); + component::logger()->debug("[Target] 丢失buff"); lost_cn = 0; first_in_ = true; return; @@ -83,7 +83,7 @@ void SmallTarget::get_target( // 处理发散 if (std::abs(ekf_.x[6]) > SMALL_W + CV_PI / 18 || std::abs(ekf_.x[6]) < SMALL_W - CV_PI / 18) { unsolvable_ = true; - tools::logger()->debug("[Target] 小符角度发散spd: {:.2f}", ekf_.x[6] * 180 / CV_PI); + component::logger()->debug("[Target] 小符角度发散spd: {:.2f}", ekf_.x[6] * 180 / CV_PI); first_in_ = true; return; } @@ -116,10 +116,10 @@ void SmallTarget::predict(double dt) // clang-format on auto f = [&](const Eigen::VectorXd & x) -> Eigen::VectorXd { Eigen::VectorXd x_prior = A_ * x; - x_prior[0] = tools::limit_rad(x_prior[0]); - x_prior[2] = tools::limit_rad(x_prior[2]); - x_prior[4] = tools::limit_rad(x_prior[4]); - x_prior[5] = tools::limit_rad(x_prior[5]); + x_prior[0] = component::limit_rad(x_prior[0]); + x_prior[2] = component::limit_rad(x_prior[2]); + x_prior[4] = component::limit_rad(x_prior[4]); + x_prior[5] = component::limit_rad(x_prior[5]); return x_prior; }; ekf_.predict(A_, Q_, f); @@ -172,14 +172,14 @@ void SmallTarget::init(double nowtime, const PowerRune & p) // 防止夹角求和出现异常值 auto x_add = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd { Eigen::VectorXd c = a + b; - c[0] = tools::limit_rad(c[0]); - c[2] = tools::limit_rad(c[2]); - c[4] = tools::limit_rad(c[4]); - c[5] = tools::limit_rad(c[5]); + c[0] = component::limit_rad(c[0]); + c[2] = component::limit_rad(c[2]); + c[4] = component::limit_rad(c[4]); + c[5] = component::limit_rad(c[5]); return c; }; // 创建扩展卡尔曼滤波器对象 - ekf_ = tools::ExtendedKalmanFilter(x0_, P0_, x_add); + ekf_ = component::ExtendedKalmanFilter(x0_, P0_, x_add); } void SmallTarget::update(double nowtime, const PowerRune & p) @@ -247,9 +247,9 @@ void SmallTarget::update(double nowtime, const PowerRune & p) // 防止夹角求差出现异常值 auto z_subtract1 = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd { Eigen::VectorXd c = a - b; //4 1 - c[0] = tools::limit_rad(c[0]); - c[1] = tools::limit_rad(c[1]); - c[3] = tools::limit_rad(c[3]); + c[0] = component::limit_rad(c[0]); + c[1] = component::limit_rad(c[1]); + c[3] = component::limit_rad(c[3]); return c; }; @@ -276,18 +276,18 @@ void SmallTarget::update(double nowtime, const PowerRune & p) // 定义非线性转换函数h: x -> z auto h2 = [&](const Eigen::VectorXd & x) -> Eigen::Vector3d { Eigen::VectorXd R_ypd{{x[0], x[2], x[3]}}; - Eigen::VectorXd R_xyz = tools::ypd2xyz(R_ypd); + Eigen::VectorXd R_xyz = component::ypd2xyz(R_ypd); Eigen::VectorXd R_xyz_and_yr{{R_ypd[0], R_ypd[1], R_ypd[2], x[4], x[5]}}; Eigen::VectorXd B_xyz = point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.7)); - Eigen::VectorXd B_ypd = tools::xyz2ypd(B_xyz); + Eigen::VectorXd B_ypd = component::xyz2ypd(B_xyz); return B_ypd; }; // 防止夹角求差出现异常值 auto z_subtract2 = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd { Eigen::VectorXd c = a - b; //6 1 - c[0] = tools::limit_rad(c[0]); - c[1] = tools::limit_rad(c[1]); + c[0] = component::limit_rad(c[0]); + c[1] = component::limit_rad(c[1]); return c; }; @@ -314,7 +314,7 @@ Eigen::MatrixXd SmallTarget::h_jacobian() const };// 5*7 Eigen::VectorXd R_ypd{{ekf_.x[0], ekf_.x[2], ekf_.x[3]}}; - Eigen::MatrixXd H_ypd2xyz = tools::ypd2xyz_jacobian(R_ypd); // 3*3 + Eigen::MatrixXd H_ypd2xyz = component::ypd2xyz_jacobian(R_ypd); // 3*3 Eigen::MatrixXd H1{ {H_ypd2xyz(0, 0), H_ypd2xyz(0, 1), H_ypd2xyz(0, 2), 0.0, 0.0}, {H_ypd2xyz(1, 0), H_ypd2xyz(1, 1), H_ypd2xyz(1, 2), 0.0, 0.0}, @@ -337,17 +337,17 @@ Eigen::MatrixXd SmallTarget::h_jacobian() const };// 3*5 Eigen::VectorXd B_xyz = point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.7)); - Eigen::MatrixXd H3 = tools::xyz2ypd_jacobian(B_xyz);// 3*3 + Eigen::MatrixXd H3 = component::xyz2ypd_jacobian(B_xyz);// 3*3 // clang-format on return H3 * H2 * H1 * H0; // 3*7 // auto h2 = [&](const Eigen::VectorXd & x) -> Eigen::Vector3d { // Eigen::VectorXd R_ypd{{x[0], x[2], x[3]}}; - // Eigen::VectorXd R_xyz = tools::ypd2xyz(R_ypd); + // Eigen::VectorXd R_xyz = component::ypd2xyz(R_ypd); // Eigen::VectorXd R_xyz_and_yr{{R_ypd[0], R_ypd[1], R_ypd[2], x[4], x[5]}}; // Eigen::VectorXd B_xyz = point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.7)); - // Eigen::VectorXd B_ypd = tools::xyz2ypd(B_xyz); + // Eigen::VectorXd B_ypd = component::xyz2ypd(B_xyz); // return B_ypd; // }; } @@ -368,7 +368,7 @@ void BigTarget::get_target( } static std::chrono::steady_clock::time_point start_timestamp = timestamp; - auto time_gap = tools::delta_time(timestamp, start_timestamp); + auto time_gap = component::delta_time(timestamp, start_timestamp); // init if (first_in_) { @@ -380,7 +380,7 @@ void BigTarget::get_target( // 处理识别时间间隔过大 if (lost_cn > 6) { unsolvable_ = true; - tools::logger()->debug("[Target] 丢失buff"); + component::logger()->debug("[Target] 丢失buff"); lost_cn = 0; first_in_ = true; return; @@ -394,7 +394,7 @@ void BigTarget::get_target( if ( ekf_.x[7] > 1.045 * 1.5 || ekf_.x[7] < 0.78 / 1.5 || ekf_.x[8] > 2.0 * 1.5 || ekf_.x[8] < 1.884 / 1.5) { - tools::logger()->debug("[Target] 大符角度发散a: {:.2f}b:{:.2f}", ekf_.x[7], ekf_.x[8]); + component::logger()->debug("[Target] 大符角度发散a: {:.2f}b:{:.2f}", ekf_.x[7], ekf_.x[8]); first_in_ = true; return; } @@ -446,10 +446,10 @@ void BigTarget::predict(double dt) // 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; auto f = [&](const Eigen::VectorXd & x) -> Eigen::VectorXd { Eigen::VectorXd x_prior = x; - x_prior[0] = tools::limit_rad(x_prior[0] + dt * x_prior[1]); - x_prior[2] = tools::limit_rad(x_prior[2]); - x_prior[4] = tools::limit_rad(x_prior[4]); // yaw - x_prior[5] = tools::limit_rad(x_prior[5] + voter.clockwise() * + x_prior[0] = component::limit_rad(x_prior[0] + dt * x_prior[1]); + x_prior[2] = component::limit_rad(x_prior[2]); + x_prior[4] = component::limit_rad(x_prior[4]); // yaw + x_prior[5] = component::limit_rad(x_prior[5] + voter.clockwise() * (-a / w * std::cos(w * t + fi) + a / w * std::cos(w * lasttime_ + fi) + (2.09 - a) * dt)); // roll x_prior[6] = a * sin(w * t + fi) + 2.09 - a; // spd return x_prior; @@ -513,15 +513,15 @@ void BigTarget::init(double nowtime, const PowerRune & p) // 防止夹角求和出现异常值 auto x_add = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd { Eigen::VectorXd c = a + b; - c[0] = tools::limit_rad(c[0]); - c[2] = tools::limit_rad(c[2]); - c[4] = tools::limit_rad(c[4]); - c[5] = tools::limit_rad(c[5]); - c[9] = tools::limit_rad(c[9]); + c[0] = component::limit_rad(c[0]); + c[2] = component::limit_rad(c[2]); + c[4] = component::limit_rad(c[4]); + c[5] = component::limit_rad(c[5]); + c[9] = component::limit_rad(c[9]); return c; }; // 创建扩展卡尔曼滤波器对象 - ekf_ = tools::ExtendedKalmanFilter(x0_, P0_, x_add); + ekf_ = component::ExtendedKalmanFilter(x0_, P0_, x_add); } void BigTarget::update(double nowtime, const PowerRune & p) @@ -593,9 +593,9 @@ void BigTarget::update(double nowtime, const PowerRune & p) // 防止夹角求差出现异常值 auto z_subtract1 = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd { Eigen::VectorXd c = a - b; //4 1 - c[0] = tools::limit_rad(c[0]); - c[1] = tools::limit_rad(c[1]); - c[3] = tools::limit_rad(c[3]); + c[0] = component::limit_rad(c[0]); + c[1] = component::limit_rad(c[1]); + c[3] = component::limit_rad(c[3]); return c; }; @@ -622,18 +622,18 @@ void BigTarget::update(double nowtime, const PowerRune & p) // 定义非线性转换函数h: x -> z auto h2 = [&](const Eigen::VectorXd & x) -> Eigen::Vector3d { Eigen::VectorXd R_ypd{{x[0], x[2], x[3]}}; - Eigen::VectorXd R_xyz = tools::ypd2xyz(R_ypd); + Eigen::VectorXd R_xyz = component::ypd2xyz(R_ypd); Eigen::VectorXd R_xyz_and_yr{{R_ypd[0], R_ypd[1], R_ypd[2], x[4], x[5]}}; Eigen::VectorXd B_xyz = point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.7)); - Eigen::VectorXd B_ypd = tools::xyz2ypd(B_xyz); + Eigen::VectorXd B_ypd = component::xyz2ypd(B_xyz); return B_ypd; }; // 防止夹角求差出现异常值 auto z_subtract2 = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd { Eigen::VectorXd c = a - b; //6 1 - c[0] = tools::limit_rad(c[0]); - c[1] = tools::limit_rad(c[1]); + c[0] = component::limit_rad(c[0]); + c[1] = component::limit_rad(c[1]); return c; }; @@ -673,7 +673,7 @@ Eigen::MatrixXd BigTarget::h_jacobian() const };// 5*7 Eigen::VectorXd R_ypd{{ekf_.x[0], ekf_.x[2], ekf_.x[3]}}; - Eigen::MatrixXd H_ypd2xyz = tools::ypd2xyz_jacobian(R_ypd); // 3*3 + Eigen::MatrixXd H_ypd2xyz = component::ypd2xyz_jacobian(R_ypd); // 3*3 Eigen::MatrixXd H1{ {H_ypd2xyz(0, 0), H_ypd2xyz(0, 1), H_ypd2xyz(0, 2), 0.0, 0.0}, {H_ypd2xyz(1, 0), H_ypd2xyz(1, 1), H_ypd2xyz(1, 2), 0.0, 0.0}, @@ -696,17 +696,17 @@ Eigen::MatrixXd BigTarget::h_jacobian() const };// 3*5 Eigen::VectorXd B_xyz = point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.7)); - Eigen::MatrixXd H3 = tools::xyz2ypd_jacobian(B_xyz);// 3*3 + Eigen::MatrixXd H3 = component::xyz2ypd_jacobian(B_xyz);// 3*3 // clang-format on return H3 * H2 * H1 * H0; // 3*7 // auto h2 = [&](const Eigen::VectorXd & x) -> Eigen::Vector3d { // Eigen::VectorXd R_ypd{{x[0], x[2], x[3]}}; - // Eigen::VectorXd R_xyz = tools::ypd2xyz(R_ypd); + // Eigen::VectorXd R_xyz = component::ypd2xyz(R_ypd); // Eigen::VectorXd R_xyz_and_yr{{R_ypd[0], R_ypd[1], R_ypd[2], x[4], x[5]}}; // Eigen::VectorXd B_xyz = point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.7)); - // Eigen::VectorXd B_ypd = tools::xyz2ypd(B_xyz); + // Eigen::VectorXd B_ypd = component::xyz2ypd(B_xyz); // return B_ypd; // }; } diff --git a/src/module/auto_buff/buff_target.hpp b/src/module/auto_buff/buff_target.hpp index 2cb4c5d..7bd2063 100644 --- a/src/module/auto_buff/buff_target.hpp +++ b/src/module/auto_buff/buff_target.hpp @@ -59,7 +59,7 @@ protected: Eigen::MatrixXd Q_; Eigen::MatrixXd H_; Eigen::MatrixXd R_; - tools::ExtendedKalmanFilter ekf_; + component::ExtendedKalmanFilter ekf_; double lasttime_ = 0; Voter voter; // 逆时针-1 顺时针1 bool first_in_; @@ -108,7 +108,7 @@ private: Eigen::MatrixXd h_jacobian() const; - tools::RansacSineFitter spd_fitter_; + component::RansacSineFitter spd_fitter_; double fit_spd_; }; diff --git a/src/module/auto_buff/buff_type.cpp b/src/module/auto_buff/buff_type.cpp index 3b5b548..9ffeb3f 100644 --- a/src/module/auto_buff/buff_type.cpp +++ b/src/module/auto_buff/buff_type.cpp @@ -65,7 +65,7 @@ PowerRune::PowerRune( } // error else { - tools::logger()->debug("[PowerRune] 识别出错!"); + component::logger()->debug("[PowerRune] 识别出错!"); unsolvable_ = true; return; } diff --git a/src/module/auto_buff/yolo11_buff.cpp b/src/module/auto_buff/yolo11_buff.cpp index 6a8bf17..21cc43b 100644 --- a/src/module/auto_buff/yolo11_buff.cpp +++ b/src/module/auto_buff/yolo11_buff.cpp @@ -15,7 +15,7 @@ YOLO11_BUFF::YOLO11_BUFF(const std::string & config) } else if (yaml["yolo11_model_path"]) { model_path = yaml["yolo11_model_path"].as(); } else { - tools::logger()->warn("[YOLO11_BUFF] No model key in config, fallback to {}", model_path); + component::logger()->warn("[YOLO11_BUFF] No model key in config, fallback to {}", model_path); } model = core.read_model(model_path); // printInputAndOutputsInfo(*model); // 打印模型信息 @@ -37,7 +37,7 @@ std::vector YOLO11_BUFF::get_multicandidateboxes(cv::Mat & // const float factor = fill_tensor_data_image(input_tensor, image); // 填充图片到合适的input size if (image.empty()) { - tools::logger()->warn("Empty img!, camera drop!"); + component::logger()->warn("Empty img!, camera drop!"); return std::vector (); } diff --git a/src/module/omniperception/decider.cpp b/src/module/omniperception/decider.cpp index 6da2cf4..f6992bd 100644 --- a/src/module/omniperception/decider.cpp +++ b/src/module/omniperception/decider.cpp @@ -24,12 +24,12 @@ Decider::Decider(const std::string & config_path) : detector_(config_path), coun mode_ = yaml["mode"].as(); } -io::Command Decider::decide( - auto_aim::YOLO & yolo, const Eigen::Vector3d & gimbal_pos, io::USBCamera & usbcam1, - io::USBCamera & usbcam2, io::Camera & back_camera) +device::Command Decider::decide( + auto_aim::YOLO & yolo, const Eigen::Vector3d & gimbal_pos, device::USBCamera & usbcam1, + device::USBCamera & usbcam2, device::Camera & back_camera) { Eigen::Vector2d delta_angle; - io::USBCamera * cams[] = {&usbcam1, &usbcam2}; + device::USBCamera * cams[] = {&usbcam1, &usbcam2}; cv::Mat usb_img; std::chrono::steady_clock::time_point timestamp; @@ -51,25 +51,25 @@ io::Command Decider::decide( delta_angle = this->delta_angle(armors, cams[count_]->device_name); } - tools::logger()->debug( + component::logger()->debug( "[{} camera] delta yaw:{:.2f},target pitch:{:.2f},armor number:{},armor name:{}", (count_ == 2 ? "back" : cams[count_]->device_name), delta_angle[0], delta_angle[1], armors.size(), auto_aim::ARMOR_NAMES[armors.front().name]); count_ = (count_ + 1) % 3; - return io::Command{ - true, false, tools::limit_rad(gimbal_pos[0] + delta_angle[0] / 57.3), - tools::limit_rad(delta_angle[1] / 57.3)}; + return device::Command{ + true, false, component::limit_rad(gimbal_pos[0] + delta_angle[0] / 57.3), + component::limit_rad(delta_angle[1] / 57.3)}; } count_ = (count_ + 1) % 3; // 如果没有找到目标，返回默认命令 - return io::Command{false, false, 0, 0}; + return device::Command{false, false, 0, 0}; } -io::Command Decider::decide( - auto_aim::YOLO & yolo, const Eigen::Vector3d & gimbal_pos, io::Camera & back_cammera) +device::Command Decider::decide( + auto_aim::YOLO & yolo, const Eigen::Vector3d & gimbal_pos, device::Camera & back_cammera) { cv::Mat img; std::chrono::steady_clock::time_point timestamp; @@ -79,31 +79,31 @@ io::Command Decider::decide( if (!empty) { auto delta_angle = this->delta_angle(armors, "back"); - tools::logger()->debug( + component::logger()->debug( "[back camera] delta yaw:{:.2f},target pitch:{:.2f},armor number:{},armor name:{}", delta_angle[0], delta_angle[1], armors.size(), auto_aim::ARMOR_NAMES[armors.front().name]); - return io::Command{ - true, false, tools::limit_rad(gimbal_pos[0] + delta_angle[0] / 57.3), - tools::limit_rad(delta_angle[1] / 57.3)}; + return device::Command{ + true, false, component::limit_rad(gimbal_pos[0] + delta_angle[0] / 57.3), + component::limit_rad(delta_angle[1] / 57.3)}; } - return io::Command{false, false, 0, 0}; + return device::Command{false, false, 0, 0}; } -io::Command Decider::decide(const std::vector & detection_queue) +device::Command Decider::decide(const std::vector & detection_queue) { if (detection_queue.empty()) { - return io::Command{false, false, 0, 0}; + return device::Command{false, false, 0, 0}; } DetectionResult dr = detection_queue.front(); - if (dr.armors.empty()) return io::Command{false, false, 0, 0}; - tools::logger()->info( + if (dr.armors.empty()) return device::Command{false, false, 0, 0}; + component::logger()->info( "omniperceptron find {},delta yaw is {:.4f}", auto_aim::ARMOR_NAMES[dr.armors.front().name], dr.delta_yaw * 57.3); - return io::Command{true, false, dr.delta_yaw, dr.delta_pitch}; + return device::Command{true, false, dr.delta_yaw, dr.delta_pitch}; }; Eigen::Vector2d Decider::delta_angle( @@ -212,7 +212,7 @@ void Decider::get_invincible_armor(const std::vector & invincible_enemy_ if (invincible_enemy_ids.empty()) return; for (const auto & id : invincible_enemy_ids) { - tools::logger()->info("invincible armor id: {}", id); + component::logger()->info("invincible armor id: {}", id); invincible_armor_.push_back(auto_aim::ArmorName(id - 1)); } } @@ -226,11 +226,11 @@ void Decider::get_auto_aim_target( for (const auto & target : auto_aim_target) { if (target <= 0 || static_cast(target) > auto_aim::ARMOR_NAMES.size()) { - tools::logger()->warn("Received invalid auto_aim target value: {}", int(target)); + component::logger()->warn("Received invalid auto_aim target value: {}", int(target)); continue; } auto_aim_targets.push_back(static_cast(target - 1)); - tools::logger()->info("nav send auto_aim target is {}", auto_aim::ARMOR_NAMES[target - 1]); + component::logger()->info("nav send auto_aim target is {}", auto_aim::ARMOR_NAMES[target - 1]); } if (auto_aim_targets.empty()) return; diff --git a/src/module/omniperception/decider.hpp b/src/module/omniperception/decider.hpp index 4744bc4..3b4e6e1 100644 --- a/src/module/omniperception/decider.hpp +++ b/src/module/omniperception/decider.hpp @@ -21,14 +21,14 @@ class Decider public: Decider(const std::string & config_path); - io::Command decide( - auto_aim::YOLO & yolo, const Eigen::Vector3d & gimbal_pos, io::USBCamera & usbcam1, - io::USBCamera & usbcam2, io::Camera & back_cammera); + device::Command decide( + auto_aim::YOLO & yolo, const Eigen::Vector3d & gimbal_pos, device::USBCamera & usbcam1, + device::USBCamera & usbcam2, device::Camera & back_cammera); - io::Command decide( - auto_aim::YOLO & yolo, const Eigen::Vector3d & gimbal_pos, io::Camera & back_cammera); + device::Command decide( + auto_aim::YOLO & yolo, const Eigen::Vector3d & gimbal_pos, device::Camera & back_cammera); - io::Command decide(const std::vector & detection_queue); + device::Command decide(const std::vector & detection_queue); Eigen::Vector2d delta_angle( const std::list & armors, const std::string & camera); diff --git a/src/module/omniperception/perceptron.cpp b/src/module/omniperception/perceptron.cpp index 57f3c55..3a35211 100644 --- a/src/module/omniperception/perceptron.cpp +++ b/src/module/omniperception/perceptron.cpp @@ -11,8 +11,8 @@ namespace omniperception { Perceptron::Perceptron( - io::USBCamera * usbcam1, io::USBCamera * usbcam2, io::USBCamera * usbcam3, - io::USBCamera * usbcam4, const std::string & config_path) + device::USBCamera * usbcam1, device::USBCamera * usbcam2, device::USBCamera * usbcam3, + device::USBCamera * usbcam4, const std::string & config_path) : detection_queue_(10), decider_(config_path), stop_flag_(false) { // 初始化 YOLO 模型 @@ -28,7 +28,7 @@ Perceptron::Perceptron( threads_.emplace_back([&] { parallel_infer(usbcam3, yolo_parallel3_); }); threads_.emplace_back([&] { parallel_infer(usbcam4, yolo_parallel4_); }); - tools::logger()->info("Perceptron initialized."); + component::logger()->info("Perceptron initialized."); } Perceptron::~Perceptron() @@ -45,7 +45,7 @@ Perceptron::~Perceptron() t.join(); } } - tools::logger()->info("Perceptron destructed."); + component::logger()->info("Perceptron destructed."); } std::vector Perceptron::get_detection_queue() @@ -64,10 +64,10 @@ std::vector Perceptron::get_detection_queue() // 将并行推理逻辑移动到类成员函数 void Perceptron::parallel_infer( - io::USBCamera * cam, std::shared_ptr & yolov8_parallel) + device::USBCamera * cam, std::shared_ptr & yolov8_parallel) { if (!cam) { - tools::logger()->error("Camera pointer is null!"); + component::logger()->error("Camera pointer is null!"); return; } try { @@ -99,7 +99,7 @@ void Perceptron::parallel_infer( } } } catch (const std::exception & e) { - tools::logger()->error("Exception in parallel_infer: {}", e.what()); + component::logger()->error("Exception in parallel_infer: {}", e.what()); } } diff --git a/src/module/omniperception/perceptron.hpp b/src/module/omniperception/perceptron.hpp index 6f19692..bc909a2 100644 --- a/src/module/omniperception/perceptron.hpp +++ b/src/module/omniperception/perceptron.hpp @@ -19,18 +19,18 @@ class Perceptron { public: Perceptron( - io::USBCamera * usbcma1, io::USBCamera * usbcam2, io::USBCamera * usbcam3, - io::USBCamera * usbcam4, const std::string & config_path); + device::USBCamera * usbcma1, device::USBCamera * usbcam2, device::USBCamera * usbcam3, + device::USBCamera * usbcam4, const std::string & config_path); ~Perceptron(); std::vector get_detection_queue(); - void parallel_infer(io::USBCamera * cam, std::shared_ptr & yolo_parallel); + void parallel_infer(device::USBCamera * cam, std::shared_ptr & yolo_parallel); private: std::vector threads_; - tools::ThreadSafeQueue detection_queue_; + component::ThreadSafeQueue detection_queue_; std::shared_ptr yolo_parallel1_; std::shared_ptr yolo_parallel2_; diff --git a/src/task/auto_aim_test.cpp b/src/task/auto_aim_test.cpp index 49a4c15..ee20a3a 100644 --- a/src/task/auto_aim_test.cpp +++ b/src/task/auto_aim_test.cpp @@ -35,8 +35,8 @@ int main(int argc, char * argv[]) auto start_index = cli.get("start-index"); auto end_index = cli.get("end-index"); - tools::Plotter plotter; - tools::Exiter exiter; + component::Plotter plotter; + component::Exiter exiter; auto video_path = fmt::format("{}.avi", input_path); auto text_path = fmt::format("{}.txt", input_path); @@ -52,7 +52,7 @@ int main(int argc, char * argv[]) auto t0 = std::chrono::steady_clock::now(); auto_aim::Target last_target; - io::Command last_command; + device::Command last_command; double last_t = -1; video.set(cv::CAP_PROP_POS_FRAMES, start_index); @@ -93,13 +93,13 @@ int main(int argc, char * argv[]) /// 调试输出 auto finish = std::chrono::steady_clock::now(); - tools::logger()->info( + component::logger()->info( "[{}] yolo: {:.1f}ms, tracker: {:.1f}ms, aimer: {:.1f}ms", frame_count, - tools::delta_time(tracker_start, yolo_start) * 1e3, - tools::delta_time(aimer_start, tracker_start) * 1e3, - tools::delta_time(finish, aimer_start) * 1e3); + component::delta_time(tracker_start, yolo_start) * 1e3, + component::delta_time(aimer_start, tracker_start) * 1e3, + component::delta_time(finish, aimer_start) * 1e3); - tools::draw_text( + component::draw_text( img, fmt::format( "command is {},{:.2f},{:.2f},shoot:{}", command.control, command.yaw * 57.3, @@ -107,10 +107,10 @@ int main(int argc, char * argv[]) {10, 60}, {154, 50, 205}); Eigen::Quaternion gimbal_q = {w, x, y, z}; - tools::draw_text( + component::draw_text( img, fmt::format( - "gimbal yaw{:.2f}", (tools::eulers(gimbal_q.toRotationMatrix(), 2, 1, 0) * 57.3)[0]), + "gimbal yaw{:.2f}", (component::eulers(gimbal_q.toRotationMatrix(), 2, 1, 0) * 57.3)[0]), {10, 90}, {255, 255, 255}); nlohmann::json data; @@ -128,7 +128,7 @@ int main(int argc, char * argv[]) } Eigen::Quaternion q{w, x, y, z}; - auto yaw = tools::eulers(q, 2, 1, 0)[0]; + auto yaw = component::eulers(q, 2, 1, 0)[0]; data["gimbal_yaw"] = yaw * 57.3; data["cmd_yaw"] = command.yaw * 57.3; data["shoot"] = command.shoot; @@ -149,7 +149,7 @@ int main(int argc, char * argv[]) for (const Eigen::Vector4d & xyza : armor_xyza_list) { auto image_points = solver.reproject_armor(xyza.head(3), xyza[3], target.armor_type, target.name); - tools::draw_points(img, image_points, {0, 255, 0}); + component::draw_points(img, image_points, {0, 255, 0}); } // aimer瞄准位置 @@ -157,7 +157,7 @@ int main(int argc, char * argv[]) Eigen::Vector4d aim_xyza = aim_point.xyza; auto image_points = solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name); - if (aim_point.valid) tools::draw_points(img, image_points, {0, 0, 255}); + if (aim_point.valid) component::draw_points(img, image_points, {0, 0, 255}); // 观测器内部数据 Eigen::VectorXd x = target.ekf_x(); diff --git a/src/task/auto_buff_test.cpp b/src/task/auto_buff_test.cpp index 5a09d60..fec1ec6 100644 --- a/src/task/auto_buff_test.cpp +++ b/src/task/auto_buff_test.cpp @@ -36,8 +36,8 @@ int main(int argc, char * argv[]) auto start_index = cli.get("start-index"); auto end_index = cli.get("end-index"); - tools::Plotter plotter; - tools::Exiter exiter; + component::Plotter plotter; + component::Exiter exiter; auto video_path = fmt::format("{}.avi", input_path); auto text_path = fmt::format("{}.txt", input_path); @@ -53,7 +53,7 @@ int main(int argc, char * argv[]) cv::Mat img, drawing; auto t0 = std::chrono::steady_clock::now(); - io::Command last_command; + device::Command last_command; double last_t = -1; video.set(cv::CAP_PROP_POS_FRAMES, start_index); @@ -109,17 +109,17 @@ int main(int argc, char * argv[]) auto & p = power_runes.value(); // 显示 - for (int i = 0; i < 4; i++) tools::draw_point(img, p.target().points[i]); - tools::draw_point(img, p.target().center, {0, 0, 255}, 3); - tools::draw_point(img, p.r_center, {0, 0, 255}, 3); + for (int i = 0; i < 4; i++) component::draw_point(img, p.target().points[i]); + component::draw_point(img, p.target().center, {0, 0, 255}, 3); + component::draw_point(img, p.r_center, {0, 0, 255}, 3); // 当前帧target更新后buff auto Rxyz_in_world_now = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0)); auto image_points = solver.reproject_buff(Rxyz_in_world_now, target.ekf_x()[4], target.ekf_x()[5]); - tools::draw_points( + component::draw_points( img, std::vector(image_points.begin(), image_points.begin() + 4), {0, 255, 0}); - tools::draw_points( + component::draw_points( img, std::vector(image_points.begin() + 4, image_points.end()), {0, 255, 0}); // buff瞄准位置(预测) @@ -127,9 +127,9 @@ int main(int argc, char * argv[]) auto Rxyz_in_world_pre = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0)); image_points = solver.reproject_buff(Rxyz_in_world_pre, target_copy.ekf_x()[4], target_copy.ekf_x()[5]); - tools::draw_points( + component::draw_points( img, std::vector(image_points.begin(), image_points.begin() + 4), {255, 0, 0}); - tools::draw_points( + component::draw_points( img, std::vector(image_points.begin() + 4, image_points.end()), {255, 0, 0}); // 观测器内部数据 @@ -152,7 +152,7 @@ int main(int argc, char * argv[]) } // 云台响应情况 - Eigen::Vector3d ypr = tools::eulers(solver.R_gimbal2world(), 2, 1, 0); + Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0); data["gimbal_yaw"] = ypr[0] * 57.3; data["gimbal_pitch"] = -ypr[1] * 57.3; diff --git a/src/task/camera_detect_test.cpp b/src/task/camera_detect_test.cpp index a8b78ba..25bb0f7 100644 --- a/src/task/camera_detect_test.cpp +++ b/src/task/camera_detect_test.cpp @@ -26,9 +26,9 @@ int main(int argc, char * argv[]) auto config_path = cli.get(0); auto use_tradition = cli.get("tradition"); - tools::Exiter exiter; + component::Exiter exiter; - io::Camera camera(config_path); + device::Camera camera(config_path); auto_aim::Detector detector(config_path, true); auto_aim::YOLO yolo(config_path, true); @@ -50,8 +50,8 @@ int main(int argc, char * argv[]) armors = yolo.detect(img); auto now = std::chrono::steady_clock::now(); - auto dt = tools::delta_time(now, last); - tools::logger()->info("{:.2f} fps", 1 / dt); + auto dt = component::delta_time(now, last); + component::logger()->info("{:.2f} fps", 1 / dt); auto key = cv::waitKey(33); if (key == 'q') break; diff --git a/src/task/camera_test.cpp b/src/task/camera_test.cpp index 4748c83..2f974ea 100644 --- a/src/task/camera_test.cpp +++ b/src/task/camera_test.cpp @@ -19,11 +19,11 @@ int main(int argc, char * argv[]) return 0; } - tools::Exiter exiter; + component::Exiter exiter; auto config_path = cli.get("config-path"); auto display = cli.has("display"); - io::Camera camera(config_path); + device::Camera camera(config_path); cv::Mat img; std::chrono::steady_clock::time_point timestamp; @@ -31,10 +31,10 @@ int main(int argc, char * argv[]) while (!exiter.exit()) { camera.read(img, timestamp); - auto dt = tools::delta_time(timestamp, last_stamp); + auto dt = component::delta_time(timestamp, last_stamp); last_stamp = timestamp; - tools::logger()->info("{:.2f} fps", 1 / dt); + component::logger()->info("{:.2f} fps", 1 / dt); if (!display) continue; cv::imshow("img", img); diff --git a/src/task/camera_thread_test.cpp b/src/task/camera_thread_test.cpp index 9dfb56b..7735192 100644 --- a/src/task/camera_thread_test.cpp +++ b/src/task/camera_thread_test.cpp @@ -26,10 +26,10 @@ const std::string keys = "{help h usage ? | | 输出命令行参数说明}" "{@config-path | configs/ascento.yaml | 位置参数，yaml配置文件路径 }"; -tools::OrderedQueue frame_queue; +component::OrderedQueue frame_queue; // 处理detect任务的线程函数 -void detect_frame(tools::Frame && frame, auto_aim::YOLO & yolo) +void detect_frame(component::Frame && frame, auto_aim::YOLO & yolo) { frame.armors = yolo.detect(frame.img); frame_queue.enqueue(frame); @@ -37,9 +37,9 @@ void detect_frame(tools::Frame && frame, auto_aim::YOLO & yolo) int main(int argc, char * argv[]) { - tools::Exiter exiter; - tools::Plotter plotter; - // tools::Recorder recorder(100); + component::Exiter exiter; + component::Plotter plotter; + // component::Recorder recorder(100); cv::CommandLineParser cli(argc, argv, keys); auto config_path = cli.get(0); @@ -50,7 +50,7 @@ int main(int argc, char * argv[]) // 处理线程函数 auto process_thread = std::thread([&]() { - tools::Frame process_frame; + component::Frame process_frame; while (!exiter.exit()) { process_frame = frame_queue.dequeue(); auto img = process_frame.img; @@ -66,12 +66,12 @@ int main(int argc, char * argv[]) } }); - io::Camera camera(config_path); + device::Camera camera(config_path); int num_yolo_thread = 8; - auto yolos = tools::create_yolov8s(config_path, num_yolo_thread, true); - // auto yolos = tools::create_yolo11s(config_path, num_yolo_thread, true); + auto yolos = component::create_yolov8s(config_path, num_yolo_thread, true); + // auto yolos = component::create_yolo11s(config_path, num_yolo_thread, true); std::vector yolo_used(num_yolo_thread, false); - tools::ThreadPool thread_pool(num_yolo_thread); + component::ThreadPool thread_pool(num_yolo_thread); cv::Mat img; Eigen::Quaterniond q; @@ -82,11 +82,11 @@ int main(int argc, char * argv[]) while (!exiter.exit()) { camera.read(img, t); - auto dt = tools::delta_time(t, last_t); + auto dt = component::delta_time(t, last_t); last_t = t; - // tools::logger()->info("{:.2f} fps", 1 / dt); - // tools::draw_text(img, fmt::format("{:.2f} fps", 1/dt), {10, 60}, {255, 255, 255}); + // component::logger()->info("{:.2f} fps", 1 / dt); + // component::draw_text(img, fmt::format("{:.2f} fps", 1/dt), {10, 60}, {255, 255, 255}); nlohmann::json data; data["fps"] = 1 / dt; @@ -106,7 +106,7 @@ int main(int argc, char * argv[]) } } if (yolo) { - tools::Frame frame{frame_id, img.clone(), t}; + component::Frame frame{frame_id, img.clone(), t}; detect_frame(std::move(frame), *yolo); yolo_used[yolo_id] = false; diff --git a/src/task/cboard_test.cpp b/src/task/cboard_test.cpp index edac622..7a78510 100644 --- a/src/task/cboard_test.cpp +++ b/src/task/cboard_test.cpp @@ -23,9 +23,9 @@ int main(int argc, char * argv[]) } auto config_path = cli.get(0); - tools::Exiter exiter; + component::Exiter exiter; - io::CBoard cboard(config_path); + device::CBoard cboard(config_path); while (!exiter.exit()) { auto timestamp = std::chrono::steady_clock::now(); @@ -34,9 +34,9 @@ int main(int argc, char * argv[]) Eigen::Quaterniond q = cboard.imu_at(timestamp); - Eigen::Vector3d eulers = tools::eulers(q, 2, 1, 0) * 57.3; - tools::logger()->info("z{:.2f} y{:.2f} x{:.2f} degree", eulers[0], eulers[1], eulers[2]); - tools::logger()->info("bullet speed {:.2f} m/s", cboard.bullet_speed); + Eigen::Vector3d eulers = component::eulers(q, 2, 1, 0) * 57.3; + component::logger()->info("z{:.2f} y{:.2f} x{:.2f} degree", eulers[0], eulers[1], eulers[2]); + component::logger()->info("bullet speed {:.2f} m/s", cboard.bullet_speed); } return 0; diff --git a/src/task/detector_video_test.cpp b/src/task/detector_video_test.cpp index 0099764..ad49298 100644 --- a/src/task/detector_video_test.cpp +++ b/src/task/detector_video_test.cpp @@ -31,8 +31,8 @@ int main(int argc, char * argv[]) auto end_index = cli.get("end-index"); auto use_tradition = cli.get("tradition"); - tools::Exiter exiter; - tools::Plotter plotter; + component::Exiter exiter; + component::Plotter plotter; cv::VideoCapture video(video_path); diff --git a/src/task/dm_test.cpp b/src/task/dm_test.cpp index 42f8a8c..bec0c1f 100644 --- a/src/task/dm_test.cpp +++ b/src/task/dm_test.cpp @@ -10,8 +10,8 @@ using namespace std::chrono_literals; int main() { - tools::Exiter exiter; - io::DM_IMU imu; + component::Exiter exiter; + device::DM_IMU imu; while (!exiter.exit()) { auto timestamp = std::chrono::steady_clock::now(); @@ -20,8 +20,8 @@ int main() Eigen::Quaterniond q = imu.imu_at(timestamp); - Eigen::Vector3d eulers = tools::eulers(q, 2, 1, 0) * 57.3; - tools::logger()->info("z{:.2f} y{:.2f} x{:.2f} degree", eulers[0], eulers[1], eulers[2]); + Eigen::Vector3d eulers = component::eulers(q, 2, 1, 0) * 57.3; + component::logger()->info("z{:.2f} y{:.2f} x{:.2f} degree", eulers[0], eulers[1], eulers[2]); } return 0; diff --git a/src/task/fire_test.cpp b/src/task/fire_test.cpp index 9f606f7..73f907b 100644 --- a/src/task/fire_test.cpp +++ b/src/task/fire_test.cpp @@ -25,13 +25,13 @@ int main(int argc, char * argv[]) } // 初始化绘图器、录制器、退出器 - tools::Plotter plotter; - tools::Recorder recorder; - tools::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + component::Exiter exiter; // 初始化云台 - io::Gimbal gimbal(config_path); - io::VisionToGimbal plan; + device::Gimbal gimbal(config_path); + device::VisionToGimbal plan; auto last_t = std::chrono::steady_clock::now(); plan.yaw = 0; plan.yaw_vel = 0; @@ -43,9 +43,9 @@ int main(int argc, char * argv[]) while (!exiter.exit()) { auto now = std::chrono::steady_clock::now(); auto gs = gimbal.state(); - if(tools::delta_time(now, last_t) > 1.600) { + if(component::delta_time(now, last_t) > 1.600) { plan.mode = 2; - tools::logger()->debug("fire!"); + component::logger()->debug("fire!"); last_t = now; } else plan.mode = 1; diff --git a/src/task/gimbal_response_test.cpp b/src/task/gimbal_response_test.cpp index f15e16f..a1dd5f0 100644 --- a/src/task/gimbal_response_test.cpp +++ b/src/task/gimbal_response_test.cpp @@ -49,10 +49,10 @@ int main(int argc, char * argv[]) return 0; } - tools::Exiter exiter; - tools::Plotter plotter; + component::Exiter exiter; + component::Plotter plotter; - io::CBoard cboard(config_path); + device::CBoard cboard(config_path); auto init_angle = 0; double slice = circle * 100; //切片数=周期*帧率 @@ -64,12 +64,12 @@ int main(int argc, char * argv[]) double error = 0; int count = 0; - io::Command init_command{1, 0, 0, 0}; + device::Command init_command{1, 0, 0, 0}; cboard.send(init_command); std::this_thread::sleep_for(5s); //等待云台归零 - io::Command command{0}; - io::Command last_command{0}; + device::Command command{0}; + device::Command last_command{0}; double t = 0; auto last_t = t; @@ -85,7 +85,7 @@ int main(int argc, char * argv[]) Eigen::Quaterniond q = cboard.imu_at(timestamp); - Eigen::Vector3d eulers = tools::eulers(q, 2, 1, 0); + Eigen::Vector3d eulers = component::eulers(q, 2, 1, 0); if (signal_mode == "triangle_wave") { if (count == slice) { @@ -116,7 +116,7 @@ int main(int argc, char * argv[]) data["last_cmd_pitch"] = last_command.pitch * 57.3; data["gimbal_pitch"] = eulers[1] * 57.3; } - data["t"] = tools::delta_time(std::chrono::steady_clock::now(), t0); + data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0); last_command = command; plotter.plot(data); std::this_thread::sleep_for(8ms); //模拟自瞄100fps @@ -127,7 +127,7 @@ int main(int argc, char * argv[]) cmd_angle += delta_angle; count = 0; } - command = {1, 0, tools::limit_rad(cmd_angle / 57.3), 0}; + command = {1, 0, component::limit_rad(cmd_angle / 57.3), 0}; count++; cboard.send(command); diff --git a/src/task/gimbal_test.cpp b/src/task/gimbal_test.cpp index fc7ba84..fbcb15e 100644 --- a/src/task/gimbal_test.cpp +++ b/src/task/gimbal_test.cpp @@ -26,10 +26,10 @@ int main(int argc, char * argv[]) return 0; } - tools::Exiter exiter; - tools::Plotter plotter; + component::Exiter exiter; + component::Plotter plotter; - io::Gimbal gimbal(config_path); + device::Gimbal gimbal(config_path); auto t0 = std::chrono::steady_clock::now(); auto last_mode = gimbal.mode(); @@ -44,21 +44,21 @@ int main(int argc, char * argv[]) auto mode = gimbal.mode(); if (mode != last_mode) { - tools::logger()->info("Gimbal mode changed: {}", gimbal.str(mode)); + component::logger()->info("Gimbal mode changed: {}", gimbal.str(mode)); last_mode = mode; } auto t = std::chrono::steady_clock::now(); auto state = gimbal.state(); auto q = gimbal.q(t); - auto ypr = tools::eulers(q, 2, 1, 0); + auto ypr = component::eulers(q, 2, 1, 0); auto fired = state.bullet_count > last_bullet_count; last_bullet_count = state.bullet_count; if (!first_fired && fired) { first_fired = true; - tools::logger()->info("Gimbal first fired after: {:.3f}s", tools::delta_time(t, fire_stamp)); + component::logger()->info("Gimbal first fired after: {:.3f}s", component::delta_time(t, fire_stamp)); } if (fire && fire_count > 20) { @@ -87,7 +87,7 @@ int main(int argc, char * argv[]) data["bullet_count"] = state.bullet_count; data["fired"] = fired ? 1 : 0; data["fire"] = test_fire && fire ? 1 : 0; - data["t"] = tools::delta_time(t, t0); + data["t"] = component::delta_time(t, t0); plotter.plot(data); std::this_thread::sleep_for(9ms); diff --git a/src/task/handeye_test.cpp b/src/task/handeye_test.cpp index ec12262..f58b12d 100644 --- a/src/task/handeye_test.cpp +++ b/src/task/handeye_test.cpp @@ -30,7 +30,7 @@ int main(int argc, char * argv[]) return 0; } - tools::Exiter exiter; + component::Exiter exiter; auto config_path = cli.get("config-path"); auto display = cli.has("display"); @@ -39,8 +39,8 @@ int main(int argc, char * argv[]) auto grid_num = yaml["grid_num"].as(); auto grid_size = yaml["grid_size"].as(); auto delay = yaml["delay"].as(); - io::CBoard cboard(config_path); - io::Camera camera(config_path); + device::CBoard cboard(config_path); + device::Camera camera(config_path); auto_aim::Solver solver(config_path); cv::Mat img; @@ -65,9 +65,9 @@ int main(int argc, char * argv[]) } } Eigen::Vector3d euler = solver.R_gimbal2world().eulerAngles(2, 1, 0) * 180.0 / M_PI; - tools::draw_text(result, fmt::format("yaw {:.2f}", euler[0]), {40, 40}, {0, 0, 255}); - tools::draw_text(result, fmt::format("pitch {:.2f}", euler[1]), {40, 80}, {0, 0, 255}); - tools::draw_text(result, fmt::format("roll {:.2f}", euler[2]), {40, 120}, {0, 0, 255}); + component::draw_text(result, fmt::format("yaw {:.2f}", euler[0]), {40, 40}, {0, 0, 255}); + component::draw_text(result, fmt::format("pitch {:.2f}", euler[1]), {40, 80}, {0, 0, 255}); + component::draw_text(result, fmt::format("roll {:.2f}", euler[2]), {40, 120}, {0, 0, 255}); if (!display) continue; cv::imshow("result", result); if (cv::waitKey(1) == 'q') break; diff --git a/src/task/minimum_vision_system.cpp b/src/task/minimum_vision_system.cpp index ea87533..901cf0b 100644 --- a/src/task/minimum_vision_system.cpp +++ b/src/task/minimum_vision_system.cpp @@ -29,10 +29,10 @@ int main(int argc, char * argv[]) auto config_path = cli.get("@config-path"); - tools::Exiter exiter; - tools::Plotter plotter; - io::Camera camera(config_path); - io::DM_IMU dm_imu; + component::Exiter exiter; + component::Plotter plotter; + device::Camera camera(config_path); + device::DM_IMU dm_imu; auto_aim::multithread::MultiThreadDetector detector(config_path); auto_aim::Solver solver(config_path); @@ -60,7 +60,7 @@ int main(int argc, char * argv[]) solver.set_R_gimbal2world(q); - Eigen::Vector3d gimbal_pos = tools::eulers(solver.R_gimbal2world(), 2, 1, 0); + Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0); auto targets = tracker.track(armors, t); @@ -68,7 +68,7 @@ int main(int argc, char * argv[]) shooter.shoot(command, aimer, targets, gimbal_pos); - auto dt = tools::delta_time(t, last_t); + auto dt = component::delta_time(t, last_t); last_t = t; data["dt"] = dt; @@ -94,14 +94,14 @@ int main(int argc, char * argv[]) if (!targets.empty()) { auto target = targets.front(); - tools::draw_text(img, fmt::format("[{}]", tracker.state()), {10, 30}, {255, 255, 255}); + component::draw_text(img, fmt::format("[{}]", tracker.state()), {10, 30}, {255, 255, 255}); // 当前帧target更新后 std::vector armor_xyza_list = target.armor_xyza_list(); for (const Eigen::Vector4d & xyza : armor_xyza_list) { auto image_points = solver.reproject_armor(xyza.head(3), xyza[3], target.armor_type, target.name); - tools::draw_points(img, image_points, {0, 255, 0}); + component::draw_points(img, image_points, {0, 255, 0}); } // aimer瞄准位置 @@ -110,9 +110,9 @@ int main(int argc, char * argv[]) auto image_points = solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name); if (aim_point.valid) - tools::draw_points(img, image_points, {0, 0, 255}); // red + component::draw_points(img, image_points, {0, 0, 255}); // red else - tools::draw_points(img, image_points, {255, 0, 0}); // blue + component::draw_points(img, image_points, {255, 0, 0}); // blue // 观测器内部数据 Eigen::VectorXd x = target.ekf_x(); diff --git a/src/task/multi_usbcamera_test.cpp b/src/task/multi_usbcamera_test.cpp index ff053cf..cf8ecab 100644 --- a/src/task/multi_usbcamera_test.cpp +++ b/src/task/multi_usbcamera_test.cpp @@ -21,14 +21,14 @@ int main(int argc, char * argv[]) cli.printMessage(); return 0; } - tools::Exiter exiter; + component::Exiter exiter; auto config_path = cli.get(0); auto display = cli.has("display"); - io::USBCamera usbcam1("video0", config_path); - io::USBCamera usbcam2("video2", config_path); - io::Camera camera("configs/camera.yaml"); + device::USBCamera usbcam1("video0", config_path); + device::USBCamera usbcam2("video2", config_path); + device::Camera camera("configs/camera.yaml"); cv::Mat img1, img2, img3; std::chrono::steady_clock::time_point timestamp; @@ -38,10 +38,10 @@ int main(int argc, char * argv[]) usbcam2.read(img2, timestamp); camera.read(img3, timestamp); - auto dt = tools::delta_time(timestamp, last_stamp); + auto dt = component::delta_time(timestamp, last_stamp); last_stamp = timestamp; - tools::logger()->info("{:.2f} fps", 1 / dt); + component::logger()->info("{:.2f} fps", 1 / dt); if (!display) continue; cv::imshow("img1", img1); diff --git a/src/task/planner_test.cpp b/src/task/planner_test.cpp index b8f4ee7..01580e9 100644 --- a/src/task/planner_test.cpp +++ b/src/task/planner_test.cpp @@ -30,10 +30,10 @@ int main(int argc, char * argv[]) return 0; } - tools::Exiter exiter; - tools::Plotter plotter; + component::Exiter exiter; + component::Plotter plotter; - io::Gimbal gimbal(config_path); + device::Gimbal gimbal(config_path); auto_aim::Planner planner(config_path); auto_aim::Target target(d, w, 0.2, 0.1); @@ -50,7 +50,7 @@ int main(int argc, char * argv[]) plan.pitch_acc); nlohmann::json data; - data["t"] = tools::delta_time(std::chrono::steady_clock::now(), t0); + data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0); data["gimbal_yaw"] = gs.yaw; data["gimbal_yaw_vel"] = gs.yaw_vel; diff --git a/src/task/planner_test_offline.cpp b/src/task/planner_test_offline.cpp index 47fcc64..e8f6505 100644 --- a/src/task/planner_test_offline.cpp +++ b/src/task/planner_test_offline.cpp @@ -28,8 +28,8 @@ int main(int argc, char * argv[]) return 0; } - tools::Exiter exiter; - tools::Plotter plotter; + component::Exiter exiter; + component::Plotter plotter; auto_aim::Planner planner(config_path); auto_aim::Target target(d, w, 0.2, 0.1); @@ -42,7 +42,7 @@ int main(int argc, char * argv[]) auto plan = planner.plan(target, 22); nlohmann::json data; - data["t"] = tools::delta_time(std::chrono::steady_clock::now(), t0); + data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0); data["target_yaw"] = plan.target_yaw; data["target_pitch"] = plan.target_pitch; diff --git a/src/task/publish_test.cpp b/src/task/publish_test.cpp index 1aa112c..5157b66 100644 --- a/src/task/publish_test.cpp +++ b/src/task/publish_test.cpp @@ -8,8 +8,8 @@ int main(int argc, char ** argv) { - tools::Exiter exiter; - io::ROS2 ros2; + component::Exiter exiter; + device::ROS2 ros2; double i = 0; while (!exiter.exit()) { diff --git a/src/task/subscribe_test.cpp b/src/task/subscribe_test.cpp index 883c163..df40998 100644 --- a/src/task/subscribe_test.cpp +++ b/src/task/subscribe_test.cpp @@ -7,15 +7,15 @@ int main(int argc, char ** argv) { - tools::Exiter exiter; - io::ROS2 ros2; + component::Exiter exiter; + device::ROS2 ros2; int i = 0; while (!exiter.exit()) { auto x = ros2.subscribe_enemy_status(); - // tools::logger()->info("invincible enemy ids size is{}", x.size()); + // component::logger()->info("invincible enemy ids size is{}", x.size()); for (const auto & id : x) { - tools::logger()->info("id:{}", id); + component::logger()->info("id:{}", id); } // i++; diff --git a/src/task/topic_loop_test.cpp b/src/task/topic_loop_test.cpp index 2a718de..d469822 100644 --- a/src/task/topic_loop_test.cpp +++ b/src/task/topic_loop_test.cpp @@ -7,8 +7,8 @@ int main(int argc, char ** argv) { - tools::Exiter exiter; - io::ROS2 ros2; + component::Exiter exiter; + device::ROS2 ros2; rclcpp::Clock clock; auto string_publisher = ros2.create_publisher("temp_node", "enemy_status", 10); @@ -28,7 +28,7 @@ int main(int argc, char ** argv) if (i % 3 == 0) { auto x = ros2.subscribe_enemy_status(); - // tools::logger()->info("invincible enemy ids size is{}", x.size()); + // component::logger()->info("invincible enemy ids size is{}", x.size()); } std::this_thread::sleep_for(std::chrono::seconds(1)); diff --git a/src/task/usbcamera_detect_test.cpp b/src/task/usbcamera_detect_test.cpp index ec3be54..4596987 100644 --- a/src/task/usbcamera_detect_test.cpp +++ b/src/task/usbcamera_detect_test.cpp @@ -22,13 +22,13 @@ int main(int argc, char * argv[]) cli.printMessage(); return 0; } - tools::Exiter exiter; + component::Exiter exiter; auto config_path = cli.get(0); auto device_name = cli.get("name"); auto display = cli.has("display"); - io::USBCamera usbcam(device_name, config_path); + device::USBCamera usbcam(device_name, config_path); auto_aim::YOLO yolo(config_path, true); @@ -39,10 +39,10 @@ int main(int argc, char * argv[]) usbcam.read(img, timestamp); yolo.detect(img); - auto dt = tools::delta_time(timestamp, last_stamp); + auto dt = component::delta_time(timestamp, last_stamp); last_stamp = timestamp; - tools::logger()->info("{:.2f} fps", 1 / dt); + component::logger()->info("{:.2f} fps", 1 / dt); std::this_thread::sleep_for(10ms); if (!display) continue; diff --git a/src/task/usbcamera_test.cpp b/src/task/usbcamera_test.cpp index 6145180..b5879f4 100644 --- a/src/task/usbcamera_test.cpp +++ b/src/task/usbcamera_test.cpp @@ -22,13 +22,13 @@ int main(int argc, char * argv[]) cli.printMessage(); return 0; } - tools::Exiter exiter; + component::Exiter exiter; auto config_path = cli.get(0); auto device_name = cli.get("name"); auto display = cli.has("display"); - io::USBCamera usbcam(device_name, config_path); + device::USBCamera usbcam(device_name, config_path); cv::Mat img; std::chrono::steady_clock::time_point timestamp; @@ -36,10 +36,10 @@ int main(int argc, char * argv[]) while (!exiter.exit()) { usbcam.read(img, timestamp); - auto dt = tools::delta_time(timestamp, last_stamp); + auto dt = component::delta_time(timestamp, last_stamp); last_stamp = timestamp; - tools::logger()->info("{:.2f} fps", 1 / dt); + component::logger()->info("{:.2f} fps", 1 / dt); std::this_thread::sleep_for(10ms); if (!display) continue;