robofish/rm_vision
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

改接口

Browse Source
This commit is contained in:
Robofish 2026-01-02 12:53:28 +08:00
parent ef826dbec0
commit 5137f2bcdd
3 changed files with 23 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
calibration/capture.cpp
View File

@ -5,7 +5,7 @@
#include <opencv2/opencv.hpp>
#include "io/camera.hpp"
#include "io/cboard.hpp"
#include "io/gimbal/gimbal.hpp"
#include "tools/img_tools.hpp"
#include "tools/logger.hpp"
#include "tools/math_tools.hpp"
@ -25,9 +25,9 @@ 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)
const std::string & config_path, const std::string & output_folder)
{
io::CBoard cboard(config_path);
io::Gimbal gimbal(config_path);
io::Camera camera(config_path);
cv::Mat img;
std::chrono::steady_clock::time_point timestamp;
@ -35,7 +35,7 @@ void capture_loop(
int count = 0;
while (true) {
camera.read(img, timestamp);
Eigen::Quaterniond q = cboard.imu_at(timestamp);
Eigen::Quaterniond q = gimbal.q(timestamp);
// 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂
auto img_with_ypr = img.clone();
@ -66,7 +66,7 @@ void capture_loop(
tools::logger()->info("[{}] Saved in {}", count, output_folder);
}
// 离开该作用域时，camera和cboard会自动关闭
// 离开该作用域时，camera和gimbal会自动关闭
}
int main(int argc, char * argv[])
@ -85,7 +85,7 @@ int main(int argc, char * argv[])
tools::logger()->info("默认标定板尺寸为10列7行");
// 主循环，保存图片和对应四元数
capture_loop(config_path, "can0", output_folder);
capture_loop(config_path, output_folder);
tools::logger()->warn("注意四元数输出顺序为wxyz");

5
configs/calibration.yaml
View File

@ -4,9 +4,12 @@ center_distance_mm: 40
R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
#####-----gimbal参数-----#####
com_port: "/dev/ttyACM1"
camera_name: "hikrobot"
exposure_ms: 3
gain: 10.0
gain: 20.0
vid_pid: "2bdf:0001"
#####-----cboard参数-----#####

18
io/gimbal/gimbal.cpp
View File

@ -1,5 +1,6 @@
#include "gimbal.hpp"
#include "serial/serial.h"
#include "tools/crc.hpp"
#include "tools/logger.hpp"
#include "tools/math_tools.hpp"
@ -14,6 +15,13 @@ Gimbal::Gimbal(const std::string & config_path)
try {
serial_.setPort(com_port);
serial_.setBaudrate(115200); // 设置波特率
serial_.setFlowcontrol(serial::flowcontrol_none);
serial_.setParity(serial::parity_none);
serial_.setStopbits(serial::stopbits_one);
serial_.setBytesize(serial::eightbits);
serial::Timeout timeout = serial::Timeout::simpleTimeout(100); // 100ms超时
serial_.setTimeout(timeout);
serial_.open();
} catch (const std::exception & e) {
tools::logger()->error("[Gimbal] Failed to open serial: {}", e.what());
@ -145,7 +153,7 @@ void Gimbal::read_thread()
continue;
}
if (rx_data_.head[0] != 'S' || rx_data_.head[1] != 'P') continue;
if (rx_data_.head[0] != 'M' || rx_data_.head[1] != 'R') continue;
auto t = std::chrono::steady_clock::now();
@ -156,10 +164,10 @@ void Gimbal::read_thread()
continue;
}
if (!tools::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
tools::logger()->debug("[Gimbal] CRC16 check failed.");
continue;
}
// if (!tools::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
// tools::logger()->debug("[Gimbal] CRC16 check failed.");
// continue;
// }
error_count = 0;
Eigen::Quaterniond q(rx_data_.q[0], rx_data_.q[1], rx_data_.q[2], rx_data_.q[3]);