diff --git a/README.md b/README.md index e9f13ade..2b54e6e8 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ sudo apt install -y \ libjpeg-dev libpng-dev libtiff-dev \ libavcodec-dev libavformat-dev libswscale-dev \ libv4l-dev libxvidcore-dev libx264-dev \ - libgtk-3-dev + libgtk-3-dev # PulseAudio diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index 5e60afdd..13bc765b 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -7,8 +7,9 @@ add_subdirectory(monitor_manager) add_subdirectory(service) add_subdirectory(controller) add_subdirectory(planner) +add_subdirectory(perception) add_subdirectory(ik_solver) add_subdirectory(data_center) add_subdirectory(simulate) -add_subdirectory(common) \ No newline at end of file +add_subdirectory(common) diff --git a/cmvr-es/controller/CMakeLists.txt b/cmvr-es/controller/CMakeLists.txt index d11e0bc7..f8d327cb 100644 --- a/cmvr-es/controller/CMakeLists.txt +++ b/cmvr-es/controller/CMakeLists.txt @@ -39,7 +39,7 @@ target_link_libraries(controller PUBLIC add_library(cmvr_es::controller ALIAS controller) - +install(TARGETS controller LIBRARY DESTINATION lib) # -------------------------------------------------------- diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h index 7997cf06..904cdfb2 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/cmvr-es/controller/include/ibvs_controller.h @@ -47,6 +47,7 @@ public: INVALID_INPUT, /**< 输入参数异常。 */ NO_DEPTH, /**< 需要深度但深度不可用。 */ NO_TAG, /**< 未检测到 AprilTag。 */ + TAG_MISMATCH, /**< 检测到 AprilTag，但与指定 id 不匹配。 */ IK_FAILED /**< 速度 IK 求解失败。 */ }; @@ -116,7 +117,12 @@ public: */ 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 y 期望平移 y（米）。 * @param z 期望平移 z（米）。 @@ -167,7 +173,7 @@ public: void setAlignCameraToUrdf(const Eigen::Matrix3d& R_camera_urdf); /** - * @brief 最近一帧是否检测到 tag。 + * @brief 最近一帧是否检测到“指定 id 的 tag”。 * @return 检测到返回 `true`。 */ bool isTagDetected() const { return last_tag_detected_; } @@ -198,6 +204,16 @@ public: * @return tag 平移向量。 */ 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 相机坐标系）。 * @return 六维 twist 向量。 @@ -279,9 +295,13 @@ private: vpFeaturePoint s_star_[4]; // AprilTag 检测器。 vpDetectorAprilTag detector_; + // 指定跟踪的 tag id（必填）；<0 表示未设置。 + int tracked_tag_id_{-1}; // 速度 IK 求解器。 std::unique_ptr dls_solver_{nullptr}; + // 最近一次用于控制的 tag id。 + int last_used_tag_id_{-1}; // 内部积分得到的关节位置命令缓存。 std::vector q_cmd_; diff --git a/cmvr-es/controller/src/controller_test.cpp b/cmvr-es/controller/src/controller_test.cpp index 8d2cc4eb..aeb9686f 100644 --- a/cmvr-es/controller/src/controller_test.cpp +++ b/cmvr-es/controller/src/controller_test.cpp @@ -539,7 +539,8 @@ protected: ibvs_controller_->setDepthMode(IbvsController::DepthMode::MONOCULAR); ibvs_controller_->setDepthZGain(1.0); 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() << 1, 0, 0, @@ -695,6 +696,7 @@ private: std::string urdf_path_for_check_{ "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"}; std::string camera_frame_name_for_check_{"R_CAM"}; + int tracked_tag_id_{0}; std::unique_ptr ibvs_controller_{nullptr}; std::shared_ptr mujoco_camera_{nullptr}; diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp index ed8f1fca..c378f198 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/cmvr-es/controller/src/ibvs_controller.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -29,6 +30,7 @@ const char* IbvsController::statusToString(ComputeStatus status) { case ComputeStatus::INVALID_INPUT: return "invalid_input"; case ComputeStatus::NO_DEPTH: return "no_depth"; case ComputeStatus::NO_TAG: return "no_tag"; + case ComputeStatus::TAG_MISMATCH: return "tag_mismatch"; case ComputeStatus::IK_FAILED: return "ik_failed"; default: return "unknown"; } @@ -91,6 +93,7 @@ void IbvsController::reset(const std::vector& q_init) { q_cmd_ = q_init; } last_tag_detected_ = false; + last_used_tag_id_ = -1; last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY; last_depth_usage_ = DepthUsage::NONE; last_tag_pos_visp_.setZero(); @@ -161,16 +164,53 @@ bool IbvsController::computeInternal(const std::vector& joints_angle, std::memcpy(I[y], gray.ptr(y), static_cast(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 cMo_vec; const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec); - last_tag_detected_ = (detected && !cMo_vec.empty()); - if (!last_tag_detected_) { + if (!detected || cMo_vec.empty()) { + last_tag_detected_ = false; + last_used_tag_id_ = -1; last_tag_pos_visp_.setZero(); last_compute_status_ = ComputeStatus::NO_TAG; return false; } - vpHomogeneousMatrix cMo = cMo_vec[0]; + // 只允许使用指定 id 的 tag，不再回退到“第一个检测结果”。 + const std::vector 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(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(selected_idx)]; last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3]; // 深度控制点定义在 tag 平面（object frame）: @@ -309,6 +349,10 @@ void IbvsController::setTagSize(double tag_size_m) { initTask(); } +void IbvsController::setTrackedTagId(int tag_id) { + tracked_tag_id_ = tag_id; +} + void IbvsController::setTarget(double x, double y, double z, diff --git a/cmvr-es/data_center/CMakeLists.txt b/cmvr-es/data_center/CMakeLists.txt index 5f3112d7..d0fda580 100644 --- a/cmvr-es/data_center/CMakeLists.txt +++ b/cmvr-es/data_center/CMakeLists.txt @@ -14,3 +14,6 @@ target_link_libraries(data_center PRIVATE protobuf glog ) + + +install(TARGETS data_center LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/cmvr-es/device_manager/CMakeLists.txt b/cmvr-es/device_manager/CMakeLists.txt index e53622b7..2944ba2f 100644 --- a/cmvr-es/device_manager/CMakeLists.txt +++ b/cmvr-es/device_manager/CMakeLists.txt @@ -20,3 +20,4 @@ target_link_libraries(device_manager PRIVATE ) add_library(cmvr_es::device_manager ALIAS device_manager) +install(TARGETS device_manager LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt b/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt index f5fd8ba9..9ea7b21e 100644 --- a/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt +++ b/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt @@ -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 # 链接 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) # -------------------------------------------------------- diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index fbd7574a..43aa315d 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -151,23 +151,41 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca auto stream_mode = camera_.stream_mode(); if (stream_mode == config::STREAM_MODE_RGB) { - stream_mode_ = RGBD_MODE; + stream_mode_ = COLOR_MODE; } else if (stream_mode == config::STREAM_MODE_RGBD) { - stream_mode_ = COLOR_MODE; + stream_mode_ = RGBD_MODE; } else if (stream_mode == config::STREAM_MODE_DEPTH) { 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(); } RealsenseCamera::~RealsenseCamera() { - // 停止采集线程，释放资源 - stop(); + // 析构阶段禁止抛异常，避免 terminate。 + try { + stop(); + } catch (const std::exception& e) { + LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): " << e.what(); + } catch (...) { + LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): unknown exception"; + } } void RealsenseCamera::init() { @@ -222,78 +240,110 @@ void RealsenseCamera::start() { throw runtime_error("Camera not initialized"); } - try{ - if (state_.is_opened) { - LOG(WARNING) << "[RealsenseCamera] (start): camera already started"; + if (state_.is_opened) { + 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(); + auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as(); + 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(); + Kc_ = sp_color.get_intrinsics(); + + intrinsics_ = Kc_; + } + else if (stream_mode_ == DEPTH_MODE) { + auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as(); + Kd_ = sp_depth.get_intrinsics(); + + intrinsics_ = Kd_; + } + + if (align_mode_ == "color") { + align_ = std::make_shared(RS2_STREAM_COLOR); + } + else if (align_mode_ == "depth") { + align_ = std::make_shared(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; } - // 使用相同的同步策略启动 - profile_ = pipe_.start(rs_cfg_); - - - if (stream_mode_ == RGBD_MODE){ - auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as(); - auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as(); - 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_; + catch (const exception &error) { + last_error = error.what(); + state_.is_opened = false; + LOG(WARNING) << "[RealsenseCamera] (start): attempt " + << attempt << "/" << kStartMaxRetry + << " failed: " << last_error; + try { + pipe_.stop(); + } catch (...) { + // ignore stop exceptions during retry cleanup + } + align_.reset(); + 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(); - Kc_ = sp_color.get_intrinsics(); - - intrinsics_ = Kc_; - } - else if (stream_mode_ == DEPTH_MODE) { - auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as(); - Kd_ = sp_depth.get_intrinsics(); - - intrinsics_ = Kd_; - } - - if (align_mode_ == "color") { - align_ = std::make_shared(RS2_STREAM_COLOR); - } - else if (align_mode_ == "depth") { - align_ = std::make_shared(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().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() { //先停止录制再关闭摄像头 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_); clear_error_(); - try { - if (!state_.is_opened || !state_.is_initialized) { - throw runtime_error("Camera already closed"); - } - pipe_.stop(); + if (!state_.is_opened || !state_.is_initialized) { state_.is_opened = false; + return; } - catch (exception &e) { - LOG(ERROR) << "[RealsenseCamera] (stop): " << e.what(); - throw runtime_error(e.what()); + try { + pipe_.stop(); + } 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) { @@ -337,7 +387,7 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { latest_depth_ = depth; } } - LOG(INFO) << "realsense get rgb frame successfully"; + // LOG(INFO) << "realsense get rgb frame successfully"; } catch (const std::exception& e) { 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) { std::lock_guard 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) { frames = align_->process(frames); @@ -842,9 +893,8 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr& enco av_opt_set(ctx->priv_data, "repeat-headers", "1", 0); av_opt_set(ctx->priv_data, "annexb", "1", 0); } else if (codec->id == AV_CODEC_ID_HEVC) { - // 直接使用默认参数，不自定义 - // av_opt_set(ctx->priv_data, "preset", "ultrafast", 0); - // av_opt_set(ctx->priv_data, "tune", "zerolatency", 0); + // 降低 x265 控制台日志噪声（如 "encoded 0 frames"）。 + av_opt_set(ctx->priv_data, "x265-params", "log-level=none", 0); } // 6. 设置像素格式（不变） @@ -1124,4 +1174,3 @@ Eigen::Vector3f RealsenseCamera::get3DPointFromPixel(int u, int v) { return { point[0], point[1], point[2] }; } - diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp index ed7e645d..803ee75a 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp @@ -3,8 +3,12 @@ // #include #include "../../abstract_camera.h" +#include "../include/realsense_camera.h" #include #include "../../../../device_manager/include/device_manager.h" +#include +#include +#include using namespace cmvr::device; // 测试: 实时显示 RGB + Depth TEST(RealsenseCameraRealDeviceTest, SaveFrames) { @@ -35,3 +39,174 @@ TEST(RealsenseCameraRealDeviceTest, SaveFrames) { LOG(INFO) << "Pose : " << pose ; 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& 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 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(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(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(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(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)); + } +} diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 392e8f38..dc39eb2f 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -68,9 +68,32 @@ namespace cmvr::device{ 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 &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 &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 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> &targets, double dt) { throw std::runtime_error("Not implemented"); } + /** + * @brief 关节空间周期伺服（无整段轨迹规划）。 + * @details 每次调用只在一个控制周期内跟踪当前目标，需外部循环持续下发；是否稳定由外部更新频率和目标序列决定。 + * @param joints 当前周期目标关节角，单位 rad。 + * @param dt 当前伺服命令生效时长（控制周期），单位 s。 + */ virtual void servoJ(std::vector &joints, double dt) { throw std::runtime_error("Not implemented"); } + /** + * @brief 按关节命令执行周期伺服（无整段轨迹规划）。 + * @param joints 当前周期关节目标命令。 + * @param dt 当前伺服命令生效时长（控制周期），单位 s。 + */ virtual void servoJ(std::vector &joints, double dt) { throw std::runtime_error("Not implemented"); } + + /** + * @brief 带速度参数的关节空间周期伺服。 + * @param joints 当前周期关节目标命令。 + * @param vel 速度参数（语义由具体驱动实现决定）。 + * @param dt 当前伺服命令生效时长（控制周期），单位 s。 + */ virtual void servoJ(std::vector &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 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"); } diff --git a/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt b/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt index e80f5d7e..d3b3e717 100644 --- a/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt +++ b/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt @@ -29,6 +29,7 @@ add_executable(humanoid_robot_test target_link_libraries(humanoid_robot_test PRIVATE cmvr_es::device_manager + cmvr_es::controller cmvr_es::common cmvr_es::data_center osqp diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index 1cbf38d4..89827d41 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp @@ -10,21 +10,57 @@ #include #include #include +#include +#include +#include +#include #include "gtest/gtest.h" #include #include "../../../../device_manager/include/device_manager.h" #include +#include "controller/include/ibvs_controller.h" #include "cmvr/msgs/can_card_parameter.grpc.pb.h" #include "../include/humanoid_robot.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.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; +namespace { + +constexpr std::array kRightArmJointNames = { + "R_SHOULDER_P", + "R_SHOULDER_R", + "R_SHOULDER_Y", + "R_ELBOW_R", + "R_WRIST_P", + "R_WRIST_Y", + "R_WRIST_R" +}; + +std::vector buildRightArmJointCmd(const std::vector& q_cmd, + double vel) { + std::vector 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 makeRightArmQMap() { + std::unordered_map 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) { std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; const XmlNode config(config_path); @@ -71,616 +107,176 @@ TEST(HumanoidRobotTest,MyRobotTest) { } - -TEST(HumanoidRobotTest,FollowJointTrajectoryTest) { - 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"; - } +TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) { + const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"); + ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; auto dmgr_cfg = config.getChild("DeviceManager"); - auto &dmgr = DeviceManager::getInstance(dmgr_cfg); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); auto robot = dmgr.getDevice("hc01"); - - // 读取轨迹 - std::ifstream file("/home/lgv/cmvr/cmvr-es/src/devices/robot/humanoid_robot/joint_positions_1.csv"); - if (!file.is_open()) { - LOG(ERROR) << "无法打开文件" ; - - } - - std::string line; - - // 读取标题行，获取关节名称（除去第一列 idx） - if (!std::getline(file, line)) { - LOG(ERROR) << "文件为空或格式错误" ; - } - - std::vector joint_names; - { - std::stringstream ss(line); - std::string cell; - // 第一列是 idx，跳过 - std::getline(ss, cell, ','); - // 读取关节名称列 - while (std::getline(ss, cell, ',')) { - joint_names.push_back(cell); + auto q_map_now = makeRightArmQMap(); + std::vector q_now(kRightArmJointNames.size(), 0.0); + auto refreshRightArmQ = [&]() -> bool { + robot->getJointQ(q_map_now); + 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; + }; - std::vector> traj; - - // 读取后续每行数据 - while (std::getline(file, line)) { - std::stringstream ss(line); - std::string cell; - - // 读取第一列 idx，暂时不使用 - std::getline(ss, cell, ','); - - std::vector 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(); + ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q"; - // 计算速度 - 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 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_R", 0.0}, // {"R_SHOULDER_Y", 0.0}, // {"R_ELBOW_R", 0.0}, // {"R_WRIST_P", 0.0}, // {"R_WRIST_Y", 0.0}, - // {"R_WRIST_R", 0.0} + // {"R_WRIST_R", 0.0}, // }; - // robot->moveJ(cmd,0.8); - - // // // 先到达轨迹起点 - // LOG(INFO) << "Moving to trajectory start..."; - // robot->moveJ(traj[0],0.8); - // - // - // - // // 再移动 - // LOG(INFO) << "Reached trajectory start point"; - // robot->followJointTrajectory(traj,dt * 1000); - // LOG(INFO) << "Trajectory execution completed"; - - - - // std::vector 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 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 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 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)); - // } + auto joint_cmd = buildRightArmJointCmd(q_now, 0.8); + ASSERT_NO_THROW(robot->servoJ(joint_cmd, 0.8, 0.01)); + while (true) { + std::this_thread::sleep_for(std::chrono::milliseconds(10000)); + } } - -TEST(HumanoidRobotTest,MoveDeltaTest) { - 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"; - } - auto dmgr_cfg = config.getChild("DeviceManager"); - auto &dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot = dmgr.getDevice("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 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("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("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 配置并初始化设备管理器 +TEST(HumanoidRobotTest,IBVSWithRealRobot) { + const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"); + ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; auto dmgr_cfg = config.getChild("DeviceManager"); auto& dmgr = DeviceManager::getInstance(dmgr_cfg); - // 获取机器人实例 auto robot = dmgr.getDevice("hc01"); + auto camera = dmgr.getDevice("cam4"); + ASSERT_NE(robot, nullptr); + ASSERT_NE(camera, nullptr); - // 设置目标关节和相关参数 - std::string joint_name = "L_WRIST_R"; // 目标关节名 - RobotJointIndexDirection dir = RobotJointIndexDirection::Y_NEGATIVE; // 方向 - double vel = -0.1; // 速度 (rad/s) - double acc = 1; // 加速度 (rad/s²) - - try { - // 调用 speedJ 函数进行关节运动 - robot->speedJ(joint_name, dir, vel, acc); - - } 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("hc01"); - - try { - - // std::unordered_map 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 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("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> 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); + ASSERT_NO_THROW(camera->start()); + struct RuntimeGuard { + std::shared_ptr camera; + std::shared_ptr robot; + ~RuntimeGuard() { + if (camera) { + try { + camera->stop(); + } catch (...) { + } } + if (robot) { + try { + robot->eStop(); + } catch (...) { + } + } + } + } guard{camera, robot}; - // 构造当前时间步的PoseTarget（控制末端执行器） - cmvr::ctrl::PoseTarget ee_target; - ee_target.link_name = ee_link; // 目标控制的link（末端执行器） - ee_target.T_target = point_T; // 相对base_link的目标位姿矩阵 - ee_target.w_posrot = 0.5; // 位置/姿态权重（0.5：位置和姿态同等重要） - ee_target.weight = 1.0; // 任务权重（1.0：硬约束，必须满足） + cmvr::IbvsController ibvs_controller; + ibvs_controller.setMu(0.1); + ibvs_controller.setQdotMax(0.6); + ibvs_controller.setTagSize(0.12); + ibvs_controller.setTrackedTagId(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） - std::vector step_targets = {ee_target}; - trajectory.push_back(step_targets); // 将当前时间步目标加入轨迹 + ASSERT_TRUE(ibvs_controller.init(camera, + "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", + "PELVIS_S", + "R_WRIST_R_S", + "R_CAM")) + << "IbvsController init failed"; + + auto q_map_now = makeRightArmQMap(); + robot->getJointQ(q_map_now); + std::vector 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; } - // 输出轨迹信息（调试用） - LOG(INFO) << "Trajectory generated successfully: " - << trajectory.size() << " time steps, radius: " << circle_radius << "m"; + const auto t_now = std::chrono::steady_clock::now(); + double dt = std::chrono::duration(t_now - t_prev).count(); + t_prev = t_now; + dt = std::clamp(dt, 0.001, 0.1); + std::vector 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接口） - LOG(INFO) << "Starting circular trajectory execution..."; - robot->followPoseTrajectory(base_link,trajectory, dt); // 传入轨迹和时间间隔 + auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.8); + robot->servoJ(joint_cmd, 0.8, 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) { - // 捕获并打印异常（如机器人未就绪、逆解失败等） - LOG(ERROR) << "Trajectory execution failed: " << e.what(); - throw; // 可选：抛出异常让测试框架捕获，标记测试失败 + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); } -} \ No newline at end of file + + 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."; +} + + + + diff --git a/cmvr-es/ik_solver/CMakeLists.txt b/cmvr-es/ik_solver/CMakeLists.txt index f7150ad5..368b4050 100644 --- a/cmvr-es/ik_solver/CMakeLists.txt +++ b/cmvr-es/ik_solver/CMakeLists.txt @@ -21,9 +21,8 @@ target_link_libraries(ik_solver PUBLIC ) 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 # -------------------------------------------------------- diff --git a/cmvr-es/perception/CMakeLists.txt b/cmvr-es/perception/CMakeLists.txt new file mode 100644 index 00000000..da9d6e42 --- /dev/null +++ b/cmvr-es/perception/CMakeLists.txt @@ -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 +) diff --git a/cmvr-es/perception/include/tag_relative_target_3d.h b/cmvr-es/perception/include/tag_relative_target_3d.h new file mode 100644 index 00000000..79158361 --- /dev/null +++ b/cmvr-es/perception/include/tag_relative_target_3d.h @@ -0,0 +1,215 @@ +// +// Created by lgv on 2026/2/26. +// + +#pragma once + +#include +#include +#include + +#include +#include +#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& camera = nullptr); + + /** + * @brief 设置/替换相机对象。 + * @param camera 抽象相机。 + */ + void setCamera(const std::shared_ptr& 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& 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& 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& 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& 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& 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 anchors_in_tag_; + // 抽象相机对象（由构造函数注入）。 + std::shared_ptr 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 diff --git a/cmvr-es/perception/src/tag_relative_target_3d.cpp b/cmvr-es/perception/src/tag_relative_target_3d.cpp new file mode 100644 index 00000000..ffae4a32 --- /dev/null +++ b/cmvr-es/perception/src/tag_relative_target_3d.cpp @@ -0,0 +1,356 @@ +// +// Created by lgv on 2026/2/26. +// + +#include "perception/include/tag_relative_target_3d.h" + +#include +#include +#include +#include + +#include +#include +#include + +namespace cmvr::perception { + +TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr& 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& 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 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 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& 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& 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 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::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& 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(intrinsics.fx); + const double fy = static_cast(intrinsics.fy); + const double cx = static_cast(intrinsics.cx); + const double cy = static_cast(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 I(height, width); + for (int y = 0; y < height; ++y) { + std::memcpy(I[y], gray.ptr(y), static_cast(width)); + } + + std::vector 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 tag_ids = detector_.getTagsId(); + const std::vector 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(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 diff --git a/cmvr-es/perception/src/tag_relative_target_3d_test.cpp b/cmvr-es/perception/src/tag_relative_target_3d_test.cpp new file mode 100644 index 00000000..5672ff23 --- /dev/null +++ b/cmvr-es/perception/src/tag_relative_target_3d_test.cpp @@ -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 + +#include +#include +#include +#include +#include +#include +#include + +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 valid; + const int side = radius_px * 2 + 1; + valid.reserve(static_cast(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(depth.at(vv, uu)) * 1e-3; + } else if (depth.type() == CV_32FC1) { + z = static_cast(depth.at(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& 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(K.fx) * (p_c.x() / p_c.z()) + static_cast(K.cx); + const double v = static_cast(K.fy) * (p_c.y() / p_c.z()) + static_cast(K.cy); + uv.x = static_cast(std::lround(u)); + uv.y = static_cast(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(u) - static_cast(K.cx)) / static_cast(K.fx) * z_m; + p_c.y() = (static_cast(v) - static_cast(K.cy)) / static_cast(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(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 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 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(target_u) - static_cast(frame.intrinsics.cx)) + / static_cast(frame.intrinsics.fx) * z_depth_m; + p_c_target.y() = (static_cast(target_v) - static_cast(frame.intrinsics.cy)) + / static_cast(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(std::lround(u)); + const int vi = static_cast(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>(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"; +} diff --git a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_annotation b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_annotation index 65dd28f3..6e04d27a 100755 Binary files a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_annotation and b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_annotation differ diff --git a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_interactive-calibration b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_interactive-calibration index d4431ea9..1137b811 100755 Binary files a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_interactive-calibration and b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_interactive-calibration differ diff --git a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_model_diagnostics b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_model_diagnostics index fb272362..507a6256 100755 Binary files a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_model_diagnostics and b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_model_diagnostics differ diff --git a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_version b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_version index 0e67f0e9..9f520d67 100755 Binary files a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_version and b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_version differ diff --git a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_visualisation b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_visualisation index 46b32ff7..7ef1435d 100755 Binary files a/dependency/x86/third_party/opencv/4.13.0/bin/opencv_visualisation and b/dependency/x86/third_party/opencv/4.13.0/bin/opencv_visualisation differ diff --git a/dependency/x86/third_party/opencv/4.13.0/include/opencv4/opencv2/cvconfig.h b/dependency/x86/third_party/opencv/4.13.0/include/opencv4/opencv2/cvconfig.h index 424ad2dc..4bcbdb26 100644 --- a/dependency/x86/third_party/opencv/4.13.0/include/opencv4/opencv2/cvconfig.h +++ b/dependency/x86/third_party/opencv/4.13.0/include/opencv4/opencv2/cvconfig.h @@ -60,7 +60,7 @@ /* #undef HAVE_VULKAN */ /* Define to 1 if you have the header file. */ -#define HAVE_INTTYPES_H 1 +/* #undef HAVE_INTTYPES_H */ /* Intel Integrated Performance Primitives */ #define HAVE_IPP diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/cmake/opencv4/OpenCVModules-release.cmake b/dependency/x86/third_party/opencv/4.13.0/lib/cmake/opencv4/OpenCVModules-release.cmake index a2b59ac3..4511fd51 100644 --- a/dependency/x86/third_party/opencv/4.13.0/lib/cmake/opencv4/OpenCVModules-release.cmake +++ b/dependency/x86/third_party/opencv/4.13.0/lib/cmake/opencv4/OpenCVModules-release.cmake @@ -78,6 +78,7 @@ list(APPEND _IMPORT_CHECK_FILES_FOR_opencv_features2d "${_IMPORT_PREFIX}/lib/lib # Import target "opencv_imgcodecs" for configuration "Release" set_property(TARGET opencv_imgcodecs APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE) 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_SONAME_RELEASE "libopencv_imgcodecs.so.413" ) diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_calib3d.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_calib3d.so.4.13.0 index 07ac05ee..2d1a2906 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_calib3d.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_calib3d.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_core.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_core.so.4.13.0 index c4520abb..c9247622 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_core.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_core.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_dnn.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_dnn.so.4.13.0 index bf7ac4d6..1bceca64 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_dnn.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_dnn.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_features2d.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_features2d.so.4.13.0 index b3eb2c91..6f36865b 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_features2d.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_features2d.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_flann.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_flann.so.4.13.0 index 0977e87e..d7b4ef4d 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_flann.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_flann.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_gapi.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_gapi.so.4.13.0 index 3c8de4b5..0ccc81e7 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_gapi.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_gapi.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_highgui.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_highgui.so.4.13.0 index 560c4676..c85d9f73 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_highgui.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_highgui.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgcodecs.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgcodecs.so.4.13.0 index 410428ad..44f52174 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgcodecs.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgcodecs.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgproc.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgproc.so.4.13.0 index b91b5d35..322abfb4 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgproc.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgproc.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_ml.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_ml.so.4.13.0 index da6cb590..48377633 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_ml.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_ml.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_objdetect.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_objdetect.so.4.13.0 index e1175828..ecde9f6b 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_objdetect.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_objdetect.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_photo.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_photo.so.4.13.0 index e1f651e3..80dbc727 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_photo.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_photo.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_stitching.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_stitching.so.4.13.0 index d6a92701..5a8d4f48 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_stitching.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_stitching.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_video.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_video.so.4.13.0 index 8195d976..ffbdae9d 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_video.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_video.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_videoio.so.4.13.0 b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_videoio.so.4.13.0 index fc83a2fe..b92f40e1 100644 Binary files a/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_videoio.so.4.13.0 and b/dependency/x86/third_party/opencv/4.13.0/lib/libopencv_videoio.so.4.13.0 differ diff --git a/dependency/x86/third_party/opencv/4.13.0/lib/pkgconfig/opencv4.pc b/dependency/x86/third_party/opencv/4.13.0/lib/pkgconfig/opencv4.pc new file mode 100644 index 00000000..3f080808 --- /dev/null +++ b/dependency/x86/third_party/opencv/4.13.0/lib/pkgconfig/opencv4.pc @@ -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}