feat: add ibvs real robot test
This commit is contained in:
parent
b218fdccf8
commit
021b14b387
@ -7,6 +7,7 @@ add_subdirectory(monitor_manager)
|
|||||||
add_subdirectory(service)
|
add_subdirectory(service)
|
||||||
add_subdirectory(controller)
|
add_subdirectory(controller)
|
||||||
add_subdirectory(planner)
|
add_subdirectory(planner)
|
||||||
|
add_subdirectory(perception)
|
||||||
|
|
||||||
add_subdirectory(ik_solver)
|
add_subdirectory(ik_solver)
|
||||||
add_subdirectory(data_center)
|
add_subdirectory(data_center)
|
||||||
|
|||||||
@ -39,7 +39,7 @@ target_link_libraries(controller PUBLIC
|
|||||||
|
|
||||||
add_library(cmvr_es::controller ALIAS controller)
|
add_library(cmvr_es::controller ALIAS controller)
|
||||||
|
|
||||||
|
install(TARGETS controller LIBRARY DESTINATION lib)
|
||||||
|
|
||||||
|
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
|
|||||||
@ -47,6 +47,7 @@ public:
|
|||||||
INVALID_INPUT, /**< 输入参数异常。 */
|
INVALID_INPUT, /**< 输入参数异常。 */
|
||||||
NO_DEPTH, /**< 需要深度但深度不可用。 */
|
NO_DEPTH, /**< 需要深度但深度不可用。 */
|
||||||
NO_TAG, /**< 未检测到 AprilTag。 */
|
NO_TAG, /**< 未检测到 AprilTag。 */
|
||||||
|
TAG_MISMATCH, /**< 检测到 AprilTag，但与指定 id 不匹配。 */
|
||||||
IK_FAILED /**< 速度 IK 求解失败。 */
|
IK_FAILED /**< 速度 IK 求解失败。 */
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -116,7 +117,12 @@ public:
|
|||||||
*/
|
*/
|
||||||
void setTagSize(double tag_size_m);
|
void setTagSize(double tag_size_m);
|
||||||
/**
|
/**
|
||||||
* @brief 设置期望目标位姿（`cMo_des`）。
|
* @brief 设置要跟踪的 tag id（必填）。
|
||||||
|
* @param tag_id 指定 tag id，必须 >= 0。
|
||||||
|
*/
|
||||||
|
void setTrackedTagId(int tag_id);
|
||||||
|
/**
|
||||||
|
* @brief 设置期望目标位姿（`cMo_des tag 坐标系相对于VISP相机的期望位姿`）。
|
||||||
* @param x 期望平移 x（米）。
|
* @param x 期望平移 x（米）。
|
||||||
* @param y 期望平移 y（米）。
|
* @param y 期望平移 y（米）。
|
||||||
* @param z 期望平移 z（米）。
|
* @param z 期望平移 z（米）。
|
||||||
@ -167,7 +173,7 @@ public:
|
|||||||
void setAlignCameraToUrdf(const Eigen::Matrix3d& R_camera_urdf);
|
void setAlignCameraToUrdf(const Eigen::Matrix3d& R_camera_urdf);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 最近一帧是否检测到 tag。
|
* @brief 最近一帧是否检测到“指定 id 的 tag”。
|
||||||
* @return 检测到返回 `true`。
|
* @return 检测到返回 `true`。
|
||||||
*/
|
*/
|
||||||
bool isTagDetected() const { return last_tag_detected_; }
|
bool isTagDetected() const { return last_tag_detected_; }
|
||||||
@ -198,6 +204,16 @@ public:
|
|||||||
* @return tag 平移向量。
|
* @return tag 平移向量。
|
||||||
*/
|
*/
|
||||||
const Eigen::Vector3d& lastTagPositionVisp() const { return last_tag_pos_visp_; }
|
const Eigen::Vector3d& lastTagPositionVisp() const { return last_tag_pos_visp_; }
|
||||||
|
/**
|
||||||
|
* @brief 获取当前指定的 tag id。
|
||||||
|
* @return 指定的 tag id；若未设置返回 `-1`。
|
||||||
|
*/
|
||||||
|
int trackedTagId() const { return tracked_tag_id_; }
|
||||||
|
/**
|
||||||
|
* @brief 获取最近一次用于控制的 tag id。
|
||||||
|
* @return 最近使用的 tag id；若本帧未成功使用返回 `-1`。
|
||||||
|
*/
|
||||||
|
int lastUsedTagId() const { return last_used_tag_id_; }
|
||||||
/**
|
/**
|
||||||
* @brief 获取最近输出的相机 twist（ViSP 相机坐标系）。
|
* @brief 获取最近输出的相机 twist（ViSP 相机坐标系）。
|
||||||
* @return 六维 twist 向量。
|
* @return 六维 twist 向量。
|
||||||
@ -279,9 +295,13 @@ private:
|
|||||||
vpFeaturePoint s_star_[4];
|
vpFeaturePoint s_star_[4];
|
||||||
// AprilTag 检测器。
|
// AprilTag 检测器。
|
||||||
vpDetectorAprilTag detector_;
|
vpDetectorAprilTag detector_;
|
||||||
|
// 指定跟踪的 tag id（必填）；<0 表示未设置。
|
||||||
|
int tracked_tag_id_{-1};
|
||||||
|
|
||||||
// 速度 IK 求解器。
|
// 速度 IK 求解器。
|
||||||
std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
|
std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
|
||||||
|
// 最近一次用于控制的 tag id。
|
||||||
|
int last_used_tag_id_{-1};
|
||||||
|
|
||||||
// 内部积分得到的关节位置命令缓存。
|
// 内部积分得到的关节位置命令缓存。
|
||||||
std::vector<double> q_cmd_;
|
std::vector<double> q_cmd_;
|
||||||
|
|||||||
@ -539,7 +539,8 @@ protected:
|
|||||||
ibvs_controller_->setDepthMode(IbvsController::DepthMode::MONOCULAR);
|
ibvs_controller_->setDepthMode(IbvsController::DepthMode::MONOCULAR);
|
||||||
ibvs_controller_->setDepthZGain(1.0);
|
ibvs_controller_->setDepthZGain(1.0);
|
||||||
ibvs_controller_->setVelocityLimit6(vmax6_);
|
ibvs_controller_->setVelocityLimit6(vmax6_);
|
||||||
ibvs_controller_->setTarget(0.0,0.0,0.15);
|
ibvs_controller_->setTrackedTagId(tracked_tag_id_);
|
||||||
|
ibvs_controller_->setTarget(0.0,0.0,0.35);
|
||||||
|
|
||||||
const Eigen::Matrix3d R_align = (Eigen::Matrix3d() <<
|
const Eigen::Matrix3d R_align = (Eigen::Matrix3d() <<
|
||||||
1, 0, 0,
|
1, 0, 0,
|
||||||
@ -695,6 +696,7 @@ private:
|
|||||||
std::string urdf_path_for_check_{
|
std::string urdf_path_for_check_{
|
||||||
"/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"};
|
"/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"};
|
||||||
std::string camera_frame_name_for_check_{"R_CAM"};
|
std::string camera_frame_name_for_check_{"R_CAM"};
|
||||||
|
int tracked_tag_id_{0};
|
||||||
|
|
||||||
std::unique_ptr<IbvsController> ibvs_controller_{nullptr};
|
std::unique_ptr<IbvsController> ibvs_controller_{nullptr};
|
||||||
std::shared_ptr<device::MujocoCamera> mujoco_camera_{nullptr};
|
std::shared_ptr<device::MujocoCamera> mujoco_camera_{nullptr};
|
||||||
|
|||||||
@ -10,6 +10,7 @@
|
|||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
|
#include <iostream>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
|
|
||||||
#include <opencv2/imgproc.hpp>
|
#include <opencv2/imgproc.hpp>
|
||||||
@ -29,6 +30,7 @@ const char* IbvsController::statusToString(ComputeStatus status) {
|
|||||||
case ComputeStatus::INVALID_INPUT: return "invalid_input";
|
case ComputeStatus::INVALID_INPUT: return "invalid_input";
|
||||||
case ComputeStatus::NO_DEPTH: return "no_depth";
|
case ComputeStatus::NO_DEPTH: return "no_depth";
|
||||||
case ComputeStatus::NO_TAG: return "no_tag";
|
case ComputeStatus::NO_TAG: return "no_tag";
|
||||||
|
case ComputeStatus::TAG_MISMATCH: return "tag_mismatch";
|
||||||
case ComputeStatus::IK_FAILED: return "ik_failed";
|
case ComputeStatus::IK_FAILED: return "ik_failed";
|
||||||
default: return "unknown";
|
default: return "unknown";
|
||||||
}
|
}
|
||||||
@ -91,6 +93,7 @@ void IbvsController::reset(const std::vector<double>& q_init) {
|
|||||||
q_cmd_ = q_init;
|
q_cmd_ = q_init;
|
||||||
}
|
}
|
||||||
last_tag_detected_ = false;
|
last_tag_detected_ = false;
|
||||||
|
last_used_tag_id_ = -1;
|
||||||
last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
|
last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
|
||||||
last_depth_usage_ = DepthUsage::NONE;
|
last_depth_usage_ = DepthUsage::NONE;
|
||||||
last_tag_pos_visp_.setZero();
|
last_tag_pos_visp_.setZero();
|
||||||
@ -161,16 +164,53 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
|||||||
std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
|
std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (tracked_tag_id_ < 0) {
|
||||||
|
last_tag_detected_ = false;
|
||||||
|
last_used_tag_id_ = -1;
|
||||||
|
last_tag_pos_visp_.setZero();
|
||||||
|
last_compute_status_ = ComputeStatus::INVALID_INPUT;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
std::vector<vpHomogeneousMatrix> cMo_vec;
|
std::vector<vpHomogeneousMatrix> cMo_vec;
|
||||||
const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
|
const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
|
||||||
last_tag_detected_ = (detected && !cMo_vec.empty());
|
if (!detected || cMo_vec.empty()) {
|
||||||
if (!last_tag_detected_) {
|
last_tag_detected_ = false;
|
||||||
|
last_used_tag_id_ = -1;
|
||||||
last_tag_pos_visp_.setZero();
|
last_tag_pos_visp_.setZero();
|
||||||
last_compute_status_ = ComputeStatus::NO_TAG;
|
last_compute_status_ = ComputeStatus::NO_TAG;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
vpHomogeneousMatrix cMo = cMo_vec[0];
|
// 只允许使用指定 id 的 tag，不再回退到“第一个检测结果”。
|
||||||
|
const std::vector<int> tag_ids = detector_.getTagsId();
|
||||||
|
const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size());
|
||||||
|
int selected_idx = -1;
|
||||||
|
for (size_t i = 0; i < pair_size; ++i) {
|
||||||
|
if (tag_ids[i] == tracked_tag_id_) {
|
||||||
|
selected_idx = static_cast<int>(i);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (selected_idx < 0) {
|
||||||
|
std::cout << "[IbvsController] TAG_MISMATCH target_tag_id=" << tracked_tag_id_
|
||||||
|
<< ", detected_tag_ids=[";
|
||||||
|
for (size_t i = 0; i < tag_ids.size(); ++i) {
|
||||||
|
if (i > 0) std::cout << ",";
|
||||||
|
std::cout << tag_ids[i];
|
||||||
|
}
|
||||||
|
std::cout << "]\n";
|
||||||
|
|
||||||
|
last_tag_detected_ = false;
|
||||||
|
last_used_tag_id_ = -1;
|
||||||
|
last_tag_pos_visp_.setZero();
|
||||||
|
last_compute_status_ = ComputeStatus::TAG_MISMATCH;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
last_tag_detected_ = true;
|
||||||
|
last_used_tag_id_ = tracked_tag_id_;
|
||||||
|
vpHomogeneousMatrix cMo = cMo_vec[static_cast<size_t>(selected_idx)];
|
||||||
last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3];
|
last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3];
|
||||||
|
|
||||||
// 深度控制点定义在 tag 平面（object frame）:
|
// 深度控制点定义在 tag 平面（object frame）:
|
||||||
@ -309,6 +349,10 @@ void IbvsController::setTagSize(double tag_size_m) {
|
|||||||
initTask();
|
initTask();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void IbvsController::setTrackedTagId(int tag_id) {
|
||||||
|
tracked_tag_id_ = tag_id;
|
||||||
|
}
|
||||||
|
|
||||||
void IbvsController::setTarget(double x,
|
void IbvsController::setTarget(double x,
|
||||||
double y,
|
double y,
|
||||||
double z,
|
double z,
|
||||||
|
|||||||
@ -14,3 +14,6 @@ target_link_libraries(data_center PRIVATE
|
|||||||
protobuf
|
protobuf
|
||||||
glog
|
glog
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
install(TARGETS data_center LIBRARY DESTINATION lib)
|
||||||
@ -20,3 +20,4 @@ target_link_libraries(device_manager PRIVATE
|
|||||||
)
|
)
|
||||||
|
|
||||||
add_library(cmvr_es::device_manager ALIAS device_manager)
|
add_library(cmvr_es::device_manager ALIAS device_manager)
|
||||||
|
install(TARGETS device_manager LIBRARY DESTINATION lib)
|
||||||
|
|||||||
@ -5,7 +5,14 @@ target_include_directories(realsense_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
|||||||
add_library(cmvr_es::device::realsense_camera ALIAS realsense_camera) # ✅ 命名空间别名 OK
|
add_library(cmvr_es::device::realsense_camera ALIAS realsense_camera) # ✅ 命名空间别名 OK
|
||||||
|
|
||||||
# 链接 librealsense2
|
# 链接 librealsense2
|
||||||
target_link_libraries(realsense_camera PRIVATE realsense2)
|
target_link_libraries(realsense_camera PRIVATE
|
||||||
|
realsense2
|
||||||
|
avcodec
|
||||||
|
avformat
|
||||||
|
avutil
|
||||||
|
swscale
|
||||||
|
swresample
|
||||||
|
)
|
||||||
|
|
||||||
install(TARGETS realsense_camera LIBRARY DESTINATION lib)
|
install(TARGETS realsense_camera LIBRARY DESTINATION lib)
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
|
|||||||
@ -151,23 +151,41 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca
|
|||||||
auto stream_mode = camera_.stream_mode();
|
auto stream_mode = camera_.stream_mode();
|
||||||
if (stream_mode == config::STREAM_MODE_RGB)
|
if (stream_mode == config::STREAM_MODE_RGB)
|
||||||
{
|
{
|
||||||
stream_mode_ = RGBD_MODE;
|
stream_mode_ = COLOR_MODE;
|
||||||
}
|
}
|
||||||
else if (stream_mode == config::STREAM_MODE_RGBD)
|
else if (stream_mode == config::STREAM_MODE_RGBD)
|
||||||
{
|
{
|
||||||
stream_mode_ = COLOR_MODE;
|
stream_mode_ = RGBD_MODE;
|
||||||
}
|
}
|
||||||
else if (stream_mode == config::STREAM_MODE_DEPTH)
|
else if (stream_mode == config::STREAM_MODE_DEPTH)
|
||||||
{
|
{
|
||||||
stream_mode_ = DEPTH_MODE;
|
stream_mode_ = DEPTH_MODE;
|
||||||
}
|
}
|
||||||
|
else {
|
||||||
|
stream_mode_ = RGBD_MODE;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto align_mode = camera_.align_mode();
|
||||||
|
if (align_mode == config::ALIGN_MODE_COLOR) {
|
||||||
|
align_mode_ = "color";
|
||||||
|
} else if (align_mode == config::ALIGN_MODE_DEPTH) {
|
||||||
|
align_mode_ = "depth";
|
||||||
|
} else {
|
||||||
|
align_mode_ = "color";
|
||||||
|
}
|
||||||
|
|
||||||
codec_ = camera_.codec();
|
codec_ = camera_.codec();
|
||||||
}
|
}
|
||||||
|
|
||||||
RealsenseCamera::~RealsenseCamera() {
|
RealsenseCamera::~RealsenseCamera() {
|
||||||
// 停止采集线程，释放资源
|
// 析构阶段禁止抛异常，避免 terminate。
|
||||||
stop();
|
try {
|
||||||
|
stop();
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): " << e.what();
|
||||||
|
} catch (...) {
|
||||||
|
LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): unknown exception";
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void RealsenseCamera::init() {
|
void RealsenseCamera::init() {
|
||||||
@ -222,78 +240,110 @@ void RealsenseCamera::start() {
|
|||||||
throw runtime_error("Camera not initialized");
|
throw runtime_error("Camera not initialized");
|
||||||
}
|
}
|
||||||
|
|
||||||
try{
|
if (state_.is_opened) {
|
||||||
if (state_.is_opened) {
|
LOG(WARNING) << "[RealsenseCamera] (start): camera already started";
|
||||||
LOG(WARNING) << "[RealsenseCamera] (start): camera already started";
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
constexpr int kStartMaxRetry = 3;
|
||||||
|
constexpr int kRetrySleepMs = 300;
|
||||||
|
constexpr unsigned int kProbeTimeoutMs = 2000;
|
||||||
|
std::string last_error;
|
||||||
|
state_.is_opened = false;
|
||||||
|
for (int attempt = 1; attempt <= kStartMaxRetry; ++attempt) {
|
||||||
|
try {
|
||||||
|
// 每次尝试前重建 pipeline，避免复用异常状态。
|
||||||
|
pipe_ = rs2::pipeline();
|
||||||
|
|
||||||
|
// 使用相同的同步策略启动
|
||||||
|
profile_ = pipe_.start(rs_cfg_);
|
||||||
|
|
||||||
|
if (stream_mode_ == RGBD_MODE){
|
||||||
|
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
|
||||||
|
auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
|
||||||
|
Kd_ = sp_depth.get_intrinsics();
|
||||||
|
Kc_ = sp_color.get_intrinsics();
|
||||||
|
Ec2d_ = sp_color.get_extrinsics_to(sp_depth);
|
||||||
|
Ed2c_ = sp_depth.get_extrinsics_to(sp_color);
|
||||||
|
|
||||||
|
intrinsics_ = Kc_;
|
||||||
|
}
|
||||||
|
else if (stream_mode_ == COLOR_MODE) {
|
||||||
|
auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
|
||||||
|
Kc_ = sp_color.get_intrinsics();
|
||||||
|
|
||||||
|
intrinsics_ = Kc_;
|
||||||
|
}
|
||||||
|
else if (stream_mode_ == DEPTH_MODE) {
|
||||||
|
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
|
||||||
|
Kd_ = sp_depth.get_intrinsics();
|
||||||
|
|
||||||
|
intrinsics_ = Kd_;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (align_mode_ == "color") {
|
||||||
|
align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
|
||||||
|
}
|
||||||
|
else if (align_mode_ == "depth") {
|
||||||
|
align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 启动后做一次首帧探测，避免“start 成功但长期无帧”假阳性。
|
||||||
|
rs2::frameset probe = pipe_.wait_for_frames(kProbeTimeoutMs);
|
||||||
|
if (!probe.get_color_frame()) {
|
||||||
|
throw runtime_error("no color frame after start");
|
||||||
|
}
|
||||||
|
if (stream_mode_ == RGBD_MODE && !probe.get_depth_frame()) {
|
||||||
|
throw runtime_error("no depth frame after start");
|
||||||
|
}
|
||||||
|
|
||||||
|
state_.is_opened = true;
|
||||||
|
LOG(INFO) << "realsense start streaming successfully";
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// 使用相同的同步策略启动
|
catch (const exception &error) {
|
||||||
profile_ = pipe_.start(rs_cfg_);
|
last_error = error.what();
|
||||||
|
state_.is_opened = false;
|
||||||
|
LOG(WARNING) << "[RealsenseCamera] (start): attempt "
|
||||||
if (stream_mode_ == RGBD_MODE){
|
<< attempt << "/" << kStartMaxRetry
|
||||||
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
|
<< " failed: " << last_error;
|
||||||
auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
|
try {
|
||||||
Kd_ = sp_depth.get_intrinsics();
|
pipe_.stop();
|
||||||
Kc_ = sp_color.get_intrinsics();
|
} catch (...) {
|
||||||
Ec2d_ = sp_color.get_extrinsics_to(sp_depth);
|
// ignore stop exceptions during retry cleanup
|
||||||
Ed2c_ = sp_depth.get_extrinsics_to(sp_color);
|
}
|
||||||
|
align_.reset();
|
||||||
intrinsics_ = Kc_;
|
std::this_thread::sleep_for(std::chrono::milliseconds(kRetrySleepMs));
|
||||||
}
|
}
|
||||||
else if (stream_mode_ == COLOR_MODE) {
|
|
||||||
auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
|
|
||||||
Kc_ = sp_color.get_intrinsics();
|
|
||||||
|
|
||||||
intrinsics_ = Kc_;
|
|
||||||
}
|
|
||||||
else if (stream_mode_ == DEPTH_MODE) {
|
|
||||||
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
|
|
||||||
Kd_ = sp_depth.get_intrinsics();
|
|
||||||
|
|
||||||
intrinsics_ = Kd_;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (align_mode_ == "color") {
|
|
||||||
align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
|
|
||||||
}
|
|
||||||
else if (align_mode_ == "depth") {
|
|
||||||
align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
|
|
||||||
}
|
|
||||||
|
|
||||||
// auto frames = get_frameset(true);
|
|
||||||
// rs2::frame color_frame = frames.get_color_frame();
|
|
||||||
// auto profile = color_frame.get_profile();
|
|
||||||
// auto color_profile = frames.get_color_frame().get_profile();
|
|
||||||
// intrinsics_ = color_profile.as<rs2::video_stream_profile>().get_intrinsics();
|
|
||||||
|
|
||||||
state_.is_opened = true;
|
|
||||||
LOG(INFO) << "realsense start streaming successfully";
|
|
||||||
}
|
|
||||||
catch (const exception &error) {
|
|
||||||
state_.is_opened = false;
|
|
||||||
LOG(ERROR) << "realsense start streaming error: " << error.what();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
LOG(ERROR) << "realsense start streaming error: " << last_error;
|
||||||
|
throw runtime_error("realsense start streaming error: " + last_error);
|
||||||
}
|
}
|
||||||
|
|
||||||
void RealsenseCamera::stop() {
|
void RealsenseCamera::stop() {
|
||||||
//先停止录制再关闭摄像头
|
//先停止录制再关闭摄像头
|
||||||
if (state_.is_recording) {
|
if (state_.is_recording) {
|
||||||
stopRecording();
|
try {
|
||||||
|
stopRecording();
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
LOG(WARNING) << "[RealsenseCamera] (stop): stopRecording failed: " << e.what();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
std::lock_guard lock(ctrl_mtx_);
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
clear_error_();
|
clear_error_();
|
||||||
try {
|
if (!state_.is_opened || !state_.is_initialized) {
|
||||||
if (!state_.is_opened || !state_.is_initialized) {
|
|
||||||
throw runtime_error("Camera already closed");
|
|
||||||
}
|
|
||||||
pipe_.stop();
|
|
||||||
state_.is_opened = false;
|
state_.is_opened = false;
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
try {
|
||||||
LOG(ERROR) << "[RealsenseCamera] (stop): " << e.what();
|
pipe_.stop();
|
||||||
throw runtime_error(e.what());
|
} catch (const std::exception& e) {
|
||||||
|
LOG(WARNING) << "[RealsenseCamera] (stop): " << e.what();
|
||||||
}
|
}
|
||||||
|
align_.reset();
|
||||||
|
pipe_ = rs2::pipeline();
|
||||||
|
state_.is_opened = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
|
void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
|
||||||
@ -337,7 +387,7 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
|
|||||||
latest_depth_ = depth;
|
latest_depth_ = depth;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
LOG(INFO) << "realsense get rgb frame successfully";
|
// LOG(INFO) << "realsense get rgb frame successfully";
|
||||||
}
|
}
|
||||||
catch (const std::exception& e) {
|
catch (const std::exception& e) {
|
||||||
throw std::runtime_error("[RealsenseCamera] (getRGBImage): Failed to get image: " + std::string(e.what()));
|
throw std::runtime_error("[RealsenseCamera] (getRGBImage): Failed to get image: " + std::string(e.what()));
|
||||||
@ -619,7 +669,8 @@ void RealsenseCamera::resumeRecording() {
|
|||||||
|
|
||||||
rs2::frameset RealsenseCamera::get_frameset(bool align) {
|
rs2::frameset RealsenseCamera::get_frameset(bool align) {
|
||||||
std::lock_guard<std::mutex> lock(frame_mtx_);
|
std::lock_guard<std::mutex> lock(frame_mtx_);
|
||||||
rs2::frameset frames = pipe_.wait_for_frames();
|
constexpr unsigned int kWaitTimeoutMs = 300;
|
||||||
|
rs2::frameset frames = pipe_.wait_for_frames(kWaitTimeoutMs);
|
||||||
// 如果需要对齐，添加对齐逻辑
|
// 如果需要对齐，添加对齐逻辑
|
||||||
if (align_ && align && stream_mode_ == RGBD_MODE) {
|
if (align_ && align && stream_mode_ == RGBD_MODE) {
|
||||||
frames = align_->process(frames);
|
frames = align_->process(frames);
|
||||||
@ -842,9 +893,8 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& enco
|
|||||||
av_opt_set(ctx->priv_data, "repeat-headers", "1", 0);
|
av_opt_set(ctx->priv_data, "repeat-headers", "1", 0);
|
||||||
av_opt_set(ctx->priv_data, "annexb", "1", 0);
|
av_opt_set(ctx->priv_data, "annexb", "1", 0);
|
||||||
} else if (codec->id == AV_CODEC_ID_HEVC) {
|
} else if (codec->id == AV_CODEC_ID_HEVC) {
|
||||||
// 直接使用默认参数，不自定义
|
// 降低 x265 控制台日志噪声（如 "encoded 0 frames"）。
|
||||||
// av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
|
av_opt_set(ctx->priv_data, "x265-params", "log-level=none", 0);
|
||||||
// av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 6. 设置像素格式（不变）
|
// 6. 设置像素格式（不变）
|
||||||
@ -1124,4 +1174,3 @@ Eigen::Vector3f RealsenseCamera::get3DPointFromPixel(int u, int v) {
|
|||||||
|
|
||||||
return { point[0], point[1], point[2] };
|
return { point[0], point[1], point[2] };
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -3,8 +3,12 @@
|
|||||||
//
|
//
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include "../../abstract_camera.h"
|
#include "../../abstract_camera.h"
|
||||||
|
#include "../include/realsense_camera.h"
|
||||||
#include <opencv2/opencv.hpp>
|
#include <opencv2/opencv.hpp>
|
||||||
#include "../../../../device_manager/include/device_manager.h"
|
#include "../../../../device_manager/include/device_manager.h"
|
||||||
|
#include <chrono>
|
||||||
|
#include <iostream>
|
||||||
|
#include <thread>
|
||||||
using namespace cmvr::device;
|
using namespace cmvr::device;
|
||||||
// 测试: 实时显示 RGB + Depth
|
// 测试: 实时显示 RGB + Depth
|
||||||
TEST(RealsenseCameraRealDeviceTest, SaveFrames) {
|
TEST(RealsenseCameraRealDeviceTest, SaveFrames) {
|
||||||
@ -35,3 +39,174 @@ TEST(RealsenseCameraRealDeviceTest, SaveFrames) {
|
|||||||
LOG(INFO) << "Pose : " << pose ;
|
LOG(INFO) << "Pose : " << pose ;
|
||||||
cam->stop();
|
cam->stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
constexpr const char* kRsSerialForHealthCheck = "243122074587";
|
||||||
|
constexpr int kRsWidth = 640;
|
||||||
|
constexpr int kRsHeight = 480;
|
||||||
|
constexpr int kRsFps = 30;
|
||||||
|
constexpr int kWarmupMaxTries = 80;
|
||||||
|
constexpr int kWarmupSleepMs = 30;
|
||||||
|
|
||||||
|
cmvr::config::RealSenseCameraConfig makeRsConfig() {
|
||||||
|
cmvr::config::RealSenseCameraConfig cfg;
|
||||||
|
cfg.set_id("realsense_health_check");
|
||||||
|
cfg.set_serialnumber(kRsSerialForHealthCheck);
|
||||||
|
cfg.set_width(kRsWidth);
|
||||||
|
cfg.set_height(kRsHeight);
|
||||||
|
cfg.set_fps(kRsFps);
|
||||||
|
cfg.set_codec("H265");
|
||||||
|
cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
|
||||||
|
cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
|
||||||
|
cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
|
||||||
|
cfg.set_buffer_size(30);
|
||||||
|
cfg.set_sync(true);
|
||||||
|
cfg.set_enable(true);
|
||||||
|
return cfg;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool tryGetFirstFrame(const std::shared_ptr<RealsenseCamera>& camera,
|
||||||
|
cv::Mat& color,
|
||||||
|
cv::Mat& depth,
|
||||||
|
Rs2Intrinsics& intrinsics,
|
||||||
|
int& exception_count,
|
||||||
|
std::string* last_error = nullptr) {
|
||||||
|
exception_count = 0;
|
||||||
|
if (last_error) {
|
||||||
|
last_error->clear();
|
||||||
|
}
|
||||||
|
for (int i = 0; i < kWarmupMaxTries; ++i) {
|
||||||
|
try {
|
||||||
|
camera->getRGBDImages(color, depth, intrinsics);
|
||||||
|
if (!color.empty() && !depth.empty()) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
++exception_count;
|
||||||
|
if (last_error) {
|
||||||
|
*last_error = e.what();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(kWarmupSleepMs));
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
struct RealsenseStopGuard {
|
||||||
|
std::shared_ptr<RealsenseCamera> camera;
|
||||||
|
~RealsenseStopGuard() {
|
||||||
|
if (!camera) return;
|
||||||
|
try {
|
||||||
|
camera->stop();
|
||||||
|
} catch (...) {
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
TEST(RealsenseCameraHealthCheck, StartAndGetFirstFrame) {
|
||||||
|
if (std::string(kRsSerialForHealthCheck).empty()) {
|
||||||
|
GTEST_SKIP() << "kRsSerialForHealthCheck is empty";
|
||||||
|
}
|
||||||
|
|
||||||
|
auto camera = std::make_shared<RealsenseCamera>(makeRsConfig());
|
||||||
|
ASSERT_NO_THROW(camera->init());
|
||||||
|
ASSERT_NO_THROW(camera->start());
|
||||||
|
RealsenseStopGuard stop_guard{camera};
|
||||||
|
|
||||||
|
CameraState state{};
|
||||||
|
camera->getState(state);
|
||||||
|
EXPECT_TRUE(state.is_initialized);
|
||||||
|
EXPECT_TRUE(state.is_opened);
|
||||||
|
|
||||||
|
cv::Mat color, depth;
|
||||||
|
Rs2Intrinsics intrinsics{};
|
||||||
|
int exception_count = 0;
|
||||||
|
std::string last_error;
|
||||||
|
const auto t0 = std::chrono::steady_clock::now();
|
||||||
|
const bool ok = tryGetFirstFrame(camera, color, depth, intrinsics, exception_count, &last_error);
|
||||||
|
const auto t1 = std::chrono::steady_clock::now();
|
||||||
|
const auto first_frame_ms = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count();
|
||||||
|
|
||||||
|
std::cout << "[RealsenseCameraHealthCheck] first_frame_ok=" << ok
|
||||||
|
<< " first_frame_ms=" << first_frame_ms
|
||||||
|
<< " exceptions=" << exception_count
|
||||||
|
<< " last_error=" << last_error << "\n";
|
||||||
|
|
||||||
|
ASSERT_TRUE(ok) << "Failed to get first RGBD frame after start";
|
||||||
|
EXPECT_EQ(color.type(), CV_8UC3);
|
||||||
|
EXPECT_TRUE(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(RealsenseCameraHealthCheck, RGBDReadLoopStability) {
|
||||||
|
if (std::string(kRsSerialForHealthCheck).empty()) {
|
||||||
|
GTEST_SKIP() << "kRsSerialForHealthCheck is empty";
|
||||||
|
}
|
||||||
|
|
||||||
|
auto camera = std::make_shared<RealsenseCamera>(makeRsConfig());
|
||||||
|
ASSERT_NO_THROW(camera->init());
|
||||||
|
ASSERT_NO_THROW(camera->start());
|
||||||
|
RealsenseStopGuard stop_guard{camera};
|
||||||
|
|
||||||
|
cv::Mat color, depth;
|
||||||
|
Rs2Intrinsics intrinsics{};
|
||||||
|
int warmup_exceptions = 0;
|
||||||
|
ASSERT_TRUE(tryGetFirstFrame(camera, color, depth, intrinsics, warmup_exceptions))
|
||||||
|
<< "Warmup failed before stability loop";
|
||||||
|
|
||||||
|
constexpr int kReadLoopFrames = 150;
|
||||||
|
int success = 0;
|
||||||
|
int fail = 0;
|
||||||
|
int exceptions = 0;
|
||||||
|
for (int i = 0; i < kReadLoopFrames; ++i) {
|
||||||
|
try {
|
||||||
|
camera->getRGBDImages(color, depth, intrinsics);
|
||||||
|
if (!color.empty() && !depth.empty()) {
|
||||||
|
++success;
|
||||||
|
} else {
|
||||||
|
++fail;
|
||||||
|
}
|
||||||
|
} catch (const std::exception&) {
|
||||||
|
++fail;
|
||||||
|
++exceptions;
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||||
|
}
|
||||||
|
|
||||||
|
std::cout << "[RealsenseCameraHealthCheck] read_loop success=" << success
|
||||||
|
<< " fail=" << fail
|
||||||
|
<< " exceptions=" << exceptions << "\n";
|
||||||
|
|
||||||
|
EXPECT_GT(success, 0);
|
||||||
|
EXPECT_GE(success, fail);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(RealsenseCameraHealthCheck, RestartWithoutReplug) {
|
||||||
|
if (std::string(kRsSerialForHealthCheck).empty()) {
|
||||||
|
GTEST_SKIP() << "kRsSerialForHealthCheck is empty";
|
||||||
|
}
|
||||||
|
|
||||||
|
auto camera = std::make_shared<RealsenseCamera>(makeRsConfig());
|
||||||
|
ASSERT_NO_THROW(camera->init());
|
||||||
|
RealsenseStopGuard stop_guard{camera};
|
||||||
|
|
||||||
|
for (int cycle = 0; cycle < 2; ++cycle) {
|
||||||
|
ASSERT_NO_THROW(camera->start()) << "start failed at cycle " << cycle;
|
||||||
|
|
||||||
|
cv::Mat color, depth;
|
||||||
|
Rs2Intrinsics intrinsics{};
|
||||||
|
int exception_count = 0;
|
||||||
|
std::string last_error;
|
||||||
|
const bool ok = tryGetFirstFrame(camera, color, depth, intrinsics, exception_count, &last_error);
|
||||||
|
std::cout << "[RealsenseCameraHealthCheck] cycle=" << cycle
|
||||||
|
<< " first_frame_ok=" << ok
|
||||||
|
<< " exceptions=" << exception_count
|
||||||
|
<< " last_error=" << last_error << "\n";
|
||||||
|
EXPECT_TRUE(ok) << "Failed to get frame at cycle " << cycle;
|
||||||
|
|
||||||
|
EXPECT_NO_THROW(camera->stop()) << "stop failed at cycle " << cycle;
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(200));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|||||||
@ -68,9 +68,32 @@ namespace cmvr::device{
|
|||||||
|
|
||||||
virtual void eStop() { throw std::runtime_error("Not implemented"); }
|
virtual void eStop() { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 关节空间 point-to-point 运动（内部轨迹规划）。
|
||||||
|
* @details 控制器会在内部生成满足速度/加速度约束的关节轨迹，并做多关节时间同步，使关节协调到达目标点。
|
||||||
|
* @param joints 目标关节角，单位 rad。
|
||||||
|
* @param max_vel 主导轴最大速度，单位 rad/s。
|
||||||
|
* @param max_acc 主导轴最大加速度，单位 rad/s^2。
|
||||||
|
*/
|
||||||
virtual void moveJ(std::vector<double> &joints, double max_vel=0.5, double max_acc=0.1) { throw std::runtime_error("Not implemented"); }
|
virtual void moveJ(std::vector<double> &joints, double max_vel=0.5, double max_acc=0.1) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 按关节命令执行关节空间运动（内部轨迹规划）。
|
||||||
|
* @param cmd 目标关节命令（关节名 + 角度，`vel` 字段可由具体实现决定是否使用）。
|
||||||
|
* @param vel 主导轴最大速度，单位 rad/s。
|
||||||
|
* @param acc 主导轴最大加速度，单位 rad/s^2。
|
||||||
|
*/
|
||||||
virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 末端位姿目标的关节空间运动。
|
||||||
|
* @details 典型实现为先做 IK 求解关节目标，再按 `moveJ` 语义执行（关节空间规划与同步）。
|
||||||
|
* @param base_link 基坐标系 link 名称。
|
||||||
|
* @param ee_link 末端 link 名称。
|
||||||
|
* @param pose 目标末端位姿。
|
||||||
|
* @param vel 主导轴最大速度，单位 rad/s。
|
||||||
|
* @param acc 主导轴最大加速度，单位 rad/s^2。
|
||||||
|
*/
|
||||||
virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
||||||
virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
@ -97,10 +120,38 @@ namespace cmvr::device{
|
|||||||
|
|
||||||
virtual void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) { throw std::runtime_error("Not implemented"); }
|
virtual void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 关节空间周期伺服（无整段轨迹规划）。
|
||||||
|
* @details 每次调用只在一个控制周期内跟踪当前目标，需外部循环持续下发；是否稳定由外部更新频率和目标序列决定。
|
||||||
|
* @param joints 当前周期目标关节角，单位 rad。
|
||||||
|
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
|
||||||
|
*/
|
||||||
virtual void servoJ(std::vector<double> &joints, double dt) { throw std::runtime_error("Not implemented"); }
|
virtual void servoJ(std::vector<double> &joints, double dt) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 按关节命令执行周期伺服（无整段轨迹规划）。
|
||||||
|
* @param joints 当前周期关节目标命令。
|
||||||
|
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
|
||||||
|
*/
|
||||||
virtual void servoJ(std::vector<JointPoint> &joints, double dt) { throw std::runtime_error("Not implemented"); }
|
virtual void servoJ(std::vector<JointPoint> &joints, double dt) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 带速度参数的关节空间周期伺服。
|
||||||
|
* @param joints 当前周期关节目标命令。
|
||||||
|
* @param vel 速度参数（语义由具体驱动实现决定）。
|
||||||
|
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
|
||||||
|
*/
|
||||||
virtual void servoJ(std::vector<JointPoint> &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); }
|
virtual void servoJ(std::vector<JointPoint> &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 基于末端位姿目标的周期伺服。
|
||||||
|
* @details 典型实现为每周期执行 IK 并下发关节伺服目标；属于实时闭环接口，不等价于一次性到位的 `moveJ`。
|
||||||
|
* @param base_link 基坐标系 link 名称。
|
||||||
|
* @param ee_link 末端 link 名称。
|
||||||
|
* @param pose 当前周期目标末端位姿。
|
||||||
|
* @param vel 速度参数（语义由具体驱动实现决定）。
|
||||||
|
* @param acc 加速度参数（语义由具体驱动实现决定）。
|
||||||
|
*/
|
||||||
virtual void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
virtual void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
||||||
virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
||||||
|
|
||||||
|
|||||||
@ -29,6 +29,7 @@ add_executable(humanoid_robot_test
|
|||||||
target_link_libraries(humanoid_robot_test
|
target_link_libraries(humanoid_robot_test
|
||||||
PRIVATE
|
PRIVATE
|
||||||
cmvr_es::device_manager
|
cmvr_es::device_manager
|
||||||
|
cmvr_es::controller
|
||||||
cmvr_es::common
|
cmvr_es::common
|
||||||
cmvr_es::data_center
|
cmvr_es::data_center
|
||||||
osqp
|
osqp
|
||||||
|
|||||||
@ -10,21 +10,57 @@
|
|||||||
#include <thread>
|
#include <thread>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
#include <unordered_map>
|
||||||
|
#include <array>
|
||||||
|
#include <algorithm>
|
||||||
|
#include <limits>
|
||||||
|
|
||||||
#include "gtest/gtest.h"
|
#include "gtest/gtest.h"
|
||||||
#include <glog/logging.h>
|
#include <glog/logging.h>
|
||||||
#include "../../../../device_manager/include/device_manager.h"
|
#include "../../../../device_manager/include/device_manager.h"
|
||||||
#include <libgen.h>
|
#include <libgen.h>
|
||||||
|
|
||||||
|
#include "controller/include/ibvs_controller.h"
|
||||||
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
|
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
|
||||||
#include "../include/humanoid_robot.h"
|
#include "../include/humanoid_robot.h"
|
||||||
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
|
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
|
||||||
#include "../../../../utils/base/include/abstract_interpolation.h"
|
#include "../../../../utils/base/include/abstract_interpolation.h"
|
||||||
|
|
||||||
// 定义一个命令行参数 --config_path
|
|
||||||
DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file");
|
|
||||||
using namespace cmvr::device;
|
using namespace cmvr::device;
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
constexpr std::array<const char*, 7> kRightArmJointNames = {
|
||||||
|
"R_SHOULDER_P",
|
||||||
|
"R_SHOULDER_R",
|
||||||
|
"R_SHOULDER_Y",
|
||||||
|
"R_ELBOW_R",
|
||||||
|
"R_WRIST_P",
|
||||||
|
"R_WRIST_Y",
|
||||||
|
"R_WRIST_R"
|
||||||
|
};
|
||||||
|
|
||||||
|
std::vector<JointPoint> buildRightArmJointCmd(const std::vector<double>& q_cmd,
|
||||||
|
double vel) {
|
||||||
|
std::vector<JointPoint> cmd;
|
||||||
|
cmd.reserve(kRightArmJointNames.size());
|
||||||
|
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
|
||||||
|
cmd.emplace_back(kRightArmJointNames[i], q_cmd[i], vel);
|
||||||
|
}
|
||||||
|
return cmd;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::unordered_map<std::string, double> makeRightArmQMap() {
|
||||||
|
std::unordered_map<std::string, double> q_map;
|
||||||
|
q_map.reserve(kRightArmJointNames.size());
|
||||||
|
for (const auto* name : kRightArmJointNames) {
|
||||||
|
q_map.emplace(name, 0.0);
|
||||||
|
}
|
||||||
|
return q_map;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
TEST(HumanoidRobotTest,GetState) {
|
TEST(HumanoidRobotTest,GetState) {
|
||||||
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
||||||
const XmlNode config(config_path);
|
const XmlNode config(config_path);
|
||||||
@ -71,616 +107,176 @@ TEST(HumanoidRobotTest,MyRobotTest) {
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
|
||||||
TEST(HumanoidRobotTest,FollowJointTrajectoryTest) {
|
const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml");
|
||||||
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
|
||||||
const XmlNode config(config_path);
|
|
||||||
|
|
||||||
if (!config.hasChild("DeviceManager")){
|
|
||||||
LOG(ERROR) << "Device Manager node not found";
|
|
||||||
}
|
|
||||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||||
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||||
|
|
||||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
||||||
|
|
||||||
|
auto q_map_now = makeRightArmQMap();
|
||||||
// 读取轨迹
|
std::vector<double> q_now(kRightArmJointNames.size(), 0.0);
|
||||||
std::ifstream file("/home/lgv/cmvr/cmvr-es/src/devices/robot/humanoid_robot/joint_positions_1.csv");
|
auto refreshRightArmQ = [&]() -> bool {
|
||||||
if (!file.is_open()) {
|
robot->getJointQ(q_map_now);
|
||||||
LOG(ERROR) << "无法打开文件" ;
|
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
|
||||||
|
const auto it = q_map_now.find(kRightArmJointNames[i]);
|
||||||
}
|
if (it == q_map_now.end()) return false;
|
||||||
|
q_now[i] = it->second;
|
||||||
std::string line;
|
|
||||||
|
|
||||||
// 读取标题行，获取关节名称（除去第一列 idx）
|
|
||||||
if (!std::getline(file, line)) {
|
|
||||||
LOG(ERROR) << "文件为空或格式错误" ;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<std::string> joint_names;
|
|
||||||
{
|
|
||||||
std::stringstream ss(line);
|
|
||||||
std::string cell;
|
|
||||||
// 第一列是 idx，跳过
|
|
||||||
std::getline(ss, cell, ',');
|
|
||||||
// 读取关节名称列
|
|
||||||
while (std::getline(ss, cell, ',')) {
|
|
||||||
joint_names.push_back(cell);
|
|
||||||
}
|
}
|
||||||
}
|
return true;
|
||||||
|
};
|
||||||
|
|
||||||
std::vector<std::vector<JointPoint>> traj;
|
ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q";
|
||||||
|
|
||||||
// 读取后续每行数据
|
|
||||||
while (std::getline(file, line)) {
|
|
||||||
std::stringstream ss(line);
|
|
||||||
std::string cell;
|
|
||||||
|
|
||||||
// 读取第一列 idx，暂时不使用
|
|
||||||
std::getline(ss, cell, ',');
|
|
||||||
|
|
||||||
std::vector<JointPoint> joints;
|
|
||||||
|
|
||||||
// 读取每个关节角度
|
|
||||||
for (size_t i = 0; i < joint_names.size(); ++i) {
|
|
||||||
if (!std::getline(ss, cell, ',')) {
|
|
||||||
LOG(ERROR) << "数据列不足，格式错误";
|
|
||||||
}
|
|
||||||
double angle = std::stod(cell); // 字符串转 double
|
|
||||||
|
|
||||||
JointPoint cmd;
|
|
||||||
cmd.joint_name = joint_names[i];
|
|
||||||
cmd.rad = angle;
|
|
||||||
joints.push_back(cmd);
|
|
||||||
}
|
|
||||||
traj.push_back(joints);
|
|
||||||
}
|
|
||||||
|
|
||||||
file.close();
|
|
||||||
|
|
||||||
|
|
||||||
// 计算速度
|
|
||||||
double dt = 0.01; // 采样周期(s)
|
|
||||||
size_t N = traj.size();
|
|
||||||
|
|
||||||
for (size_t i = 0; i < N; ++i) {
|
|
||||||
if (i == 0) {
|
|
||||||
// 第一个点没有前一帧，速度设为 0
|
|
||||||
for (auto &cmd : traj[i]) {
|
|
||||||
cmd.vel = 0.0;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// 后向差分
|
|
||||||
for (size_t j = 0; j < traj[i].size(); ++j) {
|
|
||||||
double pos_prev = traj[i - 1][j].rad;
|
|
||||||
double pos_curr = traj[i][j].rad;
|
|
||||||
traj[i][j].vel = (pos_curr - pos_prev) / dt;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// // 测试打印读取结果
|
|
||||||
// for (size_t i = 0; i < traj.size(); ++i) {
|
|
||||||
// std::cout << "Index " << i << ":" << std::endl;
|
|
||||||
// for (const auto& cmd : traj[i]) {
|
|
||||||
// std::cout << cmd.joint_name
|
|
||||||
// << " = " << cmd.rad << " rad, "
|
|
||||||
// << cmd.vel << " rad/s; ";
|
|
||||||
// }
|
|
||||||
// std::cout << std::endl;
|
|
||||||
// }
|
|
||||||
|
|
||||||
|
|
||||||
// 1：
|
|
||||||
|
|
||||||
|
|
||||||
//
|
|
||||||
// std::vector<JointPoint> cmd{};
|
// std::vector<JointPoint> cmd{};
|
||||||
// cmd = {
|
// cmd = {
|
||||||
// // {"L_SHOULDER_P", 0.0},
|
|
||||||
// // {"L_SHOULDER_R", 0.0},
|
|
||||||
// // {"L_SHOULDER_Y", 0.0},
|
|
||||||
// // {"L_ELBOW_R", 0.0},
|
|
||||||
// // {"L_WRIST_P", 0.0},
|
|
||||||
// // {"L_WRIST_Y", 0.0},
|
|
||||||
// // {"L_WRIST_R", 0.0},
|
|
||||||
//
|
|
||||||
// {"R_SHOULDER_P", 0.0},
|
// {"R_SHOULDER_P", 0.0},
|
||||||
// {"R_SHOULDER_R", 0.0},
|
// {"R_SHOULDER_R", 0.0},
|
||||||
// {"R_SHOULDER_Y", 0.0},
|
// {"R_SHOULDER_Y", 0.0},
|
||||||
// {"R_ELBOW_R", 0.0},
|
// {"R_ELBOW_R", 0.0},
|
||||||
// {"R_WRIST_P", 0.0},
|
// {"R_WRIST_P", 0.0},
|
||||||
// {"R_WRIST_Y", 0.0},
|
// {"R_WRIST_Y", 0.0},
|
||||||
// {"R_WRIST_R", 0.0}
|
// {"R_WRIST_R", 0.0},
|
||||||
// };
|
// };
|
||||||
// robot->moveJ(cmd,0.8);
|
auto joint_cmd = buildRightArmJointCmd(q_now, 0.8);
|
||||||
|
ASSERT_NO_THROW(robot->servoJ(joint_cmd, 0.8, 0.01));
|
||||||
// // // 先到达轨迹起点
|
while (true) {
|
||||||
// LOG(INFO) << "Moving to trajectory start...";
|
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
|
||||||
// robot->moveJ(traj[0],0.8);
|
}
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
// // 再移动
|
|
||||||
// LOG(INFO) << "Reached trajectory start point";
|
|
||||||
// robot->followJointTrajectory(traj,dt * 1000);
|
|
||||||
// LOG(INFO) << "Trajectory execution completed";
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// std::vector<JointPoint> cmd1{
|
|
||||||
// {"L_SHOULDER_P", -0.747573},
|
|
||||||
// {"L_SHOULDER_R", -1.26911},
|
|
||||||
// {"L_SHOULDER_Y", -1.20811},
|
|
||||||
// {"L_ELBOW_R", -1.51221},
|
|
||||||
// {"L_WRIST_P", 2.64099},
|
|
||||||
// {"L_WRIST_Y", 0.417608},
|
|
||||||
// {"L_WRIST_R", -0.518287},
|
|
||||||
//
|
|
||||||
// {"R_SHOULDER_P", -0.344938},
|
|
||||||
// {"R_SHOULDER_R", 0.935147},
|
|
||||||
// {"R_SHOULDER_Y", 2.27031},
|
|
||||||
// {"R_ELBOW_R", 1.68959},
|
|
||||||
// {"R_WRIST_P", -2.32841},
|
|
||||||
// {"R_WRIST_Y", 0.460145},
|
|
||||||
// {"R_WRIST_R", 0.300996}
|
|
||||||
// };
|
|
||||||
//
|
|
||||||
// std::vector<JointPoint> cmd2{
|
|
||||||
// {"L_SHOULDER_P", -0.747573},
|
|
||||||
// {"L_SHOULDER_R", -1.26911},
|
|
||||||
// {"L_SHOULDER_Y", -1.20811},
|
|
||||||
// {"L_ELBOW_R", -1.51221},
|
|
||||||
// {"L_WRIST_P", 2.64099},
|
|
||||||
// {"L_WRIST_Y", 0.417608},
|
|
||||||
// {"L_WRIST_R", -0.518287},
|
|
||||||
//
|
|
||||||
// {"R_SHOULDER_P", 0.239368},
|
|
||||||
// {"R_SHOULDER_R", 0.871341},
|
|
||||||
// {"R_SHOULDER_Y", 1.86052},
|
|
||||||
// {"R_ELBOW_R", 1.28044},
|
|
||||||
// {"R_WRIST_P", -2.49436},
|
|
||||||
// {"R_WRIST_Y", 0.404731},
|
|
||||||
// {"R_WRIST_R", 0.280016}
|
|
||||||
// };
|
|
||||||
|
|
||||||
//
|
|
||||||
// std::vector<JointPoint> cmd1{
|
|
||||||
// {"L_SHOULDER_P", -0.747573},
|
|
||||||
// {"L_SHOULDER_R", -1.26911},
|
|
||||||
// {"L_SHOULDER_Y", -1.20811},
|
|
||||||
// {"L_ELBOW_R", -1.51221},
|
|
||||||
// {"L_WRIST_P", 2.64099},
|
|
||||||
// {"L_WRIST_Y", 0.417608},
|
|
||||||
// {"L_WRIST_R", -0.518287},
|
|
||||||
//
|
|
||||||
// {"R_SHOULDER_P", -0.956276},
|
|
||||||
// {"R_SHOULDER_R", 1.0244},
|
|
||||||
// {"R_SHOULDER_Y", 2.70621},
|
|
||||||
// {"R_ELBOW_R", 2.02276},
|
|
||||||
// {"R_WRIST_P", -1.99653},
|
|
||||||
// {"R_WRIST_Y", 0.68523},
|
|
||||||
// {"R_WRIST_R", 0.477066}
|
|
||||||
// };
|
|
||||||
//
|
|
||||||
// std::vector<JointPoint> cmd2{
|
|
||||||
// {"L_SHOULDER_P", -0.747573},
|
|
||||||
// {"L_SHOULDER_R", -1.26911},
|
|
||||||
// {"L_SHOULDER_Y", -1.20811},
|
|
||||||
// {"L_ELBOW_R", -1.51221},
|
|
||||||
// {"L_WRIST_P", 2.64099},
|
|
||||||
// {"L_WRIST_Y", 0.417608},
|
|
||||||
// {"L_WRIST_R", -0.518287},
|
|
||||||
//
|
|
||||||
// {"R_SHOULDER_P", 0.226871},
|
|
||||||
// {"R_SHOULDER_R", 0.624717},
|
|
||||||
// {"R_SHOULDER_Y", 1.15086},
|
|
||||||
// {"R_ELBOW_R", 1.30365},
|
|
||||||
// {"R_WRIST_P", -2.14683},
|
|
||||||
// {"R_WRIST_Y", 0.196003},
|
|
||||||
// {"R_WRIST_R", 0.106678}
|
|
||||||
// };
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//
|
|
||||||
// while (true) {
|
|
||||||
// robot->moveJ(cmd1,0.8);
|
|
||||||
// robot->moveJ(cmd2,0.8);
|
|
||||||
// std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
||||||
// }
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST(HumanoidRobotTest,IBVSWithRealRobot) {
|
||||||
TEST(HumanoidRobotTest,MoveDeltaTest) {
|
const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml");
|
||||||
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
|
||||||
const XmlNode config(config_path);
|
|
||||||
|
|
||||||
if (!config.hasChild("DeviceManager")){
|
|
||||||
LOG(ERROR) << "Device Manager node not found";
|
|
||||||
}
|
|
||||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
|
||||||
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
|
||||||
|
|
||||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
|
||||||
|
|
||||||
cmvr::msgs::Pose3d pose;
|
|
||||||
pose.mutable_position()->set_x( 0);
|
|
||||||
pose.mutable_position()->set_y(0);
|
|
||||||
pose.mutable_position()->set_z(0);
|
|
||||||
|
|
||||||
pose.mutable_euler()->set_rx(0);
|
|
||||||
pose.mutable_euler()->set_ry(0);
|
|
||||||
pose.mutable_euler()->set_rz(-0.7854);
|
|
||||||
|
|
||||||
try {
|
|
||||||
// robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.1);
|
|
||||||
// robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.01);
|
|
||||||
robot->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.05);
|
|
||||||
}catch (std::exception &e) {
|
|
||||||
LOG(INFO) << e.what();
|
|
||||||
robot->torqueOff();
|
|
||||||
LOG(INFO) << e.what();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(HumanoidRobotTest,AngleToTest) {
|
|
||||||
double rx = 0.9497;
|
|
||||||
double ry = 1.6082;
|
|
||||||
double rz = -2.4832;
|
|
||||||
|
|
||||||
Eigen::Matrix3d R = HumanoidRobot<7>::eulerZYXToRotationMatrix(rx, ry, rz);
|
|
||||||
Eigen::Vector3d euler = HumanoidRobot<7>::rotationMatrixToEulerZYX(R);
|
|
||||||
|
|
||||||
std::cout << "Original Euler angles (rad):\n" << Eigen::Vector3d(rx, ry, rz).transpose() << "\n";
|
|
||||||
std::cout << "Recovered Euler angles (rad):\n" << euler.transpose() << "\n";
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
std::atomic<bool> running{true};
|
|
||||||
|
|
||||||
// 信号处理函数
|
|
||||||
void signalHandler(int signum) {
|
|
||||||
std::cout << "\nInterrupt signal (" << signum << ") received. Stopping..." << std::endl;
|
|
||||||
running = false;
|
|
||||||
}
|
|
||||||
TEST(HumanoidRobotTest, MoveIKTest) {
|
|
||||||
// 注册信号处理
|
|
||||||
std::signal(SIGINT, signalHandler);
|
|
||||||
|
|
||||||
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
|
||||||
const XmlNode config(config_path);
|
|
||||||
|
|
||||||
if (!config.hasChild("DeviceManager")) {
|
|
||||||
LOG(ERROR) << "Device Manager node not found";
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
|
||||||
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
|
||||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
|
||||||
|
|
||||||
auto cur_pose = robot->fk("PELVIS_S","R_FINGER_TIP");
|
|
||||||
|
|
||||||
cmvr::msgs::Pose3d pose;
|
|
||||||
pose.mutable_position()->set_x(0.047436);
|
|
||||||
pose.mutable_position()->set_y(-0.2884);
|
|
||||||
pose.mutable_position()->set_z(-0.605183);
|
|
||||||
pose.mutable_euler()->set_rx(-1.191976);
|
|
||||||
pose.mutable_euler()->set_ry(1.397317);
|
|
||||||
pose.mutable_euler()->set_rz(2.721208);
|
|
||||||
// Pose: {'x': 0.047436, 'y': -0.2884, 'z': -0.605183, 'rx': -1.191976, 'ry': 1.397317, 'rz': 2.721208}
|
|
||||||
|
|
||||||
|
|
||||||
// 打印关节状态线程
|
|
||||||
std::thread joint_state_thread([&]() {
|
|
||||||
while (running) {
|
|
||||||
auto joint_state = robot->getJointQ();
|
|
||||||
if (auto it = joint_state.find("WAIST_P"); it != joint_state.end()) {
|
|
||||||
LOG(INFO) << "WAIST_P POS = " << it->second;
|
|
||||||
} else {
|
|
||||||
LOG(WARNING) << "WAIST_P not found in joint_state";
|
|
||||||
}
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
// 动作控制线程
|
|
||||||
std::thread motion_thread([&]() {
|
|
||||||
while (running) {
|
|
||||||
robot->moveJ("PELVIS_S","R_FINGER_TIP",pose,0.5);
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
|
||||||
robot->moveJ("PELVIS_S","R_FINGER_TIP",cur_pose,0.5);
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
// 等待线程结束
|
|
||||||
if (motion_thread.joinable()) motion_thread.join();
|
|
||||||
if (joint_state_thread.joinable()) joint_state_thread.join();
|
|
||||||
|
|
||||||
LOG(INFO) << "Test finished gracefully.";
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
TEST(HumanoidRobotTest,MoveLTest) {
|
|
||||||
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
|
|
||||||
const XmlNode config(config_path);
|
|
||||||
|
|
||||||
if (!config.hasChild("DeviceManager")){
|
|
||||||
LOG(ERROR) << "Device Manager node not found";
|
|
||||||
}
|
|
||||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
|
||||||
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
|
||||||
|
|
||||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
|
||||||
|
|
||||||
|
|
||||||
try {
|
|
||||||
// 定义基座和末端链路名（需与URDF模型中的链路名一致）
|
|
||||||
std::string base_link = "PELVIS_S"; // 基座坐标系
|
|
||||||
std::string ee_link = "R_WRIST_R_S"; // 末端执行器坐标系
|
|
||||||
|
|
||||||
// 定义目标位姿（直线运动的终点与当前的差值）
|
|
||||||
cmvr::msgs::Pose3d delta_pose;
|
|
||||||
delta_pose.mutable_position()->set_x(0.2);
|
|
||||||
delta_pose.mutable_position()->set_y(0);
|
|
||||||
delta_pose.mutable_position()->set_z(0);
|
|
||||||
delta_pose.mutable_euler()->set_rx(0);
|
|
||||||
delta_pose.mutable_euler()->set_ry(0);
|
|
||||||
delta_pose.mutable_euler()->set_rz(0);
|
|
||||||
|
|
||||||
// 4. 设置运动参数（速度单位：m/s，加速度单位：m/s²）
|
|
||||||
double vel = 0.1; // 最大线速度 0.1m/s
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
double acc = 0.05; // 加速度 0.05m/s²
|
|
||||||
|
|
||||||
// 调用moveL执行直线运动
|
|
||||||
robot->moveDeltaL(base_link, ee_link, delta_pose, vel, acc);
|
|
||||||
LOG(INFO) << "直线运动完成！";
|
|
||||||
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
// 捕获异常（如IK解算失败、状态非法等）
|
|
||||||
LOG(ERROR) << "moveL调用失败：" << e.what();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
TEST(HumanoidRobotTest, SpeedJTest) {
|
|
||||||
// 配置文件路径
|
|
||||||
std::string config_path = "/home/tankaitao/cmvr-es/config/cabin_robot.xml";
|
|
||||||
const XmlNode config(config_path);
|
|
||||||
|
|
||||||
// 检查配置文件是否包含 DeviceManager 节点
|
|
||||||
if (!config.hasChild("DeviceManager")) {
|
|
||||||
LOG(ERROR) << "Device Manager node not found";
|
|
||||||
}
|
|
||||||
|
|
||||||
// 获取 DeviceManager 配置并初始化设备管理器
|
|
||||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||||
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
|
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||||
|
|
||||||
// 获取机器人实例
|
|
||||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
||||||
|
auto camera = dmgr.getDevice<AbstractCamera>("cam4");
|
||||||
|
ASSERT_NE(robot, nullptr);
|
||||||
|
ASSERT_NE(camera, nullptr);
|
||||||
|
|
||||||
// 设置目标关节和相关参数
|
ASSERT_NO_THROW(camera->start());
|
||||||
std::string joint_name = "L_WRIST_R"; // 目标关节名
|
struct RuntimeGuard {
|
||||||
RobotJointIndexDirection dir = RobotJointIndexDirection::Y_NEGATIVE; // 方向
|
std::shared_ptr<AbstractCamera> camera;
|
||||||
double vel = -0.1; // 速度 (rad/s)
|
std::shared_ptr<AbstractRobot> robot;
|
||||||
double acc = 1; // 加速度 (rad/s²)
|
~RuntimeGuard() {
|
||||||
|
if (camera) {
|
||||||
try {
|
try {
|
||||||
// 调用 speedJ 函数进行关节运动
|
camera->stop();
|
||||||
robot->speedJ(joint_name, dir, vel, acc);
|
} catch (...) {
|
||||||
|
}
|
||||||
} catch (const std::exception& e) {
|
|
||||||
LOG(ERROR) << "speedJ test failed: " << e.what();
|
|
||||||
FAIL() << "Exception thrown during speedJ test: " << e.what();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
TEST(HumanoidRobotTest, SpeedLTest) {
|
|
||||||
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
|
|
||||||
const XmlNode config(config_path);
|
|
||||||
|
|
||||||
if (!config.hasChild("DeviceManager")) {
|
|
||||||
LOG(ERROR) << "Device Manager node not found";
|
|
||||||
}
|
|
||||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
|
||||||
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
|
||||||
|
|
||||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
|
||||||
|
|
||||||
try {
|
|
||||||
|
|
||||||
// std::unordered_map<std::string,double> joint_qs;
|
|
||||||
// joint_qs.insert(std::make_pair("R_SHOULDER_P",0));
|
|
||||||
// joint_qs.insert(std::make_pair("R_SHOULDER_R",0));
|
|
||||||
// joint_qs.insert(std::make_pair("R_SHOULDER_Y",0));
|
|
||||||
// joint_qs.insert(std::make_pair("R_ELBOW_R",0));
|
|
||||||
// joint_qs.insert(std::make_pair("R_WRIST_P",0));
|
|
||||||
// joint_qs.insert(std::make_pair("R_WRIST_Y",0));
|
|
||||||
// joint_qs.insert(std::make_pair("R_WRIST_R",0));
|
|
||||||
//
|
|
||||||
// robot->getJointQ(joint_qs);
|
|
||||||
// LOG(INFO) << "======================================";
|
|
||||||
// LOG(INFO) << "获取到的关节位置信息 (单位: 弧度)";
|
|
||||||
// LOG(INFO) << "--------------------------------------";
|
|
||||||
//
|
|
||||||
// for (const auto& pair : joint_qs) {
|
|
||||||
// LOG(INFO) << "关节 " << pair.first
|
|
||||||
// << ": " << pair.second
|
|
||||||
// << " (" << pair.second * 180 / M_PI << "°)";
|
|
||||||
// }
|
|
||||||
// LOG(INFO) << "======================================";
|
|
||||||
// return ;
|
|
||||||
|
|
||||||
// 先运动到一个合适的位置
|
|
||||||
std::vector<JointPoint> cmd{
|
|
||||||
{"R_SHOULDER_P", -0.348663},
|
|
||||||
{"R_SHOULDER_R", 1.1512},
|
|
||||||
{"R_SHOULDER_Y", 1.64856},
|
|
||||||
{"R_ELBOW_R", 1.8978},
|
|
||||||
{"R_WRIST_P", -2.8002},
|
|
||||||
{"R_WRIST_Y", -0.0262979},
|
|
||||||
{"R_WRIST_R", 0.0449828}
|
|
||||||
};
|
|
||||||
robot->moveJ(cmd);
|
|
||||||
// 定义运动方向（笛卡尔坐标系）
|
|
||||||
RobotCartesian cart_direction = RobotCartesian::Y; // 沿X轴移动
|
|
||||||
|
|
||||||
// 定义运动方向（正向或反向）
|
|
||||||
RobotJointIndexDirection move_direction = RobotJointIndexDirection::ROTATE_Y; // 正向
|
|
||||||
|
|
||||||
// 设置运动参数（速度单位：m/s，加速度单位：m/s²）
|
|
||||||
double vel = 0.02; // 速度 0.1m/s
|
|
||||||
double acc = 0.05; // 加速度 0.05m/s²
|
|
||||||
|
|
||||||
LOG(INFO) << "开始执行速度控制，方向：X轴正向，速度：" << vel << "m/s，加速度：" << acc << "m/s²";
|
|
||||||
|
|
||||||
// 调用speedL执行速度控制
|
|
||||||
robot->speedL(cart_direction, move_direction, vel, acc);
|
|
||||||
|
|
||||||
// 等待一段时间让机器人运动
|
|
||||||
std::this_thread::sleep_for(std::chrono::seconds(5));
|
|
||||||
|
|
||||||
// 停止运动（假设有停止函数）
|
|
||||||
// robot->stopSpeedL();
|
|
||||||
|
|
||||||
LOG(INFO) << "速度控制完成！";
|
|
||||||
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
// 捕获异常（如IK解算失败、状态非法等）
|
|
||||||
LOG(ERROR) << "speedL调用失败：" << e.what();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(HumanoidRobotTest, followPoseTest) {
|
|
||||||
// 1. 配置文件与设备初始化
|
|
||||||
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
|
|
||||||
const XmlNode config(config_path);
|
|
||||||
|
|
||||||
if (!config.hasChild("DeviceManager")) {
|
|
||||||
LOG(ERROR) << "Device Manager node not found in config file";
|
|
||||||
return; // 配置错误，直接退出测试
|
|
||||||
}
|
|
||||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
|
||||||
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
|
||||||
|
|
||||||
// 获取机器人设备（hc01）
|
|
||||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
|
||||||
if (!robot) {
|
|
||||||
LOG(ERROR) << "Failed to get robot device 'hc01'";
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
try {
|
|
||||||
// 2. 核心参数配置
|
|
||||||
std::string base_link = "PELVIS_S"; // 基座坐标系（轨迹相对此坐标系）
|
|
||||||
std::string ee_link = "R_WRIST_R_S"; // 待控制的末端执行器（如右手腕）
|
|
||||||
const double circle_radius = 0.03; // 圆形轨迹半径（3厘米，根据工作空间调整）
|
|
||||||
const double angle_start = 0.0; // 轨迹起始角度（0弧度）
|
|
||||||
const double angle_end = 2 * M_PI; // 轨迹结束角度（2π，完整圆形）
|
|
||||||
const int num_points = 100; // 轨迹总点数（点数越多，轨迹越平滑）
|
|
||||||
const double dt = 0.02; // 相邻轨迹点的时间间隔（20ms，控制运动速度）
|
|
||||||
|
|
||||||
// 选择轨迹所在平面（保持某一轴坐标不变）
|
|
||||||
// 0: XY平面（Z轴固定） | 1: XZ平面（Y轴固定） | 2: YZ平面（X轴固定）
|
|
||||||
const int plane_choice = 2;
|
|
||||||
const std::string plane_desc = (plane_choice == 0) ? "XY平面（Z轴不变）" :
|
|
||||||
(plane_choice == 1) ? "XZ平面（Y轴不变）" : "YZ平面（X轴不变）";
|
|
||||||
LOG(INFO) << "Trajectory plane selected: " << plane_desc;
|
|
||||||
|
|
||||||
|
|
||||||
// 3. 获取末端执行器当前位姿（作为圆形轨迹的圆心）
|
|
||||||
LOG(INFO) << "Getting current pose of end-effector (" << ee_link << ") relative to base (" << base_link << ")";
|
|
||||||
const cmvr::math::Pose3d current_pose = robot->getTransform(base_link, ee_link);
|
|
||||||
|
|
||||||
// 将当前位姿转换为Eigen::Matrix4d（圆心位姿矩阵，包含位置和姿态）
|
|
||||||
Eigen::Matrix4d center_T = Eigen::Matrix4d::Identity(); // 初始化单位矩阵（姿态为默认，位置待填充）
|
|
||||||
// 3.1 填充圆心位置（从当前位姿的position提取）
|
|
||||||
center_T(0, 3) = current_pose.position.x; // 圆心X坐标（当前末端X）
|
|
||||||
center_T(1, 3) = current_pose.position.y; // 圆心Y坐标（当前末端Y）
|
|
||||||
center_T(2, 3) = current_pose.position.z; // 圆心Z坐标（当前末端Z）
|
|
||||||
LOG(INFO) << "Circle center pose (XYZ): ["
|
|
||||||
<< center_T(0,3) << ", " << center_T(1,3) << ", " << center_T(2,3) << "] (m)";
|
|
||||||
|
|
||||||
// 3.2 填充圆心姿态（从当前位姿的四元数转换为旋转矩阵）
|
|
||||||
const double w = current_pose.quaternion.w;
|
|
||||||
const double x = current_pose.quaternion.x;
|
|
||||||
const double y = current_pose.quaternion.y;
|
|
||||||
const double z = current_pose.quaternion.z;
|
|
||||||
// 四元数转旋转矩阵（标准公式，确保末端姿态与当前一致）
|
|
||||||
center_T.block<3,3>(0,0) << 1-2*y*y-2*z*z, 2*x*y-2*z*w, 2*x*z+2*y*w,
|
|
||||||
2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w,
|
|
||||||
2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x*x-2*y*y;
|
|
||||||
|
|
||||||
|
|
||||||
// 4. 生成指定平面内的圆形轨迹（格式：std::vector<std::vector<cmvr::ctrl::PoseTarget>>）
|
|
||||||
std::vector<std::vector<cmvr::ctrl::PoseTarget>> trajectory;
|
|
||||||
const double angle_step = (angle_end - angle_start) / num_points; // 每步角度增量
|
|
||||||
|
|
||||||
for (int i = 0; i <= num_points; ++i) {
|
|
||||||
const double current_angle = angle_start + i * angle_step; // 当前轨迹点角度
|
|
||||||
Eigen::Matrix4d point_T = center_T; // 复制圆心位姿，在此基础上偏移生成轨迹点
|
|
||||||
|
|
||||||
// 根据选择的平面，计算位置偏移（保持对应轴不变）
|
|
||||||
switch (plane_choice) {
|
|
||||||
case 0: // XY平面：Z轴固定，X/Y随角度变化
|
|
||||||
point_T(0, 3) += circle_radius * cos(current_angle); // X方向偏移
|
|
||||||
point_T(1, 3) += circle_radius * sin(current_angle); // Y方向偏移
|
|
||||||
break;
|
|
||||||
case 1: // XZ平面：Y轴固定，X/Z随角度变化
|
|
||||||
point_T(0, 3) += circle_radius * cos(current_angle); // X方向偏移
|
|
||||||
point_T(2, 3) += circle_radius * sin(current_angle); // Z方向偏移
|
|
||||||
break;
|
|
||||||
case 2: // YZ平面：X轴固定，Y/Z随角度变化
|
|
||||||
point_T(1, 3) += circle_radius * cos(current_angle); // Y方向偏移
|
|
||||||
point_T(2, 3) += circle_radius * sin(current_angle); // Z方向偏移
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
LOG(WARNING) << "Invalid plane choice (" << plane_choice << "), use default XY plane";
|
|
||||||
point_T(0, 3) += circle_radius * cos(current_angle);
|
|
||||||
point_T(1, 3) += circle_radius * sin(current_angle);
|
|
||||||
}
|
}
|
||||||
|
if (robot) {
|
||||||
|
try {
|
||||||
|
robot->eStop();
|
||||||
|
} catch (...) {
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} guard{camera, robot};
|
||||||
|
|
||||||
// 构造当前时间步的PoseTarget（控制末端执行器）
|
cmvr::IbvsController ibvs_controller;
|
||||||
cmvr::ctrl::PoseTarget ee_target;
|
ibvs_controller.setMu(0.1);
|
||||||
ee_target.link_name = ee_link; // 目标控制的link（末端执行器）
|
ibvs_controller.setQdotMax(0.6);
|
||||||
ee_target.T_target = point_T; // 相对base_link的目标位姿矩阵
|
ibvs_controller.setTagSize(0.12);
|
||||||
ee_target.w_posrot = 0.5; // 位置/姿态权重（0.5：位置和姿态同等重要）
|
ibvs_controller.setTrackedTagId(0);
|
||||||
ee_target.weight = 1.0; // 任务权重（1.0：硬约束，必须满足）
|
ibvs_controller.setTarget(0.0, 0.0, 0.35, 3.14159265358979323846, 0.0, 0.0);
|
||||||
|
ibvs_controller.setDepthMode(cmvr::IbvsController::DepthMode::MONOCULAR);
|
||||||
|
ibvs_controller.setDepthZGain(1.0);
|
||||||
|
|
||||||
// 每个时间步可控制多个link（此处仅控制一个末端，故vector大小为1）
|
ASSERT_TRUE(ibvs_controller.init(camera,
|
||||||
std::vector<cmvr::ctrl::PoseTarget> step_targets = {ee_target};
|
"/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
|
||||||
trajectory.push_back(step_targets); // 将当前时间步目标加入轨迹
|
"PELVIS_S",
|
||||||
|
"R_WRIST_R_S",
|
||||||
|
"R_CAM"))
|
||||||
|
<< "IbvsController init failed";
|
||||||
|
|
||||||
|
auto q_map_now = makeRightArmQMap();
|
||||||
|
robot->getJointQ(q_map_now);
|
||||||
|
std::vector<double> q_now(kRightArmJointNames.size(), 0.0);
|
||||||
|
auto refreshRightArmQ = [&]() -> bool {
|
||||||
|
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
|
||||||
|
const auto it = q_map_now.find(kRightArmJointNames[i]);
|
||||||
|
if (it == q_map_now.end()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
q_now[i] = it->second;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
};
|
||||||
|
|
||||||
|
ASSERT_TRUE(refreshRightArmQ())
|
||||||
|
<< "Failed to extract right-arm 7 joints from robot state";
|
||||||
|
ibvs_controller.reset(q_now);
|
||||||
|
|
||||||
|
const int max_steps = 300;
|
||||||
|
const int log_every = 10;
|
||||||
|
const int cycle_ms = 33;
|
||||||
|
|
||||||
|
int ok_steps = 0;
|
||||||
|
int fail_steps = 0;
|
||||||
|
auto t_prev = std::chrono::steady_clock::now();
|
||||||
|
|
||||||
|
for (int step = 0; step < max_steps; ++step) {
|
||||||
|
robot->getJointQ(q_map_now);
|
||||||
|
if (!refreshRightArmQ()) {
|
||||||
|
++fail_steps;
|
||||||
|
if ((step % log_every) == 0) {
|
||||||
|
std::cout << "[IBVS_REAL] step=" << step << " joint extract failed\n";
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
|
||||||
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 输出轨迹信息（调试用）
|
const auto t_now = std::chrono::steady_clock::now();
|
||||||
LOG(INFO) << "Trajectory generated successfully: "
|
double dt = std::chrono::duration<double>(t_now - t_prev).count();
|
||||||
<< trajectory.size() << " time steps, radius: " << circle_radius << "m";
|
t_prev = t_now;
|
||||||
|
dt = std::clamp(dt, 0.001, 0.1);
|
||||||
|
|
||||||
|
std::vector<double> q_cmd_next;
|
||||||
|
const bool ok = ibvs_controller.compute(q_now, dt, q_cmd_next);
|
||||||
|
if (!ok || q_cmd_next.size() != kRightArmJointNames.size()) {
|
||||||
|
++fail_steps;
|
||||||
|
if ((step % log_every) == 0) {
|
||||||
|
std::cout << "[IBVS_REAL] step=" << step
|
||||||
|
<< " compute failed: "
|
||||||
|
<< cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus())
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
// 5. 执行轨迹（调用followPoseTrajectory接口）
|
auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.8);
|
||||||
LOG(INFO) << "Starting circular trajectory execution...";
|
robot->servoJ(joint_cmd, 0.8, dt);
|
||||||
robot->followPoseTrajectory(base_link,trajectory, dt); // 传入轨迹和时间间隔
|
++ok_steps;
|
||||||
|
|
||||||
LOG(INFO) << "Circular trajectory execution completed!";
|
if ((step % log_every) == 0) {
|
||||||
|
const auto& t_co = ibvs_controller.lastTagPositionVisp();
|
||||||
|
const auto& v_c = ibvs_controller.lastCameraTwistVisp();
|
||||||
|
std::cout << "[IBVS_REAL] step=" << step
|
||||||
|
<< " tag=[" << t_co.x() << ", " << t_co.y() << ", " << t_co.z() << "]"
|
||||||
|
<< " v_c=[" << v_c[0] << ", " << v_c[1] << ", " << v_c[2]
|
||||||
|
<< ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]"
|
||||||
|
<< " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage())
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
} catch (const std::exception& e) {
|
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
|
||||||
// 捕获并打印异常（如机器人未就绪、逆解失败等）
|
|
||||||
LOG(ERROR) << "Trajectory execution failed: " << e.what();
|
|
||||||
throw; // 可选：抛出异常让测试框架捕获，标记测试失败
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::cout << "[IBVS_REAL] finished steps=" << max_steps
|
||||||
|
<< " ok_steps=" << ok_steps
|
||||||
|
<< " fail_steps=" << fail_steps
|
||||||
|
<< std::endl;
|
||||||
|
|
||||||
|
EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps.";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -21,9 +21,8 @@ target_link_libraries(ik_solver PUBLIC
|
|||||||
)
|
)
|
||||||
|
|
||||||
add_library(cmvr_es::ik_solver ALIAS ik_solver)
|
add_library(cmvr_es::ik_solver ALIAS ik_solver)
|
||||||
install(TARGETS ik_solver RUNTIME DESTINATION bin)
|
|
||||||
|
|
||||||
|
|
||||||
|
install(TARGETS ik_solver LIBRARY DESTINATION lib)
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
# Unit test
|
# Unit test
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
|
|||||||
30
cmvr-es/perception/CMakeLists.txt
Normal file
30
cmvr-es/perception/CMakeLists.txt
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
find_package(VISP REQUIRED)
|
||||||
|
find_package(OpenCV REQUIRED)
|
||||||
|
|
||||||
|
add_library(perception SHARED
|
||||||
|
src/tag_relative_target_3d.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
|
target_link_libraries(perception PUBLIC
|
||||||
|
${VISP_LIBRARIES}
|
||||||
|
${OpenCV_LIBS}
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(cmvr_es::perception ALIAS perception)
|
||||||
|
install(TARGETS perception LIBRARY DESTINATION lib)
|
||||||
|
|
||||||
|
add_executable(tag_relative_target_3d_test
|
||||||
|
src/tag_relative_target_3d_test.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_link_libraries(tag_relative_target_3d_test
|
||||||
|
PRIVATE
|
||||||
|
cmvr_es::perception
|
||||||
|
cmvr_es::device::realsense_camera
|
||||||
|
cmvr_es::proto
|
||||||
|
glog
|
||||||
|
gtest
|
||||||
|
gtest_main
|
||||||
|
pthread
|
||||||
|
)
|
||||||
215
cmvr-es/perception/include/tag_relative_target_3d.h
Normal file
215
cmvr-es/perception/include/tag_relative_target_3d.h
Normal file
@ -0,0 +1,215 @@
|
|||||||
|
//
|
||||||
|
// Created by lgv on 2026/2/26.
|
||||||
|
//
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
|
#include <unordered_map>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include <Eigen/Dense>
|
||||||
|
#include <visp3/detection/vpDetectorAprilTag.h>
|
||||||
|
#include "devices/camera/abstract_camera.h"
|
||||||
|
|
||||||
|
namespace cmvr::perception {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 单个 tag 观测：tag 相对于相机的位姿（`T_c_t`）。
|
||||||
|
*/
|
||||||
|
struct TagPoseObservation {
|
||||||
|
// tag id。
|
||||||
|
int tag_id{-1};
|
||||||
|
// tag 坐标系 -> 相机坐标系 变换。
|
||||||
|
Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
|
||||||
|
// 观测权重（<=0 或非法时按 1.0 处理）。
|
||||||
|
double weight{1.0};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 维护“目标点在各 tag 坐标系下的 3D 相对坐标”，并支持在线恢复目标点相机坐标。
|
||||||
|
*
|
||||||
|
* 典型流程：
|
||||||
|
* 1) 锁定目标时已知目标点相机坐标 `p_c_target` 与可见 tag 位姿观测；
|
||||||
|
* 2) 计算并缓存每个可见 tag 下的 `p_ti_target`；
|
||||||
|
* 3) 运行时只要某个已缓存的 tag 可见，就可恢复目标点在当前相机系下位置。
|
||||||
|
*/
|
||||||
|
class TagRelativeTarget3D {
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* @brief 最近一次调用状态。
|
||||||
|
*/
|
||||||
|
enum class Status {
|
||||||
|
OK = 0, // 计算成功
|
||||||
|
INVALID_INPUT, // 输入参数非法
|
||||||
|
NO_CAMERA, // 未注入相机
|
||||||
|
NO_NEW_FRAME, // 相机未取到新图像
|
||||||
|
BAD_IMAGE, // 图像或内参非法
|
||||||
|
NO_TAG, // 未检测到 tag
|
||||||
|
NO_OBSERVATION, // 无可用观测
|
||||||
|
TARGET_NOT_LOCKED, // 尚未缓存任何 tag 锚点
|
||||||
|
NO_MATCHING_TAG // 有观测，但没有命中已缓存的 tag
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 构造函数，支持直接注入抽象相机。
|
||||||
|
* @param camera 抽象相机，可为空（后续可仅使用几何接口）。
|
||||||
|
*/
|
||||||
|
explicit TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera = nullptr);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 设置/替换相机对象。
|
||||||
|
* @param camera 抽象相机。
|
||||||
|
*/
|
||||||
|
void setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera) { camera_ = camera; }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 清空全部 tag 锚点。
|
||||||
|
*/
|
||||||
|
void clear();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 是否没有任何缓存锚点。
|
||||||
|
* @return 为空返回 `true`。
|
||||||
|
*/
|
||||||
|
bool empty() const { return anchors_in_tag_.empty(); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 当前缓存锚点数量。
|
||||||
|
* @return 锚点数量。
|
||||||
|
*/
|
||||||
|
size_t anchorCount() const { return anchors_in_tag_.size(); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 获取最近状态。
|
||||||
|
* @return 最近状态枚举。
|
||||||
|
*/
|
||||||
|
Status lastStatus() const { return last_status_; }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 状态枚举转字符串。
|
||||||
|
* @param status 状态枚举。
|
||||||
|
* @return 字符串常量。
|
||||||
|
*/
|
||||||
|
static const char* statusToString(Status status);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 设置 AprilTag 边长。
|
||||||
|
* @param tag_size_m tag 边长（米）。
|
||||||
|
*/
|
||||||
|
void setTagSize(double tag_size_m);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 通过内部相机抓帧并检测 tag 观测（`T_c_t`）。
|
||||||
|
* @param observations 输出观测列表。
|
||||||
|
* @return 检测成功返回 `true`。
|
||||||
|
*/
|
||||||
|
bool detectObservations(std::vector<TagPoseObservation>& observations);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 使用外部图像与内参检测 tag 观测（`T_c_t`）。
|
||||||
|
* @param color 输入彩色/灰度图。
|
||||||
|
* @param intrinsics 相机内参。
|
||||||
|
* @param observations 输出观测列表。
|
||||||
|
* @return 检测成功返回 `true`。
|
||||||
|
*/
|
||||||
|
bool detectObservationsFromImage(const cv::Mat& color,
|
||||||
|
const device::Rs2Intrinsics& intrinsics,
|
||||||
|
std::vector<TagPoseObservation>& observations);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 使用内部相机检测结果直接建立/更新锚点。
|
||||||
|
* @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。
|
||||||
|
* @param overwrite_existing 是否覆盖已存在锚点。
|
||||||
|
* @return 成功建立至少一个锚点返回 `true`。
|
||||||
|
*/
|
||||||
|
bool lockTargetInCamera(const Eigen::Vector3d& p_c_target,
|
||||||
|
bool overwrite_existing = true);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 使用内部相机检测结果直接恢复目标点相机坐标。
|
||||||
|
* @param p_c_target_out 输出目标点相机坐标（米）。
|
||||||
|
* @param used_tag_id_out 若非空，输出主导 tag id。
|
||||||
|
* @param spread_out 若非空，输出多 tag 融合离散误差（米）。
|
||||||
|
* @return 恢复成功返回 `true`。
|
||||||
|
*/
|
||||||
|
bool resolveTargetInCamera(Eigen::Vector3d& p_c_target_out,
|
||||||
|
int* used_tag_id_out = nullptr,
|
||||||
|
double* spread_out = nullptr);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 根据“目标点相机坐标 + 可见 tag 观测”批量建立/更新各 tag 锚点。
|
||||||
|
* @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。
|
||||||
|
* @param observations 可见 tag 观测集合。
|
||||||
|
* @param overwrite_existing 是否覆盖已存在的同 id 锚点。
|
||||||
|
* @return 成功建立至少一个锚点时返回 `true`。
|
||||||
|
*/
|
||||||
|
bool lockTargetInCamera(const Eigen::Vector3d& p_c_target,
|
||||||
|
const std::vector<TagPoseObservation>& observations,
|
||||||
|
bool overwrite_existing = true);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 用单个 tag 观测建立/更新锚点。
|
||||||
|
* @param tag_id tag id。
|
||||||
|
* @param T_c_t tag 坐标系到相机坐标系变换。
|
||||||
|
* @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。
|
||||||
|
* @return 建立成功返回 `true`。
|
||||||
|
*/
|
||||||
|
bool upsertAnchorFromCamera(int tag_id,
|
||||||
|
const Eigen::Matrix4d& T_c_t,
|
||||||
|
const Eigen::Vector3d& p_c_target);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 根据当前可见 tag，恢复目标点在当前相机坐标系下的 3D 坐标。
|
||||||
|
* @param observations 当前可见 tag 观测。
|
||||||
|
* @param p_c_target_out 输出目标点相机坐标（米）。
|
||||||
|
* @param used_tag_id_out 若非空，输出本次主导 tag id（仅 1 个匹配时等于该 id，多匹配时为最大权重 id）。
|
||||||
|
* @param spread_out 若非空，输出多 tag 融合后最大离散误差（米）。
|
||||||
|
* @return 恢复成功返回 `true`。
|
||||||
|
*/
|
||||||
|
bool resolveTargetInCamera(const std::vector<TagPoseObservation>& observations,
|
||||||
|
Eigen::Vector3d& p_c_target_out,
|
||||||
|
int* used_tag_id_out = nullptr,
|
||||||
|
double* spread_out = nullptr);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 是否已缓存指定 tag 的锚点。
|
||||||
|
* @param tag_id tag id。
|
||||||
|
* @return 命中返回 `true`。
|
||||||
|
*/
|
||||||
|
bool hasAnchorForTag(int tag_id) const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 获取目标点在指定 tag 坐标系下的 3D 相对坐标。
|
||||||
|
* @param tag_id tag id。
|
||||||
|
* @param p_t_target_out 输出 `p_t_target`（米）。
|
||||||
|
* @return 获取成功返回 `true`。
|
||||||
|
*/
|
||||||
|
bool getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 获取构造时注入的相机对象。
|
||||||
|
* @return 抽象相机智能指针。
|
||||||
|
*/
|
||||||
|
const std::shared_ptr<cmvr::device::AbstractCamera>& camera() const { return camera_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
static bool isFiniteMatrix(const Eigen::Matrix4d& T);
|
||||||
|
static bool isFiniteVector(const Eigen::Vector3d& p);
|
||||||
|
static Eigen::Vector3d pointTagToCamera(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_t);
|
||||||
|
static Eigen::Vector3d pointCameraToTag(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_c);
|
||||||
|
|
||||||
|
private:
|
||||||
|
// 缓存: tag_id -> 目标点在该 tag 坐标系下的 3D 坐标。
|
||||||
|
std::unordered_map<int, Eigen::Vector3d> anchors_in_tag_;
|
||||||
|
// 抽象相机对象（由构造函数注入）。
|
||||||
|
std::shared_ptr<cmvr::device::AbstractCamera> camera_{nullptr};
|
||||||
|
// AprilTag 检测器。
|
||||||
|
vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11};
|
||||||
|
// tag 边长（米）。
|
||||||
|
double tag_size_m_{0.12};
|
||||||
|
// 最近一次调用状态。
|
||||||
|
Status last_status_{Status::TARGET_NOT_LOCKED};
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace cmvr::perception
|
||||||
356
cmvr-es/perception/src/tag_relative_target_3d.cpp
Normal file
356
cmvr-es/perception/src/tag_relative_target_3d.cpp
Normal file
@ -0,0 +1,356 @@
|
|||||||
|
//
|
||||||
|
// Created by lgv on 2026/2/26.
|
||||||
|
//
|
||||||
|
|
||||||
|
#include "perception/include/tag_relative_target_3d.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cmath>
|
||||||
|
#include <cstring>
|
||||||
|
#include <limits>
|
||||||
|
|
||||||
|
#include <opencv2/imgproc.hpp>
|
||||||
|
#include <visp3/core/vpCameraParameters.h>
|
||||||
|
#include <visp3/core/vpImage.h>
|
||||||
|
|
||||||
|
namespace cmvr::perception {
|
||||||
|
|
||||||
|
TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera)
|
||||||
|
: camera_(camera) {
|
||||||
|
detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
|
||||||
|
}
|
||||||
|
|
||||||
|
const char* TagRelativeTarget3D::statusToString(Status status) {
|
||||||
|
switch (status) {
|
||||||
|
case Status::OK: return "ok";
|
||||||
|
case Status::INVALID_INPUT: return "invalid_input";
|
||||||
|
case Status::NO_CAMERA: return "no_camera";
|
||||||
|
case Status::NO_NEW_FRAME: return "no_new_frame";
|
||||||
|
case Status::BAD_IMAGE: return "bad_image";
|
||||||
|
case Status::NO_TAG: return "no_tag";
|
||||||
|
case Status::NO_OBSERVATION: return "no_observation";
|
||||||
|
case Status::TARGET_NOT_LOCKED: return "target_not_locked";
|
||||||
|
case Status::NO_MATCHING_TAG: return "no_matching_tag";
|
||||||
|
default: return "unknown";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void TagRelativeTarget3D::setTagSize(double tag_size_m) {
|
||||||
|
if (std::isfinite(tag_size_m) && tag_size_m > 0.0) {
|
||||||
|
tag_size_m_ = tag_size_m;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::detectObservations(std::vector<TagPoseObservation>& observations) {
|
||||||
|
observations.clear();
|
||||||
|
|
||||||
|
if (!camera_) {
|
||||||
|
last_status_ = Status::NO_CAMERA;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat color;
|
||||||
|
device::Rs2Intrinsics intrinsics{};
|
||||||
|
try {
|
||||||
|
// Tag 检测只需要彩色图像与相机内参，避免依赖深度流导致阻塞/超时。
|
||||||
|
camera_->getRGBImage(color, intrinsics);
|
||||||
|
} catch (const std::exception&) {
|
||||||
|
last_status_ = Status::NO_NEW_FRAME;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (color.empty()) {
|
||||||
|
last_status_ = Status::NO_NEW_FRAME;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return detectObservationsFromImage(color, intrinsics, observations);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::lockTargetInCamera(const Eigen::Vector3d& p_c_target,
|
||||||
|
bool overwrite_existing) {
|
||||||
|
std::vector<TagPoseObservation> observations;
|
||||||
|
if (!detectObservations(observations)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return lockTargetInCamera(p_c_target, observations, overwrite_existing);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::resolveTargetInCamera(Eigen::Vector3d& p_c_target_out,
|
||||||
|
int* used_tag_id_out,
|
||||||
|
double* spread_out) {
|
||||||
|
std::vector<TagPoseObservation> observations;
|
||||||
|
if (!detectObservations(observations)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return resolveTargetInCamera(observations, p_c_target_out, used_tag_id_out, spread_out);
|
||||||
|
}
|
||||||
|
|
||||||
|
void TagRelativeTarget3D::clear() {
|
||||||
|
anchors_in_tag_.clear();
|
||||||
|
last_status_ = Status::TARGET_NOT_LOCKED;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::lockTargetInCamera(const Eigen::Vector3d& p_c_target,
|
||||||
|
const std::vector<TagPoseObservation>& observations,
|
||||||
|
bool overwrite_existing) {
|
||||||
|
if (!isFiniteVector(p_c_target)) {
|
||||||
|
last_status_ = Status::INVALID_INPUT;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (observations.empty()) {
|
||||||
|
last_status_ = Status::NO_OBSERVATION;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t updated = 0;
|
||||||
|
for (const auto& obs : observations) {
|
||||||
|
if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (!overwrite_existing && anchors_in_tag_.find(obs.tag_id) != anchors_in_tag_.end()) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
anchors_in_tag_[obs.tag_id] = pointCameraToTag(obs.T_c_t, p_c_target);
|
||||||
|
++updated;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (updated == 0) {
|
||||||
|
last_status_ = Status::INVALID_INPUT;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
last_status_ = Status::OK;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::upsertAnchorFromCamera(int tag_id,
|
||||||
|
const Eigen::Matrix4d& T_c_t,
|
||||||
|
const Eigen::Vector3d& p_c_target) {
|
||||||
|
if (tag_id < 0 || !isFiniteMatrix(T_c_t) || !isFiniteVector(p_c_target)) {
|
||||||
|
last_status_ = Status::INVALID_INPUT;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
anchors_in_tag_[tag_id] = pointCameraToTag(T_c_t, p_c_target);
|
||||||
|
last_status_ = Status::OK;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::resolveTargetInCamera(const std::vector<TagPoseObservation>& observations,
|
||||||
|
Eigen::Vector3d& p_c_target_out,
|
||||||
|
int* used_tag_id_out,
|
||||||
|
double* spread_out) {
|
||||||
|
if (used_tag_id_out) {
|
||||||
|
*used_tag_id_out = -1;
|
||||||
|
}
|
||||||
|
if (spread_out) {
|
||||||
|
*spread_out = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (anchors_in_tag_.empty()) {
|
||||||
|
last_status_ = Status::TARGET_NOT_LOCKED;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (observations.empty()) {
|
||||||
|
last_status_ = Status::NO_OBSERVATION;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<Eigen::Vector3d> matched_points;
|
||||||
|
matched_points.reserve(observations.size());
|
||||||
|
|
||||||
|
Eigen::Vector3d weighted_sum = Eigen::Vector3d::Zero();
|
||||||
|
double weight_sum = 0.0;
|
||||||
|
double best_weight = -std::numeric_limits<double>::infinity();
|
||||||
|
int best_id = -1;
|
||||||
|
|
||||||
|
for (const auto& obs : observations) {
|
||||||
|
if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const auto it = anchors_in_tag_.find(obs.tag_id);
|
||||||
|
if (it == anchors_in_tag_.end()) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const Eigen::Vector3d p_c = pointTagToCamera(obs.T_c_t, it->second);
|
||||||
|
if (!isFiniteVector(p_c)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
double w = obs.weight;
|
||||||
|
if (!std::isfinite(w) || w <= 0.0) {
|
||||||
|
w = 1.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
matched_points.push_back(p_c);
|
||||||
|
weighted_sum += w * p_c;
|
||||||
|
weight_sum += w;
|
||||||
|
|
||||||
|
if (w > best_weight) {
|
||||||
|
best_weight = w;
|
||||||
|
best_id = obs.tag_id;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (matched_points.empty() || weight_sum <= 0.0) {
|
||||||
|
last_status_ = Status::NO_MATCHING_TAG;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
p_c_target_out = weighted_sum / weight_sum;
|
||||||
|
|
||||||
|
if (spread_out) {
|
||||||
|
double max_err = 0.0;
|
||||||
|
for (const auto& p : matched_points) {
|
||||||
|
max_err = std::max(max_err, (p - p_c_target_out).norm());
|
||||||
|
}
|
||||||
|
*spread_out = max_err;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (used_tag_id_out) {
|
||||||
|
*used_tag_id_out = best_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
last_status_ = Status::OK;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::hasAnchorForTag(int tag_id) const {
|
||||||
|
return anchors_in_tag_.find(tag_id) != anchors_in_tag_.end();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const {
|
||||||
|
const auto it = anchors_in_tag_.find(tag_id);
|
||||||
|
if (it == anchors_in_tag_.end()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
p_t_target_out = it->second;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::isFiniteMatrix(const Eigen::Matrix4d& T) {
|
||||||
|
for (int r = 0; r < 4; ++r) {
|
||||||
|
for (int c = 0; c < 4; ++c) {
|
||||||
|
if (!std::isfinite(T(r, c))) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::isFiniteVector(const Eigen::Vector3d& p) {
|
||||||
|
return std::isfinite(p.x()) && std::isfinite(p.y()) && std::isfinite(p.z());
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TagRelativeTarget3D::detectObservationsFromImage(const cv::Mat& color,
|
||||||
|
const device::Rs2Intrinsics& intrinsics,
|
||||||
|
std::vector<TagPoseObservation>& observations) {
|
||||||
|
observations.clear();
|
||||||
|
|
||||||
|
if (color.empty()) {
|
||||||
|
last_status_ = Status::NO_NEW_FRAME;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (color.channels() != 1 && color.channels() != 3 && color.channels() != 4) {
|
||||||
|
last_status_ = Status::BAD_IMAGE;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const double fx = static_cast<double>(intrinsics.fx);
|
||||||
|
const double fy = static_cast<double>(intrinsics.fy);
|
||||||
|
const double cx = static_cast<double>(intrinsics.cx);
|
||||||
|
const double cy = static_cast<double>(intrinsics.cy);
|
||||||
|
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) {
|
||||||
|
last_status_ = Status::BAD_IMAGE;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat gray;
|
||||||
|
if (color.channels() == 1) {
|
||||||
|
gray = color;
|
||||||
|
} else if (color.channels() == 3) {
|
||||||
|
cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
|
||||||
|
} else {
|
||||||
|
cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
|
||||||
|
}
|
||||||
|
if (gray.empty() || gray.type() != CV_8UC1) {
|
||||||
|
last_status_ = Status::BAD_IMAGE;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (!gray.isContinuous()) {
|
||||||
|
gray = gray.clone();
|
||||||
|
}
|
||||||
|
|
||||||
|
const int width = gray.cols;
|
||||||
|
const int height = gray.rows;
|
||||||
|
vpCameraParameters cam;
|
||||||
|
cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
|
||||||
|
|
||||||
|
vpImage<unsigned char> I(height, width);
|
||||||
|
for (int y = 0; y < height; ++y) {
|
||||||
|
std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<vpHomogeneousMatrix> cMo_vec;
|
||||||
|
const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
|
||||||
|
if (!detected || cMo_vec.empty()) {
|
||||||
|
last_status_ = Status::NO_TAG;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::vector<int> tag_ids = detector_.getTagsId();
|
||||||
|
const std::vector<float> margins = detector_.getTagsDecisionMargin();
|
||||||
|
const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size());
|
||||||
|
if (pair_size == 0) {
|
||||||
|
last_status_ = Status::NO_TAG;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
observations.reserve(pair_size);
|
||||||
|
for (size_t i = 0; i < pair_size; ++i) {
|
||||||
|
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
|
||||||
|
const vpHomogeneousMatrix& cMo = cMo_vec[i];
|
||||||
|
for (int r = 0; r < 3; ++r) {
|
||||||
|
for (int c = 0; c < 3; ++c) {
|
||||||
|
T(r, c) = cMo[r][c];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
T(0, 3) = cMo[0][3];
|
||||||
|
T(1, 3) = cMo[1][3];
|
||||||
|
T(2, 3) = cMo[2][3];
|
||||||
|
|
||||||
|
double w = 1.0;
|
||||||
|
if (i < margins.size() && std::isfinite(margins[i]) && margins[i] > 0.0f) {
|
||||||
|
w = static_cast<double>(margins[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
observations.push_back(TagPoseObservation{tag_ids[i], T, w});
|
||||||
|
}
|
||||||
|
|
||||||
|
if (observations.empty()) {
|
||||||
|
last_status_ = Status::NO_TAG;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
last_status_ = Status::OK;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Vector3d TagRelativeTarget3D::pointTagToCamera(const Eigen::Matrix4d& T_c_t,
|
||||||
|
const Eigen::Vector3d& p_t) {
|
||||||
|
const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0);
|
||||||
|
const Eigen::Vector3d t = T_c_t.block<3, 1>(0, 3);
|
||||||
|
return R * p_t + t;
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Vector3d TagRelativeTarget3D::pointCameraToTag(const Eigen::Matrix4d& T_c_t,
|
||||||
|
const Eigen::Vector3d& p_c) {
|
||||||
|
const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0);
|
||||||
|
const Eigen::Vector3d t = T_c_t.block<3, 1>(0, 3);
|
||||||
|
return R.transpose() * (p_c - t);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cmvr::perception
|
||||||
524
cmvr-es/perception/src/tag_relative_target_3d_test.cpp
Normal file
524
cmvr-es/perception/src/tag_relative_target_3d_test.cpp
Normal file
@ -0,0 +1,524 @@
|
|||||||
|
//
|
||||||
|
// Created by lgv on 2026/2/26.
|
||||||
|
//
|
||||||
|
|
||||||
|
#include "perception/include/tag_relative_target_3d.h"
|
||||||
|
#include "devices/camera/realsense_camera/include/realsense_camera.h"
|
||||||
|
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cmath>
|
||||||
|
#include <iostream>
|
||||||
|
#include <thread>
|
||||||
|
#include <opencv2/highgui.hpp>
|
||||||
|
#include <opencv2/imgproc.hpp>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
// 直接在这里改测试参数。
|
||||||
|
constexpr const char* kRsSerial = "243122074587";
|
||||||
|
// Target pixel used for depth sampling. Negative means image center.
|
||||||
|
constexpr int kTargetU = 350;
|
||||||
|
constexpr int kTargetV = 250;
|
||||||
|
constexpr double kTagSize = 0.02;
|
||||||
|
// <= 0 means loop until user abort.
|
||||||
|
constexpr int kTries = -1;
|
||||||
|
constexpr int kWidth = 1280;
|
||||||
|
constexpr int kHeight = 720;
|
||||||
|
constexpr int kFps = 30;
|
||||||
|
constexpr const char* kWindowName = "TagRelativeTarget3DRealSenseTest";
|
||||||
|
constexpr int kWarmupMaxTries = 60;
|
||||||
|
constexpr int kWarmupSleepMs = 50;
|
||||||
|
constexpr int kFrameRetryMax = 3;
|
||||||
|
constexpr int kFrameRetrySleepMs = 25;
|
||||||
|
constexpr int kLogEveryNFrames = 20;
|
||||||
|
// Depth sample window radius (pixels):
|
||||||
|
// 1 -> 3x3, 2 -> 5x5, 3 -> 7x7.
|
||||||
|
constexpr int kDepthSampleRadiusTarget = 3;
|
||||||
|
constexpr int kDepthSampleRadiusTag = 3;
|
||||||
|
|
||||||
|
struct FramePack {
|
||||||
|
cv::Mat color;
|
||||||
|
cv::Mat depth;
|
||||||
|
cmvr::device::Rs2Intrinsics intrinsics{};
|
||||||
|
};
|
||||||
|
|
||||||
|
bool sampleDepthMeters(const cv::Mat& depth, int u, int v, double& z_m, int radius_px) {
|
||||||
|
if (depth.empty() || depth.channels() != 1) return false;
|
||||||
|
if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) return false;
|
||||||
|
if (radius_px < 0) return false;
|
||||||
|
|
||||||
|
std::vector<double> valid;
|
||||||
|
const int side = radius_px * 2 + 1;
|
||||||
|
valid.reserve(static_cast<size_t>(side * side));
|
||||||
|
for (int dv = -radius_px; dv <= radius_px; ++dv) {
|
||||||
|
for (int du = -radius_px; du <= radius_px; ++du) {
|
||||||
|
const int uu = u + du;
|
||||||
|
const int vv = v + dv;
|
||||||
|
if (uu < 0 || vv < 0 || uu >= depth.cols || vv >= depth.rows) continue;
|
||||||
|
|
||||||
|
double z = 0.0;
|
||||||
|
if (depth.type() == CV_16UC1) {
|
||||||
|
z = static_cast<double>(depth.at<uint16_t>(vv, uu)) * 1e-3;
|
||||||
|
} else if (depth.type() == CV_32FC1) {
|
||||||
|
z = static_cast<double>(depth.at<float>(vv, uu));
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (std::isfinite(z) && z > 1e-4) {
|
||||||
|
valid.push_back(z);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (valid.empty()) return false;
|
||||||
|
std::nth_element(valid.begin(), valid.begin() + valid.size() / 2, valid.end());
|
||||||
|
z_m = valid[valid.size() / 2];
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool fetchRGBDFrame(const std::shared_ptr<cmvr::device::RealsenseCamera>& camera,
|
||||||
|
FramePack& frame,
|
||||||
|
std::string& error_out) {
|
||||||
|
error_out.clear();
|
||||||
|
for (int retry = 0; retry < kFrameRetryMax; ++retry) {
|
||||||
|
try {
|
||||||
|
camera->getRGBDImages(frame.color, frame.depth, frame.intrinsics);
|
||||||
|
if (!frame.color.empty() && !frame.depth.empty()) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
error_out = "empty_frame";
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
error_out = e.what();
|
||||||
|
}
|
||||||
|
if (retry + 1 < kFrameRetryMax) {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(kFrameRetrySleepMs));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool projectPoint(const Eigen::Vector3d& p_c,
|
||||||
|
const cmvr::device::Rs2Intrinsics& K,
|
||||||
|
cv::Point& uv) {
|
||||||
|
if (!std::isfinite(p_c.x()) || !std::isfinite(p_c.y()) || !std::isfinite(p_c.z())) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (p_c.z() <= 1e-6) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (!std::isfinite(K.fx) || !std::isfinite(K.fy) ||
|
||||||
|
!std::isfinite(K.cx) || !std::isfinite(K.cy) ||
|
||||||
|
K.fx <= 0.0f || K.fy <= 0.0f) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
const double u = static_cast<double>(K.fx) * (p_c.x() / p_c.z()) + static_cast<double>(K.cx);
|
||||||
|
const double v = static_cast<double>(K.fy) * (p_c.y() / p_c.z()) + static_cast<double>(K.cy);
|
||||||
|
uv.x = static_cast<int>(std::lround(u));
|
||||||
|
uv.y = static_cast<int>(std::lround(v));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool backProjectPixelToCamera(const cmvr::device::Rs2Intrinsics& K,
|
||||||
|
int u,
|
||||||
|
int v,
|
||||||
|
double z_m,
|
||||||
|
Eigen::Vector3d& p_c) {
|
||||||
|
if (!std::isfinite(z_m) || z_m <= 1e-6) return false;
|
||||||
|
if (!std::isfinite(K.fx) || !std::isfinite(K.fy) ||
|
||||||
|
!std::isfinite(K.cx) || !std::isfinite(K.cy) ||
|
||||||
|
K.fx <= 0.0f || K.fy <= 0.0f) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
p_c.z() = z_m;
|
||||||
|
p_c.x() = (static_cast<double>(u) - static_cast<double>(K.cx)) / static_cast<double>(K.fx) * z_m;
|
||||||
|
p_c.y() = (static_cast<double>(v) - static_cast<double>(K.cy)) / static_cast<double>(K.fy) * z_m;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void drawTagAxes(cv::Mat& img,
|
||||||
|
const Eigen::Matrix4d& T_c_t,
|
||||||
|
const cmvr::device::Rs2Intrinsics& K,
|
||||||
|
double axis_len,
|
||||||
|
int tag_id) {
|
||||||
|
const Eigen::Matrix3d R = T_c_t.block<3,3>(0,0);
|
||||||
|
const Eigen::Vector3d t = T_c_t.block<3,1>(0,3);
|
||||||
|
const Eigen::Vector3d o_c = t;
|
||||||
|
const Eigen::Vector3d x_c = R * Eigen::Vector3d(axis_len, 0, 0) + t;
|
||||||
|
const Eigen::Vector3d y_c = R * Eigen::Vector3d(0, axis_len, 0) + t;
|
||||||
|
const Eigen::Vector3d z_c = R * Eigen::Vector3d(0, 0, axis_len) + t;
|
||||||
|
|
||||||
|
cv::Point o, x, y, z;
|
||||||
|
if (!projectPoint(o_c, K, o)) return;
|
||||||
|
if (projectPoint(x_c, K, x)) {
|
||||||
|
cv::line(img, o, x, cv::Scalar(0, 0, 255), 2); // X red
|
||||||
|
}
|
||||||
|
if (projectPoint(y_c, K, y)) {
|
||||||
|
cv::line(img, o, y, cv::Scalar(0, 255, 0), 2); // Y green
|
||||||
|
}
|
||||||
|
if (projectPoint(z_c, K, z)) {
|
||||||
|
cv::line(img, o, z, cv::Scalar(255, 0, 0), 2); // Z blue
|
||||||
|
}
|
||||||
|
cv::circle(img, o, 3, cv::Scalar(255, 255, 255), -1);
|
||||||
|
cv::putText(img,
|
||||||
|
"id=" + std::to_string(tag_id),
|
||||||
|
cv::Point(o.x + 6, o.y - 6),
|
||||||
|
cv::FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.45,
|
||||||
|
cv::Scalar(255, 255, 255),
|
||||||
|
1);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
|
||||||
|
const std::string serial = kRsSerial;
|
||||||
|
if (serial.empty()) {
|
||||||
|
GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp";
|
||||||
|
}
|
||||||
|
|
||||||
|
const double tag_size = kTagSize;
|
||||||
|
const int tries = kTries;
|
||||||
|
const bool infinite = (tries <= 0);
|
||||||
|
const int width = kWidth;
|
||||||
|
const int height = kHeight;
|
||||||
|
const int fps = kFps;
|
||||||
|
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] serial=" << serial
|
||||||
|
<< " target_uv=[" << kTargetU << ", " << kTargetV << "]"
|
||||||
|
<< " tag_size=" << tag_size
|
||||||
|
<< " tries=" << tries << "\n";
|
||||||
|
|
||||||
|
cmvr::config::RealSenseCameraConfig cam_cfg;
|
||||||
|
cam_cfg.set_id("tag_relative_target_3d_test");
|
||||||
|
cam_cfg.set_serialnumber(serial);
|
||||||
|
cam_cfg.set_width(width);
|
||||||
|
cam_cfg.set_height(height);
|
||||||
|
cam_cfg.set_fps(fps);
|
||||||
|
cam_cfg.set_codec("H265");
|
||||||
|
cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
|
||||||
|
cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
|
||||||
|
cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
|
||||||
|
cam_cfg.set_buffer_size(30);
|
||||||
|
cam_cfg.set_sync(true);
|
||||||
|
cam_cfg.set_enable(true);
|
||||||
|
|
||||||
|
auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
|
||||||
|
cmvr::perception::TagRelativeTarget3D tracker(camera);
|
||||||
|
tracker.setTagSize(tag_size);
|
||||||
|
|
||||||
|
ASSERT_NO_THROW(camera->init());
|
||||||
|
ASSERT_NO_THROW(camera->start());
|
||||||
|
struct CameraStopGuard {
|
||||||
|
std::shared_ptr<cmvr::device::RealsenseCamera> cam;
|
||||||
|
~CameraStopGuard() {
|
||||||
|
if (!cam) return;
|
||||||
|
try {
|
||||||
|
cam->stop();
|
||||||
|
} catch (...) {
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} stop_guard{camera};
|
||||||
|
|
||||||
|
bool window_enabled = true;
|
||||||
|
try {
|
||||||
|
cv::namedWindow(kWindowName, cv::WINDOW_NORMAL);
|
||||||
|
cv::resizeWindow(kWindowName, width, height);
|
||||||
|
cv::imshow(kWindowName, cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)));
|
||||||
|
cv::waitKey(1);
|
||||||
|
} catch (const cv::Exception& e) {
|
||||||
|
window_enabled = false;
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] window disabled: " << e.what() << "\n";
|
||||||
|
}
|
||||||
|
if (!window_enabled) {
|
||||||
|
GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test.";
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ok = false;
|
||||||
|
bool user_abort = false;
|
||||||
|
int frame_fail_count = 0;
|
||||||
|
int detect_success_count = 0;
|
||||||
|
int target_valid_count = 0;
|
||||||
|
int lock_success_count = 0;
|
||||||
|
int printed_count = 0;
|
||||||
|
FramePack last_good_frame;
|
||||||
|
last_good_frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20));
|
||||||
|
bool has_last_good_frame = false;
|
||||||
|
const auto t_loop_start = std::chrono::steady_clock::now();
|
||||||
|
|
||||||
|
// Warmup: try to get RGBD frames after pipeline start.
|
||||||
|
bool warmup_ok = false;
|
||||||
|
bool warmup_abort = false;
|
||||||
|
for (int i = 0; i < kWarmupMaxTries; ++i) {
|
||||||
|
FramePack warm_frame;
|
||||||
|
std::string warm_error;
|
||||||
|
warmup_ok = fetchRGBDFrame(camera, warm_frame, warm_error);
|
||||||
|
if (warmup_ok) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
const int key = cv::waitKey(1);
|
||||||
|
if (key == 27 || key == 'q' || key == 'Q') {
|
||||||
|
warmup_abort = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(kWarmupSleepMs));
|
||||||
|
}
|
||||||
|
if (warmup_abort) {
|
||||||
|
GTEST_SKIP() << "User aborted during warmup";
|
||||||
|
}
|
||||||
|
if (!warmup_ok) {
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] warmup failed, continue running and retry per frame\n";
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; infinite || i < tries; ++i) {
|
||||||
|
FramePack frame;
|
||||||
|
frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20));
|
||||||
|
std::string frame_error;
|
||||||
|
const bool frame_ok = fetchRGBDFrame(camera, frame, frame_error);
|
||||||
|
if (!frame_ok) {
|
||||||
|
++frame_fail_count;
|
||||||
|
if (has_last_good_frame) {
|
||||||
|
frame.color = last_good_frame.color.clone();
|
||||||
|
frame.depth = last_good_frame.depth.clone();
|
||||||
|
frame.intrinsics = last_good_frame.intrinsics;
|
||||||
|
} else {
|
||||||
|
frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20));
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
last_good_frame.color = frame.color.clone();
|
||||||
|
last_good_frame.depth = frame.depth.clone();
|
||||||
|
last_good_frame.intrinsics = frame.intrinsics;
|
||||||
|
has_last_good_frame = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<cmvr::perception::TagPoseObservation> observations;
|
||||||
|
const bool detected = tracker.detectObservationsFromImage(frame.color, frame.intrinsics, observations);
|
||||||
|
const auto status = tracker.lastStatus();
|
||||||
|
const char* status_str = cmvr::perception::TagRelativeTarget3D::statusToString(status);
|
||||||
|
if (detected) {
|
||||||
|
++detect_success_count;
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Vector3d p_c_target = Eigen::Vector3d::Zero();
|
||||||
|
const int target_u = (kTargetU >= 0) ? kTargetU : (frame.color.cols / 2);
|
||||||
|
const int target_v = (kTargetV >= 0) ? kTargetV : (frame.color.rows / 2);
|
||||||
|
double z_depth_m = 0.0;
|
||||||
|
bool target_pc_valid = false;
|
||||||
|
if (!frame.color.empty()) {
|
||||||
|
const bool depth_ok = sampleDepthMeters(frame.depth,
|
||||||
|
target_u,
|
||||||
|
target_v,
|
||||||
|
z_depth_m,
|
||||||
|
kDepthSampleRadiusTarget);
|
||||||
|
if (depth_ok && std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) &&
|
||||||
|
std::isfinite(frame.intrinsics.cx) && std::isfinite(frame.intrinsics.cy) &&
|
||||||
|
frame.intrinsics.fx > 1e-6f && frame.intrinsics.fy > 1e-6f) {
|
||||||
|
p_c_target.z() = z_depth_m;
|
||||||
|
p_c_target.x() = (static_cast<double>(target_u) - static_cast<double>(frame.intrinsics.cx))
|
||||||
|
/ static_cast<double>(frame.intrinsics.fx) * z_depth_m;
|
||||||
|
p_c_target.y() = (static_cast<double>(target_v) - static_cast<double>(frame.intrinsics.cy))
|
||||||
|
/ static_cast<double>(frame.intrinsics.fy) * z_depth_m;
|
||||||
|
target_pc_valid = true;
|
||||||
|
++target_valid_count;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat vis = frame.color.clone();
|
||||||
|
cv::drawMarker(vis,
|
||||||
|
cv::Point(target_u, target_v),
|
||||||
|
cv::Scalar(0, 255, 255),
|
||||||
|
cv::MARKER_CROSS,
|
||||||
|
16,
|
||||||
|
2);
|
||||||
|
cv::putText(vis,
|
||||||
|
std::string("status: ") + status_str,
|
||||||
|
cv::Point(12, 28),
|
||||||
|
cv::FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.7,
|
||||||
|
cv::Scalar(0, 255, 0),
|
||||||
|
2);
|
||||||
|
cv::putText(vis,
|
||||||
|
"press q / Esc to quit",
|
||||||
|
cv::Point(12, 56),
|
||||||
|
cv::FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.6,
|
||||||
|
cv::Scalar(0, 255, 255),
|
||||||
|
2);
|
||||||
|
cv::putText(vis,
|
||||||
|
"tags=" + std::to_string(observations.size()) +
|
||||||
|
" frame_fail=" + std::to_string(frame_fail_count),
|
||||||
|
cv::Point(12, 84),
|
||||||
|
cv::FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.5,
|
||||||
|
cv::Scalar(200, 200, 255),
|
||||||
|
1);
|
||||||
|
if (!frame_error.empty()) {
|
||||||
|
cv::putText(vis,
|
||||||
|
"frame error: " + frame_error,
|
||||||
|
cv::Point(12, 106),
|
||||||
|
cv::FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.45,
|
||||||
|
cv::Scalar(0, 0, 255),
|
||||||
|
1);
|
||||||
|
}
|
||||||
|
cv::putText(vis,
|
||||||
|
std::string("depth@target: ") + (target_pc_valid ? std::to_string(z_depth_m) + "m" : "invalid"),
|
||||||
|
cv::Point(12, 128),
|
||||||
|
cv::FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.5,
|
||||||
|
target_pc_valid ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255),
|
||||||
|
1);
|
||||||
|
|
||||||
|
// Draw detected tag axes (camera frame).
|
||||||
|
if (detected && !observations.empty()) {
|
||||||
|
const double axis_len = tag_size * 0.5;
|
||||||
|
for (const auto& obs : observations) {
|
||||||
|
drawTagAxes(vis, obs.T_c_t, frame.intrinsics, axis_len, obs.tag_id);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Draw target point (projected from camera coordinates).
|
||||||
|
if (std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) &&
|
||||||
|
std::isfinite(frame.intrinsics.cx) && std::isfinite(frame.intrinsics.cy) &&
|
||||||
|
p_c_target.z() > 1e-6) {
|
||||||
|
const double u = frame.intrinsics.fx * (p_c_target.x() / p_c_target.z()) + frame.intrinsics.cx;
|
||||||
|
const double v = frame.intrinsics.fy * (p_c_target.y() / p_c_target.z()) + frame.intrinsics.cy;
|
||||||
|
const int ui = static_cast<int>(std::lround(u));
|
||||||
|
const int vi = static_cast<int>(std::lround(v));
|
||||||
|
if (ui >= 0 && ui < vis.cols && vi >= 0 && vi < vis.rows) {
|
||||||
|
cv::circle(vis, cv::Point(ui, vi), 6, cv::Scalar(0, 0, 255), 2);
|
||||||
|
cv::putText(vis,
|
||||||
|
"target",
|
||||||
|
cv::Point(ui + 8, vi - 8),
|
||||||
|
cv::FONT_HERSHEY_SIMPLEX,
|
||||||
|
0.5,
|
||||||
|
cv::Scalar(0, 0, 255),
|
||||||
|
1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
try {
|
||||||
|
cv::imshow(kWindowName, vis);
|
||||||
|
const int key = cv::waitKey(1);
|
||||||
|
if (key == 27 || key == 'q' || key == 'Q') {
|
||||||
|
user_abort = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
} catch (const cv::Exception& e) {
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] window error: " << e.what() << "\n";
|
||||||
|
user_abort = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!detected) {
|
||||||
|
if (((i + 1) % kLogEveryNFrames) == 0) {
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] detect failed: "
|
||||||
|
<< status_str
|
||||||
|
<< " (try=" << (i + 1) << "/" << tries << ")\n";
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!observations.empty()) {
|
||||||
|
const double tag_tz = observations.front().T_c_t(2, 3);
|
||||||
|
if (((i + 1) % kLogEveryNFrames) == 0) {
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] t.z=" << tag_tz
|
||||||
|
<< " depth@target=" << (target_pc_valid ? z_depth_m : -1.0)
|
||||||
|
<< " tag_id=" << observations.front().tag_id << "\n";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (const auto& obs : observations) {
|
||||||
|
const Eigen::Vector3d p_c_tag_pose = obs.T_c_t.block<3, 1>(0, 3);
|
||||||
|
cv::Point uv_tag;
|
||||||
|
double z_tag_depth_m = 0.0;
|
||||||
|
Eigen::Vector3d p_c_tag_depth = Eigen::Vector3d::Zero();
|
||||||
|
const bool uv_ok = projectPoint(p_c_tag_pose, frame.intrinsics, uv_tag);
|
||||||
|
bool tag_depth_ok = false;
|
||||||
|
if (uv_ok) {
|
||||||
|
tag_depth_ok = sampleDepthMeters(frame.depth,
|
||||||
|
uv_tag.x,
|
||||||
|
uv_tag.y,
|
||||||
|
z_tag_depth_m,
|
||||||
|
kDepthSampleRadiusTag) &&
|
||||||
|
backProjectPixelToCamera(frame.intrinsics,
|
||||||
|
uv_tag.x,
|
||||||
|
uv_tag.y,
|
||||||
|
z_tag_depth_m,
|
||||||
|
p_c_tag_depth);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] tag_id=" << obs.tag_id
|
||||||
|
<< " pose_p_c=[" << p_c_tag_pose.x() << ", " << p_c_tag_pose.y() << ", " << p_c_tag_pose.z() << "]"
|
||||||
|
<< " depth_p_c="
|
||||||
|
<< (tag_depth_ok
|
||||||
|
? "[" + std::to_string(p_c_tag_depth.x()) + ", " +
|
||||||
|
std::to_string(p_c_tag_depth.y()) + ", " +
|
||||||
|
std::to_string(p_c_tag_depth.z()) + "]"
|
||||||
|
: "[invalid]")
|
||||||
|
<< " uv=[" << (uv_ok ? std::to_string(uv_tag.x) : "invalid")
|
||||||
|
<< ", " << (uv_ok ? std::to_string(uv_tag.y) : "invalid") << "]\n";
|
||||||
|
}
|
||||||
|
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] target_depth_p_c="
|
||||||
|
<< (target_pc_valid
|
||||||
|
? "[" + std::to_string(p_c_target.x()) + ", " +
|
||||||
|
std::to_string(p_c_target.y()) + ", " +
|
||||||
|
std::to_string(p_c_target.z()) + "]"
|
||||||
|
: "[invalid]")
|
||||||
|
<< " target_uv=[" << target_u << ", " << target_v << "]\n";
|
||||||
|
|
||||||
|
if (!target_pc_valid) {
|
||||||
|
if (((i + 1) % kLogEveryNFrames) == 0) {
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] skip lock: invalid depth at target pixel\n";
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!tracker.lockTargetInCamera(p_c_target, observations, true)) {
|
||||||
|
if (((i + 1) % kLogEveryNFrames) == 0) {
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] lock failed: "
|
||||||
|
<< cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus())
|
||||||
|
<< "\n";
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
++lock_success_count;
|
||||||
|
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] detected_tags=" << observations.size() << "\n";
|
||||||
|
for (const auto& obs : observations) {
|
||||||
|
Eigen::Vector3d p_t = Eigen::Vector3d::Zero();
|
||||||
|
if (!tracker.getAnchorInTag(obs.tag_id, p_t)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
std::cout << " tag_id=" << obs.tag_id
|
||||||
|
<< " p_t_target=[" << p_t.x() << ", " << p_t.y() << ", " << p_t.z() << "]\n";
|
||||||
|
}
|
||||||
|
++printed_count;
|
||||||
|
|
||||||
|
ok = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
try {
|
||||||
|
cv::destroyWindow(kWindowName);
|
||||||
|
} catch (...) {
|
||||||
|
// ignore window destroy errors in headless environments
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!user_abort && !infinite) {
|
||||||
|
EXPECT_TRUE(ok) << "No valid tag detected in " << tries << " tries";
|
||||||
|
}
|
||||||
|
const auto t_loop_end = std::chrono::steady_clock::now();
|
||||||
|
const double total_s = std::chrono::duration_cast<std::chrono::duration<double>>(t_loop_end - t_loop_start).count();
|
||||||
|
std::cout << "[TagRelativeTarget3DTest] summary "
|
||||||
|
<< "detect_ok=" << detect_success_count
|
||||||
|
<< " target_valid=" << target_valid_count
|
||||||
|
<< " lock_ok=" << lock_success_count
|
||||||
|
<< " printed=" << printed_count
|
||||||
|
<< " frame_fail=" << frame_fail_count
|
||||||
|
<< " duration_s=" << total_s
|
||||||
|
<< "\n";
|
||||||
|
}
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -60,7 +60,7 @@
|
|||||||
/* #undef HAVE_VULKAN */
|
/* #undef HAVE_VULKAN */
|
||||||
|
|
||||||
/* Define to 1 if you have the <inttypes.h> header file. */
|
/* Define to 1 if you have the <inttypes.h> header file. */
|
||||||
#define HAVE_INTTYPES_H 1
|
/* #undef HAVE_INTTYPES_H */
|
||||||
|
|
||||||
/* Intel Integrated Performance Primitives */
|
/* Intel Integrated Performance Primitives */
|
||||||
#define HAVE_IPP
|
#define HAVE_IPP
|
||||||
|
|||||||
@ -78,6 +78,7 @@ list(APPEND _IMPORT_CHECK_FILES_FOR_opencv_features2d "${_IMPORT_PREFIX}/lib/lib
|
|||||||
# Import target "opencv_imgcodecs" for configuration "Release"
|
# Import target "opencv_imgcodecs" for configuration "Release"
|
||||||
set_property(TARGET opencv_imgcodecs APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE)
|
set_property(TARGET opencv_imgcodecs APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE)
|
||||||
set_target_properties(opencv_imgcodecs PROPERTIES
|
set_target_properties(opencv_imgcodecs PROPERTIES
|
||||||
|
IMPORTED_LINK_DEPENDENT_LIBRARIES_RELEASE "openjp2"
|
||||||
IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libopencv_imgcodecs.so.4.13.0"
|
IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libopencv_imgcodecs.so.4.13.0"
|
||||||
IMPORTED_SONAME_RELEASE "libopencv_imgcodecs.so.413"
|
IMPORTED_SONAME_RELEASE "libopencv_imgcodecs.so.413"
|
||||||
)
|
)
|
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
13
dependency/x86/third_party/opencv/4.13.0/lib/pkgconfig/opencv4.pc
vendored
Normal file
13
dependency/x86/third_party/opencv/4.13.0/lib/pkgconfig/opencv4.pc
vendored
Normal file
@ -0,0 +1,13 @@
|
|||||||
|
# Package Information for pkg-config
|
||||||
|
|
||||||
|
prefix=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/opencv/4.13.0
|
||||||
|
exec_prefix=${prefix}
|
||||||
|
libdir=${exec_prefix}/lib
|
||||||
|
includedir=${prefix}/include/opencv4
|
||||||
|
|
||||||
|
Name: OpenCV
|
||||||
|
Description: Open Source Computer Vision Library
|
||||||
|
Version: 4.13.0
|
||||||
|
Libs: -L${exec_prefix}/lib -lopencv_gapi -lopencv_highgui -lopencv_ml -lopencv_objdetect -lopencv_photo -lopencv_stitching -lopencv_video -lopencv_calib3d -lopencv_features2d -lopencv_dnn -lopencv_flann -lopencv_videoio -lopencv_imgcodecs -lopencv_imgproc -lopencv_core
|
||||||
|
Libs.private: -ldl -lm -lpthread -lrt
|
||||||
|
Cflags: -I${includedir}
|
||||||
Loading…
Reference in New Issue
Block a user