merge lgv device control with linbo camera updates

cmvr-es/algorithms/CMakeLists.txt

@@ -2,3 +2,4 @@ add_subdirectory(motion_planner)
 add_subdirectory(kinematics/ik_solver)
 add_subdirectory(perception)
 add_subdirectory(controllers)
+add_subdirectory(collision_detection)

cmvr-es/algorithms/collision_detection/CMakeLists.txt

@@ -0,0 +1,48 @@
+add_library(self_collision_checker SHARED
+        self_collision/src/self_collision_checker.cpp
+        self_collision/src/distance_sampling_policy.cpp
+)
+
+target_include_directories(self_collision_checker PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+target_compile_definitions(self_collision_checker PRIVATE
+        PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
+        PINOCCHIO_WITH_HPP_FCL
+        COAL_DISABLE_HPP_FCL_WARNINGS
+)
+
+target_link_libraries(self_collision_checker PUBLIC
+        pinocchio_default
+        pinocchio_parsers
+        pinocchio_collision
+        coal
+)
+
+add_library(cmvr_es::self_collision_checker ALIAS self_collision_checker)
+
+add_executable(self_collision_checker_test
+        self_collision/test/self_collision_checker_test.cpp
+)
+target_link_libraries(self_collision_checker_test PRIVATE
+        cmvr_es::self_collision_checker
+        gtest
+        gtest_main
+        pthread
+)
+target_compile_definitions(self_collision_checker_test PRIVATE
+        CMVR_ES_SOURCE_DIR="${PROJECT_SOURCE_DIR}"
+)
+
+add_executable(self_collision_benchmark
+        self_collision/benchmark/self_collision_benchmark.cpp
+)
+target_link_libraries(self_collision_benchmark PRIVATE
+        cmvr_es::self_collision_checker
+)
+target_compile_definitions(self_collision_benchmark PRIVATE
+        CMVR_ES_SOURCE_DIR="${PROJECT_SOURCE_DIR}"
+)
+
+install(TARGETS self_collision_checker LIBRARY DESTINATION lib)

cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp

@@ -0,0 +1,53 @@
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
+
+int main()
+{
+    const std::string urdf_path = std::string(CMVR_ES_SOURCE_DIR) +
+                                  "/model/xiaoyan_description/dual_arm_collision.urdf";
+    const std::vector<std::string> joint_names{
+        "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R",
+        "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
+    };
+
+    cmvr::SelfCollisionChecker checker;
+    std::string error;
+    if (!checker.init(urdf_path, joint_names, {}, &error)) {
+        std::cerr << "Initialization failed: " << error << '\n';
+        return 1;
+    }
+
+    constexpr std::size_t kIterations = 2000;
+    std::vector<double> samples_us;
+    samples_us.reserve(kIterations);
+    std::vector<double> q(joint_names.size(), 0.0);
+    for (std::size_t iteration = 0; iteration < kIterations; ++iteration) {
+        q[0] = 0.2 * static_cast<double>(iteration % 100) / 100.0;
+        const auto begin = std::chrono::steady_clock::now();
+        const auto result = checker.check(q);
+        const auto end = std::chrono::steady_clock::now();
+        if (!result.valid) {
+            std::cerr << "Collision check failed: " << result.error << '\n';
+            return 1;
+        }
+        samples_us.push_back(std::chrono::duration<double, std::micro>(end - begin).count());
+    }
+
+    std::sort(samples_us.begin(), samples_us.end());
+    double total_us = 0.0;
+    for (const double sample : samples_us) {
+        total_us += sample;
+    }
+    const std::size_t p99_index = static_cast<std::size_t>(0.99 * (samples_us.size() - 1));
+    std::cout << "active_pairs=" << checker.activePairCount() << '\n'
+              << "iterations=" << samples_us.size() << '\n'
+              << "average_us=" << total_us / samples_us.size() << '\n'
+              << "p99_us=" << samples_us[p99_index] << '\n'
+              << "max_us=" << samples_us.back() << '\n';
+    return 0;
+}

cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h

@@ -0,0 +1,46 @@
+#ifndef CMVR_ES_DISTANCE_SAMPLING_POLICY_H
+#define CMVR_ES_DISTANCE_SAMPLING_POLICY_H
+
+#include <chrono>
+#include <string>
+
+#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
+
+namespace cmvr {
+
+struct DistanceSamplingOptions {
+    double max_geometry_displacement_m{0.002};
+    double max_check_period_s{0.01};
+};
+
+class DistanceSamplingPolicy {
+public:
+    using Clock = std::chrono::steady_clock;
+
+    bool configure(const DistanceSamplingOptions& options,
+                   std::string* error = nullptr);
+
+    bool shouldCheck(const CollisionGeometrySnapshot& current,
+                     Clock::time_point now) const;
+
+    void markChecked(const CollisionGeometrySnapshot& current,
+                     Clock::time_point now);
+
+    void reset();
+
+    double displacementSinceLastCheck(
+        const CollisionGeometrySnapshot& current) const;
+
+    bool hasBaseline() const { return has_baseline_; }
+
+private:
+    DistanceSamplingOptions options_{};
+    CollisionGeometrySnapshot last_checked_{};
+    Clock::time_point last_check_time_{};
+    bool configured_{false};
+    bool has_baseline_{false};
+};
+
+} // namespace cmvr
+
+#endif // CMVR_ES_DISTANCE_SAMPLING_POLICY_H

cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h

@@ -0,0 +1,82 @@
+#ifndef CMVR_ES_SELF_COLLISION_CHECKER_H
+#define CMVR_ES_SELF_COLLISION_CHECKER_H
+
+#include <cstddef>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <Eigen/Geometry>
+
+namespace cmvr {
+
+struct CollisionPair {
+    std::string first;
+    std::string second;
+};
+
+struct SelfCollisionOptions {
+    std::vector<CollisionPair> ignored_pairs;
+};
+
+struct CollisionObjectPose {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    std::size_t geometry_index{0};
+    Eigen::Vector3d position{Eigen::Vector3d::Zero()};
+    Eigen::Quaterniond orientation{Eigen::Quaterniond::Identity()};
+    double bounding_radius_m{0.0};
+};
+
+struct CollisionGeometrySnapshot {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    std::vector<CollisionObjectPose, Eigen::aligned_allocator<CollisionObjectPose>> objects;
+};
+
+struct SelfCollisionResult {
+    bool valid{false};
+    bool in_collision{false};
+    double minimum_distance_m{0.0};
+    std::string first;
+    std::string second;
+    std::string error;
+};
+
+// Instances cache Pinocchio work data and are not thread-safe.
+class SelfCollisionChecker {
+public:
+    SelfCollisionChecker();
+    ~SelfCollisionChecker();
+
+    SelfCollisionChecker(SelfCollisionChecker&&) noexcept;
+    SelfCollisionChecker& operator=(SelfCollisionChecker&&) noexcept;
+
+    SelfCollisionChecker(const SelfCollisionChecker&) = delete;
+    SelfCollisionChecker& operator=(const SelfCollisionChecker&) = delete;
+
+    bool init(const std::string& urdf_path,
+              const std::vector<std::string>& active_joint_names,
+              const SelfCollisionOptions& options,
+              std::string* error = nullptr);
+
+    bool makeSnapshot(const std::vector<double>& joint_positions,
+                      CollisionGeometrySnapshot* snapshot,
+                      std::string* error = nullptr);
+
+    SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot);
+    SelfCollisionResult check(const std::vector<double>& joint_positions);
+
+    bool initialized() const;
+    std::size_t dof() const;
+    std::size_t activePairCount() const;
+    const std::vector<std::string>& jointNames() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> impl_;
+};
+
+} // namespace cmvr
+
+#endif // CMVR_ES_SELF_COLLISION_CHECKER_H

cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp

@@ -0,0 +1,100 @@
+#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+namespace cmvr {
+namespace {
+
+void setError(std::string* error, const std::string& message)
+{
+    if (error) {
+        *error = message;
+    }
+}
+
+double rotationAngle(const Eigen::Quaterniond& first,
+                     const Eigen::Quaterniond& second)
+{
+    const double dot = std::clamp(
+        std::abs(first.normalized().dot(second.normalized())), 0.0, 1.0);
+    return 2.0 * std::acos(dot);
+}
+
+} // namespace
+
+bool DistanceSamplingPolicy::configure(const DistanceSamplingOptions& options,
+                                       std::string* error)
+{
+    if (!std::isfinite(options.max_geometry_displacement_m) ||
+        options.max_geometry_displacement_m <= 0.0) {
+        setError(error, "max_geometry_displacement_m must be finite and positive");
+        return false;
+    }
+    if (!std::isfinite(options.max_check_period_s) ||
+        options.max_check_period_s <= 0.0) {
+        setError(error, "max_check_period_s must be finite and positive");
+        return false;
+    }
+    options_ = options;
+    configured_ = true;
+    reset();
+    if (error) {
+        error->clear();
+    }
+    return true;
+}
+
+bool DistanceSamplingPolicy::shouldCheck(const CollisionGeometrySnapshot& current,
+                                         const Clock::time_point now) const
+{
+    if (!configured_ || !has_baseline_) {
+        return true;
+    }
+    const double elapsed_s = std::chrono::duration<double>(now - last_check_time_).count();
+    if (elapsed_s >= options_.max_check_period_s) {
+        return true;
+    }
+    return displacementSinceLastCheck(current) >= options_.max_geometry_displacement_m;
+}
+
+void DistanceSamplingPolicy::markChecked(const CollisionGeometrySnapshot& current,
+                                         const Clock::time_point now)
+{
+    last_checked_ = current;
+    last_check_time_ = now;
+    has_baseline_ = true;
+}
+
+void DistanceSamplingPolicy::reset()
+{
+    last_checked_.objects.clear();
+    last_check_time_ = Clock::time_point{};
+    has_baseline_ = false;
+}
+
+double DistanceSamplingPolicy::displacementSinceLastCheck(
+    const CollisionGeometrySnapshot& current) const
+{
+    if (!has_baseline_ || current.objects.size() != last_checked_.objects.size()) {
+        return std::numeric_limits<double>::infinity();
+    }
+
+    double maximum_displacement = 0.0;
+    for (std::size_t index = 0; index < current.objects.size(); ++index) {
+        const auto& previous = last_checked_.objects[index];
+        const auto& now = current.objects[index];
+        if (previous.geometry_index != now.geometry_index) {
+            return std::numeric_limits<double>::infinity();
+        }
+        const double translation = (now.position - previous.position).norm();
+        const double radius = std::max(previous.bounding_radius_m, now.bounding_radius_m);
+        const double swept_distance =
+            translation + radius * rotationAngle(previous.orientation, now.orientation);
+        maximum_displacement = std::max(maximum_displacement, swept_distance);
+    }
+    return maximum_displacement;
+}
+
+} // namespace cmvr

cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp

@@ -0,0 +1,403 @@
+#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
+
+#include <algorithm>
+#include <cmath>
+#include <filesystem>
+#include <limits>
+#include <set>
+#include <sstream>
+#include <unordered_set>
+#include <utility>
+
+#include <pinocchio/algorithm/geometry.hpp>
+#include <pinocchio/algorithm/joint-configuration.hpp>
+#include <pinocchio/collision/distance.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+
+namespace cmvr {
+namespace {
+
+using LinkPairKey = std::pair<std::string, std::string>;
+
+LinkPairKey canonicalPair(std::string first, std::string second)
+{
+    if (second < first) {
+        std::swap(first, second);
+    }
+    return {std::move(first), std::move(second)};
+}
+
+void setError(std::string* error, const std::string& message)
+{
+    if (error) {
+        *error = message;
+    }
+}
+
+} // namespace
+
+class SelfCollisionChecker::Impl {
+public:
+    bool init(const std::string& urdf_path,
+              const std::vector<std::string>& active_joint_names,
+              const SelfCollisionOptions& options,
+              std::string* error)
+    {
+        reset();
+        if (urdf_path.empty()) {
+            setError(error, "URDF path is empty");
+            return false;
+        }
+        if (!std::filesystem::is_regular_file(urdf_path)) {
+            setError(error, "URDF file does not exist: " + urdf_path);
+            return false;
+        }
+        if (active_joint_names.empty()) {
+            setError(error, "Active joint list is empty");
+            return false;
+        }
+
+        try {
+            pinocchio::urdf::buildModel(urdf_path, model_);
+            pinocchio::urdf::buildGeom(
+                model_, urdf_path, pinocchio::COLLISION, geometry_model_);
+        } catch (const std::exception& exception) {
+            setError(error, "Failed to load collision URDF: " + std::string(exception.what()));
+            reset();
+            return false;
+        }
+
+        if (geometry_model_.ngeoms == 0) {
+            setError(error, "URDF contains no collision geometry: " + urdf_path);
+            reset();
+            return false;
+        }
+
+        std::unordered_set<pinocchio::JointIndex> active_joint_ids;
+        std::unordered_set<std::string> unique_joint_names;
+        joint_names_.reserve(active_joint_names.size());
+        joint_q_indices_.reserve(active_joint_names.size());
+        for (const auto& joint_name : active_joint_names) {
+            if (joint_name.empty() || !unique_joint_names.insert(joint_name).second) {
+                setError(error, "Active joint names must be non-empty and unique");
+                reset();
+                return false;
+            }
+            if (!model_.existJointName(joint_name)) {
+                setError(error, "Joint not found in URDF: " + joint_name);
+                reset();
+                return false;
+            }
+            const pinocchio::JointIndex joint_id = model_.getJointId(joint_name);
+            const auto& joint = model_.joints[joint_id];
+            if (joint.nq() != 1) {
+                setError(error, "Only one-DoF active joints are supported: " + joint_name);
+                reset();
+                return false;
+            }
+            active_joint_ids.insert(joint_id);
+            joint_names_.push_back(joint_name);
+            joint_q_indices_.push_back(joint.idx_q());
+        }
+
+        geometry_link_names_.resize(geometry_model_.ngeoms);
+        std::unordered_set<std::string> selected_link_names;
+        for (pinocchio::GeomIndex geometry_id = 0;
+             geometry_id < geometry_model_.ngeoms;
+             ++geometry_id) {
+            auto& geometry = geometry_model_.geometryObjects[geometry_id];
+            const std::string link_name = geometry.parentFrame < model_.frames.size()
+                                              ? model_.frames[geometry.parentFrame].name
+                                              : geometry.name;
+            geometry_link_names_[geometry_id] = link_name;
+
+            const bool is_static = geometry.parentJoint == 0;
+            const bool belongs_to_active_arm = active_joint_ids.count(geometry.parentJoint) != 0;
+            if (!is_static && !belongs_to_active_arm) {
+                continue;
+            }
+
+            if (!geometry.geometry) {
+                setError(error, "Collision geometry is null for link: " + link_name);
+                reset();
+                return false;
+            }
+            geometry.geometry->computeLocalAABB();
+            selected_geometry_indices_.push_back(geometry_id);
+            selected_link_names.insert(link_name);
+        }
+
+        if (selected_geometry_indices_.size() < 2) {
+            setError(error, "Fewer than two collision geometries remain after arm filtering");
+            reset();
+            return false;
+        }
+
+        std::set<LinkPairKey> ignored_pairs;
+        for (const auto& pair : options.ignored_pairs) {
+            if (pair.first.empty() || pair.second.empty() || pair.first == pair.second) {
+                setError(error, "Ignored collision pairs require two different non-empty links");
+                reset();
+                return false;
+            }
+            if (!selected_link_names.count(pair.first) || !selected_link_names.count(pair.second)) {
+                setError(error,
+                         "Ignored collision pair references an inactive or unknown link: " +
+                             pair.first + ", " + pair.second);
+                reset();
+                return false;
+            }
+            ignored_pairs.insert(canonicalPair(pair.first, pair.second));
+        }
+
+        geometry_model_.removeAllCollisionPairs();
+        for (std::size_t first_index = 0;
+             first_index < selected_geometry_indices_.size();
+             ++first_index) {
+            const auto first_geometry_id = selected_geometry_indices_[first_index];
+            const auto& first_geometry = geometry_model_.geometryObjects[first_geometry_id];
+            for (std::size_t second_index = first_index + 1;
+                 second_index < selected_geometry_indices_.size();
+                 ++second_index) {
+                const auto second_geometry_id = selected_geometry_indices_[second_index];
+                const auto& second_geometry = geometry_model_.geometryObjects[second_geometry_id];
+
+                if (first_geometry.parentJoint == second_geometry.parentJoint) {
+                    continue;
+                }
+                if (model_.parents[first_geometry.parentJoint] == second_geometry.parentJoint ||
+                    model_.parents[second_geometry.parentJoint] == first_geometry.parentJoint) {
+                    continue;
+                }
+
+                const auto link_pair = canonicalPair(
+                    geometry_link_names_[first_geometry_id],
+                    geometry_link_names_[second_geometry_id]);
+                if (ignored_pairs.count(link_pair)) {
+                    continue;
+                }
+                geometry_model_.addCollisionPair(
+                    pinocchio::CollisionPair(first_geometry_id, second_geometry_id));
+            }
+        }
+
+        if (geometry_model_.collisionPairs.empty()) {
+            setError(error, "No active collision pairs remain after filtering");
+            reset();
+            return false;
+        }
+
+        data_ = std::make_unique<pinocchio::Data>(model_);
+        geometry_data_ = std::make_unique<pinocchio::GeometryData>(geometry_model_);
+        for (auto& request : geometry_data_->distanceRequests) {
+            request.enable_signed_distance = true;
+        }
+        neutral_q_ = pinocchio::neutral(model_);
+        initialized_ = true;
+        if (error) {
+            error->clear();
+        }
+        return true;
+    }
+
+    bool makeSnapshot(const std::vector<double>& joint_positions,
+                      CollisionGeometrySnapshot* snapshot,
+                      std::string* error)
+    {
+        if (!initialized_) {
+            setError(error, "SelfCollisionChecker is not initialized");
+            return false;
+        }
+        if (!snapshot) {
+            setError(error, "Collision snapshot output is null");
+            return false;
+        }
+        if (joint_positions.size() != joint_names_.size()) {
+            std::ostringstream stream;
+            stream << "Joint position size mismatch: expected " << joint_names_.size()
+                   << ", got " << joint_positions.size();
+            setError(error, stream.str());
+            return false;
+        }
+
+        Eigen::VectorXd q = neutral_q_;
+        for (std::size_t index = 0; index < joint_positions.size(); ++index) {
+            if (!std::isfinite(joint_positions[index])) {
+                setError(error, "Joint position contains a non-finite value: " + joint_names_[index]);
+                return false;
+            }
+            q[joint_q_indices_[index]] = joint_positions[index];
+        }
+
+        try {
+            pinocchio::updateGeometryPlacements(
+                model_, *data_, geometry_model_, *geometry_data_, q);
+        } catch (const std::exception& exception) {
+            setError(error, "Failed to update collision geometry: " + std::string(exception.what()));
+            return false;
+        }
+
+        snapshot->objects.clear();
+        snapshot->objects.reserve(selected_geometry_indices_.size());
+        for (const auto geometry_id : selected_geometry_indices_) {
+            const auto& placement = geometry_data_->oMg[geometry_id];
+            const auto& geometry = geometry_model_.geometryObjects[geometry_id];
+            CollisionObjectPose pose;
+            pose.geometry_index = geometry_id;
+            pose.position = placement.translation();
+            pose.orientation = Eigen::Quaterniond(placement.rotation()).normalized();
+            pose.bounding_radius_m = std::max(0.0, geometry.geometry->aabb_radius);
+            snapshot->objects.push_back(std::move(pose));
+        }
+        if (error) {
+            error->clear();
+        }
+        return true;
+    }
+
+    SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot)
+    {
+        SelfCollisionResult result;
+        if (!initialized_) {
+            result.error = "SelfCollisionChecker is not initialized";
+            return result;
+        }
+        if (snapshot.objects.size() != selected_geometry_indices_.size()) {
+            result.error = "Collision snapshot size does not match initialized geometry";
+            return result;
+        }
+
+        for (std::size_t index = 0; index < snapshot.objects.size(); ++index) {
+            const auto& pose = snapshot.objects[index];
+            if (pose.geometry_index != selected_geometry_indices_[index] ||
+                pose.geometry_index >= geometry_data_->oMg.size()) {
+                result.error = "Collision snapshot geometry order is invalid";
+                return result;
+            }
+            if (!pose.position.allFinite() || !pose.orientation.coeffs().allFinite() ||
+                pose.orientation.norm() <= std::numeric_limits<double>::epsilon()) {
+                result.error = "Collision snapshot contains an invalid pose";
+                return result;
+            }
+            geometry_data_->oMg[pose.geometry_index] = pinocchio::SE3(
+                pose.orientation.normalized().toRotationMatrix(), pose.position);
+        }
+
+        try {
+            const std::size_t pair_index =
+                pinocchio::computeDistances(geometry_model_, *geometry_data_);
+            if (pair_index >= geometry_model_.collisionPairs.size()) {
+                result.error = "Collision distance computation returned no active pair";
+                return result;
+            }
+            const auto& pair = geometry_model_.collisionPairs[pair_index];
+            result.minimum_distance_m = geometry_data_->distanceResults[pair_index].min_distance;
+            result.first = geometry_link_names_[pair.first];
+            result.second = geometry_link_names_[pair.second];
+            result.in_collision = result.minimum_distance_m <= 0.0;
+            result.valid = std::isfinite(result.minimum_distance_m);
+            if (!result.valid) {
+                result.error = "Collision distance is not finite";
+            }
+        } catch (const std::exception& exception) {
+            result.error = "Collision distance computation failed: " + std::string(exception.what());
+        }
+        return result;
+    }
+
+    SelfCollisionResult check(const std::vector<double>& joint_positions)
+    {
+        CollisionGeometrySnapshot snapshot;
+        std::string error;
+        if (!makeSnapshot(joint_positions, &snapshot, &error)) {
+            SelfCollisionResult result;
+            result.error = std::move(error);
+            return result;
+        }
+        return check(snapshot);
+    }
+
+    void reset()
+    {
+        initialized_ = false;
+        joint_names_.clear();
+        joint_q_indices_.clear();
+        selected_geometry_indices_.clear();
+        geometry_link_names_.clear();
+        geometry_data_.reset();
+        data_.reset();
+        model_ = pinocchio::Model{};
+        geometry_model_ = pinocchio::GeometryModel{};
+        neutral_q_.resize(0);
+    }
+
+    bool initialized_{false};
+    std::vector<std::string> joint_names_;
+    std::vector<int> joint_q_indices_;
+    std::vector<pinocchio::GeomIndex> selected_geometry_indices_;
+    std::vector<std::string> geometry_link_names_;
+    pinocchio::Model model_;
+    pinocchio::GeometryModel geometry_model_;
+    std::unique_ptr<pinocchio::Data> data_;
+    std::unique_ptr<pinocchio::GeometryData> geometry_data_;
+    Eigen::VectorXd neutral_q_;
+};
+
+SelfCollisionChecker::SelfCollisionChecker()
+    : impl_(std::make_unique<Impl>())
+{
+}
+
+SelfCollisionChecker::~SelfCollisionChecker() = default;
+SelfCollisionChecker::SelfCollisionChecker(SelfCollisionChecker&&) noexcept = default;
+SelfCollisionChecker& SelfCollisionChecker::operator=(SelfCollisionChecker&&) noexcept = default;
+
+bool SelfCollisionChecker::init(const std::string& urdf_path,
+                                const std::vector<std::string>& active_joint_names,
+                                const SelfCollisionOptions& options,
+                                std::string* error)
+{
+    return impl_->init(urdf_path, active_joint_names, options, error);
+}
+
+bool SelfCollisionChecker::makeSnapshot(const std::vector<double>& joint_positions,
+                                        CollisionGeometrySnapshot* snapshot,
+                                        std::string* error)
+{
+    return impl_->makeSnapshot(joint_positions, snapshot, error);
+}
+
+SelfCollisionResult SelfCollisionChecker::check(const CollisionGeometrySnapshot& snapshot)
+{
+    return impl_->check(snapshot);
+}
+
+SelfCollisionResult SelfCollisionChecker::check(const std::vector<double>& joint_positions)
+{
+    return impl_->check(joint_positions);
+}
+
+bool SelfCollisionChecker::initialized() const
+{
+    return impl_->initialized_;
+}
+
+std::size_t SelfCollisionChecker::dof() const
+{
+    return impl_->joint_names_.size();
+}
+
+std::size_t SelfCollisionChecker::activePairCount() const
+{
+    return impl_->geometry_model_.collisionPairs.size();
+}
+
+const std::vector<std::string>& SelfCollisionChecker::jointNames() const
+{
+    return impl_->joint_names_;
+}
+
+} // namespace cmvr

cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp

@@ -0,0 +1,236 @@
+#include <chrono>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h"
+#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
+
+namespace cmvr {
+namespace {
+
+const std::vector<std::string> kRightArmJoints{
+    "R_SHOULDER_P",
+    "R_SHOULDER_R",
+    "R_SHOULDER_Y",
+    "R_ELBOW_R",
+    "R_WRIST_P",
+    "R_WRIST_Y",
+    "R_WRIST_R",
+};
+
+const std::vector<std::string> kGen2RightArmJoints{
+    "right_arm_J1",
+    "right_arm_J2",
+    "right_arm_J3",
+    "right_arm_J4",
+    "right_arm_J5",
+    "right_arm_J6",
+    "right_arm_J7",
+};
+
+const std::vector<double> kGen2SetupPose{
+    0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0,
+};
+
+const std::vector<double> kGen2WarningPose{
+    2.45028525340088,
+    0.413065394330014,
+    -1.78610031118294,
+    2.3232081721811,
+    -2.96828882895788,
+    -1.59350098130002,
+    0.582912411114367,
+};
+
+const std::vector<double> kGen2StopPose{
+    2.13758633436379,
+    1.61835160165575,
+    -2.3836142221041,
+    0.964538527544213,
+    -0.00382525077004825,
+    1.74586899135531,
+    -0.336868659266887,
+};
+
+const std::vector<double> kGen2CollisionPose{
+    -0.42656969579233,
+    1.41426471041774,
+    -2.67949400419915,
+    2.45814854129954,
+    -2.35907388079205,
+    1.14125209449898,
+    1.53232912981414,
+};
+
+const std::vector<double> kGen2TorsoCollisionPose{
+    1.57607137794121,
+    2.06613762981425,
+    -1.76915077905899,
+    0.959251437141443,
+    -0.725973209527894,
+    1.79390262120717,
+    0.2223354372144,
+};
+
+std::string collisionUrdfPath()
+{
+    return std::string(CMVR_ES_SOURCE_DIR) +
+           "/model/xiaoyan_description/dual_arm_collision.urdf";
+}
+
+std::string gen2CollisionUrdfPath()
+{
+    return std::string(CMVR_ES_SOURCE_DIR) +
+           "/model/gen2/collision/robot_collision.urdf";
+}
+
+SelfCollisionOptions gen2CollisionOptions()
+{
+    SelfCollisionOptions options;
+    options.ignored_pairs.push_back({"arm_link_5_2", "arm_link_7_2"});
+    options.ignored_pairs.push_back({"body_link", "arm_link_2_2"});
+    return options;
+}
+
+CollisionGeometrySnapshot singleObjectSnapshot(double x,
+                                                double angle,
+                                                double radius)
+{
+    CollisionGeometrySnapshot snapshot;
+    CollisionObjectPose pose;
+    pose.geometry_index = 1;
+    pose.position = Eigen::Vector3d(x, 0.0, 0.0);
+    pose.orientation = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
+    pose.bounding_radius_m = radius;
+    snapshot.objects.push_back(pose);
+    return snapshot;
+}
+
+TEST(SelfCollisionCheckerTest, LoadsRightArmFromDualArmUrdf)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error;
+    EXPECT_EQ(checker.dof(), 7U);
+    EXPECT_GT(checker.activePairCount(), 0U);
+
+    CollisionGeometrySnapshot snapshot;
+    ASSERT_TRUE(checker.makeSnapshot(std::vector<double>(7, 0.0), &snapshot, &error)) << error;
+    EXPECT_EQ(snapshot.objects.size(), 11U);
+
+    const SelfCollisionResult result = checker.check(snapshot);
+    ASSERT_TRUE(result.valid) << result.error;
+    EXPECT_TRUE(result.first.rfind("L_", 0) != 0);
+    EXPECT_TRUE(result.second.rfind("L_", 0) != 0);
+}
+
+TEST(SelfCollisionCheckerTest, RejectsWrongJointVectorSize)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error;
+
+    CollisionGeometrySnapshot snapshot;
+    EXPECT_FALSE(checker.makeSnapshot(std::vector<double>(6, 0.0), &snapshot, &error));
+    EXPECT_NE(error.find("size mismatch"), std::string::npos);
+}
+
+TEST(SelfCollisionCheckerTest, RemovesConfiguredIgnoredPair)
+{
+    SelfCollisionChecker baseline;
+    SelfCollisionChecker filtered;
+    std::string error;
+    ASSERT_TRUE(baseline.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error;
+
+    SelfCollisionOptions options;
+    options.ignored_pairs.push_back({"base_link", "R_ELBOW_R_S"});
+    ASSERT_TRUE(filtered.init(collisionUrdfPath(), kRightArmJoints, options, &error)) << error;
+    EXPECT_EQ(filtered.activePairCount() + 1, baseline.activePairCount());
+}
+
+TEST(SelfCollisionCheckerTest, LoadsGen2RightArmCollisionModel)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(
+        gen2CollisionUrdfPath(),
+        kGen2RightArmJoints,
+        gen2CollisionOptions(),
+        &error)) << error;
+    EXPECT_EQ(checker.dof(), 7U);
+    EXPECT_EQ(checker.activePairCount(), 19U);
+
+    CollisionGeometrySnapshot snapshot;
+    ASSERT_TRUE(checker.makeSnapshot(kGen2SetupPose, &snapshot, &error)) << error;
+    EXPECT_EQ(snapshot.objects.size(), 8U);
+
+    const SelfCollisionResult setup_result = checker.check(snapshot);
+    ASSERT_TRUE(setup_result.valid) << setup_result.error;
+    EXPECT_FALSE(setup_result.in_collision);
+    EXPECT_GT(setup_result.minimum_distance_m, 0.02);
+}
+
+TEST(SelfCollisionCheckerTest, ClassifiesGen2SafetyDistances)
+{
+    SelfCollisionChecker checker;
+    std::string error;
+    ASSERT_TRUE(checker.init(
+        gen2CollisionUrdfPath(),
+        kGen2RightArmJoints,
+        gen2CollisionOptions(),
+        &error)) << error;
+
+    const SelfCollisionResult warning_result = checker.check(kGen2WarningPose);
+    ASSERT_TRUE(warning_result.valid) << warning_result.error;
+    EXPECT_FALSE(warning_result.in_collision);
+    EXPECT_GT(warning_result.minimum_distance_m, 0.005);
+    EXPECT_LE(warning_result.minimum_distance_m, 0.02);
+
+    const SelfCollisionResult stop_result = checker.check(kGen2StopPose);
+    ASSERT_TRUE(stop_result.valid) << stop_result.error;
+    EXPECT_FALSE(stop_result.in_collision);
+    EXPECT_GT(stop_result.minimum_distance_m, 0.0);
+    EXPECT_LE(stop_result.minimum_distance_m, 0.005);
+
+    const SelfCollisionResult collision_result = checker.check(kGen2CollisionPose);
+    ASSERT_TRUE(collision_result.valid) << collision_result.error;
+    EXPECT_TRUE(collision_result.in_collision);
+    EXPECT_LE(collision_result.minimum_distance_m, 0.0);
+
+    const SelfCollisionResult torso_result =
+        checker.check(kGen2TorsoCollisionPose);
+    ASSERT_TRUE(torso_result.valid) << torso_result.error;
+    EXPECT_TRUE(torso_result.in_collision);
+    EXPECT_LE(torso_result.minimum_distance_m, 0.0);
+    EXPECT_TRUE(torso_result.first == "body_link" ||
+                torso_result.second == "body_link");
+}
+
+TEST(DistanceSamplingPolicyTest, SamplesByAccumulatedGeometryDisplacement)
+{
+    DistanceSamplingPolicy policy;
+    DistanceSamplingOptions options;
+    options.max_geometry_displacement_m = 0.002;
+    options.max_check_period_s = 0.01;
+    std::string error;
+    ASSERT_TRUE(policy.configure(options, &error)) << error;
+
+    const auto start = DistanceSamplingPolicy::Clock::now();
+    const auto initial = singleObjectSnapshot(0.0, 0.0, 0.2);
+    EXPECT_TRUE(policy.shouldCheck(initial, start));
+    policy.markChecked(initial, start);
+
+    EXPECT_FALSE(policy.shouldCheck(
+        singleObjectSnapshot(0.001, 0.0, 0.2), start + std::chrono::milliseconds(1)));
+    EXPECT_TRUE(policy.shouldCheck(
+        singleObjectSnapshot(0.0021, 0.0, 0.2), start + std::chrono::milliseconds(2)));
+    EXPECT_TRUE(policy.shouldCheck(
+        singleObjectSnapshot(0.0, 0.011, 0.2), start + std::chrono::milliseconds(2)));
+    EXPECT_TRUE(policy.shouldCheck(
+        initial, start + std::chrono::milliseconds(10)));
+}
+
+} // namespace
+} // namespace cmvr

cmvr-es/algorithms/controllers/CMakeLists.txt

@@ -12,7 +12,7 @@ add_subdirectory(arm_control)
 #)  其他动态库类似
 file(GLOB SRC
         ${CMAKE_CURRENT_SOURCE_DIR}/pid/src/pid_controller.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/ibvs/src/ibvs_controller.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/pbvs/src/pbvs_controller.cpp
 
 )
 
@@ -58,3 +58,13 @@ target_link_libraries(controller PUBLIC
 add_library(cmvr_es::algorithms::controller ALIAS controller)
 
 install(TARGETS controller LIBRARY DESTINATION lib)
+
+add_executable(pbvs_controller_test
+        pbvs/src/pbvs_controller_test.cpp
+)
+
+target_link_libraries(pbvs_controller_test PRIVATE
+        cmvr_es::algorithms::controller
+        gtest
+        gtest_main
+)

cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h

@@ -25,6 +25,7 @@ public:
         double stop_command_velocity_norm{1e-3};
         double stop_measured_velocity_norm{1e-2};
         double stop_acceleration{0.5};
+        double stop_timeout_s{2.0};
     };
 
     using ReadStateCallback = std::function<bool(std::vector<double>& q, std::vector<double>& qd)>;
@@ -48,11 +49,14 @@ public:
     void shutdown();
 
     bool busy() const { return busy_.load(); }
+    double stopTimeoutS() const { return config_.stop_timeout_s; }
     CartesianVelocity getCommandTwistBase() const;
 
 private:
     void ensureWorkerStarted_();
     void workerLoop_();
+    void requestStop_(std::optional<double> acceleration = std::nullopt);
+    void abortCommand_();
     void sendZero_();
 
     static double velocityNorm_(const std::vector<double>& velocity);

cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp

@@ -29,6 +29,9 @@ CartesianVelocityController::Config normalizeConfig(CartesianVelocityController:
     if (config.stop_acceleration <= 0.0) {
         config.stop_acceleration = defaults.stop_acceleration;
     }
+    if (!std::isfinite(config.stop_timeout_s) || config.stop_timeout_s <= 0.0) {
+        config.stop_timeout_s = defaults.stop_timeout_s;
+    }
     return config;
 }
 
@@ -61,9 +64,14 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
     if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
         return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
     }
-    if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
+    if (!worker_ || !worker_->joinable()) {
+        if (busy_.exchange(true)) {
             return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
         }
+    } else {
+        // speedL is a streaming command: an existing worker may receive a new target.
+        busy_.store(true);
+    }
 
     ensureWorkerStarted_();
 
@@ -103,15 +111,13 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
     if (!worker_ || !worker_->joinable()) {
         return Result::success();
     }
-    {
+    // A completed speedL command leaves the worker thread joinable but idle.
-        std::lock_guard<std::mutex> lock(mutex_);
+    // Do not turn that idle worker into a new command just because a caller
-        target_twist_ = {};
+    // requests a stop during a task transition.
-        target_frame_ = FrameType::Base;
+    if (!busy_.load()) {
-        target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
+        return Result::success();
-        command_active_ = true;
-        ++command_version_;
     }
-    cv_.notify_all();
+    requestStop_(acceleration);
     return Result::success();
 }
 
@@ -192,27 +198,26 @@ void CartesianVelocityController::workerLoop_()
             }
 
             if (!planner_->updateSpeedLAcceleration(acceleration)) {
-                if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                    std::lock_guard<std::mutex> lock(mutex_);
+                    abortCommand_();
-                    command_active_ = false;
-                    sendZero_();
-                    busy_.store(false);
                     break;
                 }
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
                                 << acceleration;
-                sendZero_();
+                requestStop_();
-                busy_.store(false);
+                continue;
-                return;
             }
 
             std::vector<double> q_now;
             std::vector<double> qd_now;
             if (!read_state_(q_now, qd_now)) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed";
-                sendZero_();
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                busy_.store(false);
+                    abortCommand_();
-                return;
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             std::vector<double> qd_cmd;
@@ -222,9 +227,12 @@ void CartesianVelocityController::workerLoop_()
                                 << target_twist.vz << ", " << target_twist.wx << ", "
                                 << target_twist.wy << ", " << target_twist.wz
                                 << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
-                sendZero_();
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                busy_.store(false);
+                    abortCommand_();
-                return;
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             JointVelocityCommand velocity_command;
@@ -233,9 +241,12 @@ void CartesianVelocityController::workerLoop_()
             if (!send_result.ok()) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
                                 << send_result.message;
-                sendZero_();
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                busy_.store(false);
+                    abortCommand_();
-                return;
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             if (twistNorm_(target_twist) < config_.stop_twist_norm &&
@@ -260,6 +271,32 @@ void CartesianVelocityController::workerLoop_()
     busy_.store(false);
 }
 
+void CartesianVelocityController::requestStop_(const std::optional<double> acceleration)
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+        target_acceleration_ = acceleration.has_value() ? *acceleration
+                                                         : config_.stop_acceleration;
+        command_active_ = true;
+        ++command_version_;
+    }
+    cv_.notify_all();
+}
+
+void CartesianVelocityController::abortCommand_()
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        command_active_ = false;
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+    }
+    sendZero_();
+    busy_.store(false);
+}
+
 void CartesianVelocityController::sendZero_()
 {
     if (!send_velocity_) {

cmvr-es/algorithms/controllers/pbvs/include/pbvs_controller.h

@@ -0,0 +1,453 @@
+#pragma once
+
+#ifndef CMVR_PBVS_CONTROLLER_H
+#define CMVR_PBVS_CONTROLLER_H
+
+#include <array>
+
+#include <Eigen/Dense>
+
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr {
+
+/**
+ * @brief 基于相对 3D 位姿的 PBVS 控制器。
+ *
+ * 坐标系：
+ *
+ *   G : Screen Tag 坐标系，同时作为屏幕参考坐标系
+ *   P : TCP / 触控点坐标系
+ *
+ * 输入：
+ *
+ *   ^G T_P_des : TCP 相对于 Screen Tag 的目标位姿
+ *   ^G T_P_cur : TCP 相对于 Screen Tag 的当前位姿
+ *
+ * 输出：
+ *
+ *   ^G V_P =
+ *
+ *   [ vx ]
+ *   [ vy ]
+ *   [ vz ]
+ *   [ wx ]
+ *   [ wy ]
+ *   [ wz ]
+ *
+ * 即 TCP 在 Screen Tag 坐标系 G 下表达的 6D Cartesian Twist。
+ *
+ * 位置误差：
+ *
+ *   e_p = p_des - p_cur
+ *
+ * 姿态误差：
+ *
+ *   R_err = R_des * R_cur^T
+ *
+ *   e_R = Log(R_err)^vee
+ *
+ * 控制律：
+ *
+ *   v = Kp * e_p
+ *   w = Kr * e_R
+ *
+ * 本类只负责视觉伺服 6D Twist 计算，不负责：
+ *
+ * - AprilTag 检测
+ * - RobotArm
+ * - G -> Base 坐标转换
+ * - IK
+ * - speedL 下发
+ */
+class PbvsController {
+public:
+    enum class ComputeStatus {
+        OK = 0,
+        TARGET_NOT_SET,
+        INVALID_INPUT,
+        INVALID_DT,
+        INVALID_TARGET,
+        INVALID_CURRENT_POSE
+    };
+
+    struct Output {
+        // 当前位置误差，单位 m
+        Eigen::Vector3d position_error_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 当前姿态误差 rotation-vector，单位 rad
+        Eigen::Vector3d rotation_error_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // PBVS 计算得到的原始线速度，单位 m/s
+        Eigen::Vector3d raw_linear_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // PBVS 计算得到的原始角速度，单位 rad/s
+        Eigen::Vector3d raw_angular_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 经过限幅 / 加速度 / 滤波后的线速度
+        Eigen::Vector3d linear_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 经过限幅 / 加速度 / 滤波后的角速度
+        Eigen::Vector3d angular_velocity_G{
+            Eigen::Vector3d::Zero()
+        };
+
+        // 可直接取出的 CartesianVelocity。
+        //
+        // 注意：
+        // 这里仍然是在 G / ScreenTag frame 下表达。
+        device::CartesianVelocity twist_G{};
+
+        bool position_reached{false};
+        bool orientation_reached{false};
+        bool reached{false};
+
+        bool valid{false};
+    };
+
+public:
+    PbvsController();
+
+    /**
+     * @brief 设置完整目标位姿。
+     *
+     * @param T_G_P_des TCP(P) 相对于 ScreenTag(G) 的目标位姿。
+     */
+    bool setTargetPose(
+        const Eigen::Matrix4d& T_G_P_des);
+
+    /**
+     * @brief 使用目标位置 + 目标姿态设置期望位姿。
+     */
+    bool setTargetPose(
+        const Eigen::Vector3d& position_G,
+        const Eigen::Matrix3d& rotation_G_P);
+
+    /**
+     * @brief 当前是否已经设置有效目标。
+     */
+    bool hasTarget() const {
+        return target_valid_;
+    }
+
+    /**
+     * @brief 获取目标位姿。
+     */
+    const Eigen::Matrix4d& targetPose() const {
+        return T_G_P_des_;
+    }
+
+    /**
+     * @brief 根据当前 TCP 位姿计算 PBVS 速度命令。
+     *
+     * @param T_G_P_cur 当前 TCP 相对于 Screen Tag 的位姿。
+     * @param dt        控制周期，单位 s。
+     * @param output    输出结果。
+     *
+     * @return 成功返回 true。
+     */
+    bool compute(
+        const Eigen::Matrix4d& T_G_P_cur,
+        double dt,
+        Output& output);
+
+    /**
+     * @brief 便捷接口，只输出 CartesianVelocity。
+     *
+     * 注意输出仍然是 G frame。
+     */
+    bool compute(
+        const Eigen::Matrix4d& T_G_P_cur,
+        double dt,
+        device::CartesianVelocity& twist_G_out);
+
+    /**
+     * @brief 设置位置比例增益。
+     *
+     * x/y/z 分别控制。
+     */
+    void setPositionGain(
+        const Eigen::Vector3d& kp);
+
+    /**
+     * @brief 设置姿态比例增益。
+     *
+     * rx/ry/rz 分别控制。
+     */
+    void setRotationGain(
+        const Eigen::Vector3d& kr);
+
+    /**
+     * @brief 设置六维速度上限。
+     *
+     * [vx vy vz wx wy wz]
+     *
+     * 前三维 m/s；
+     * 后三维 rad/s。
+     */
+    void setVelocityLimit6(
+        const std::array<double, 6>& vmax6);
+
+    /**
+     * @brief 设置六维加速度限制。
+     *
+     * 前三维 m/s^2；
+     * 后三维 rad/s^2。
+     *
+     * <= 0 表示对应维度不限制。
+     */
+    void setAccelerationLimit6(
+        const std::array<double, 6>& amax6);
+
+    /**
+     * @brief 设置六维误差阈值。
+     *
+     * [x y z rx ry rz]
+     *
+     * 前三维 m；
+     * 后三维 rad。
+     */
+    void setTolerance6(
+        const std::array<double, 6>& tolerance6);
+
+    /**
+     * @brief 设置一阶低通 alpha。
+     *
+     * alpha = 1：
+     * 不滤波。
+     *
+     * 0 < alpha < 1：
+     *
+     * cmd =
+     * alpha * current
+     * +
+     * (1-alpha) * previous
+     */
+    void setTwistFilterAlpha(double alpha);
+
+    /**
+     * @brief 是否启用每个轴。
+     *
+     * 默认 6DoF 全部开启。
+     *
+     * 例如以后如果不希望控制 yaw：
+     *
+     * enabled[5] = false;
+     */
+    void setAxisEnabled(
+        const std::array<bool, 6>& enabled);
+
+    /**
+     * @brief 清空速度历史，但保留目标。
+     */
+    void resetTwistCommandState();
+
+    /**
+     * @brief 清空整个 PBVS 状态和目标。
+     */
+    void reset();
+
+    ComputeStatus lastComputeStatus() const {
+        return last_compute_status_;
+    }
+
+    static const char* statusToString(
+        ComputeStatus status);
+
+    const Eigen::Vector3d& lastPositionError() const {
+        return last_position_error_G_;
+    }
+
+    const Eigen::Vector3d& lastRotationError() const {
+        return last_rotation_error_G_;
+    }
+
+    const Eigen::Vector3d& positionGain() const {
+        return kp_position_;
+    }
+
+    const Eigen::Vector3d& rotationGain() const {
+        return kp_rotation_;
+    }
+
+    const std::array<double, 6>& velocityLimit6() const {
+        return vmax6_;
+    }
+
+    const std::array<double, 6>& accelerationLimit6() const {
+        return amax6_;
+    }
+
+    const std::array<double, 6>& tolerance6() const {
+        return tolerance6_;
+    }
+
+    double twistFilterAlpha() const {
+        return twist_lpf_alpha_;
+    }
+
+    const Eigen::Matrix<double, 6, 1>&
+    lastTwistCommandG() const {
+        return last_twist_cmd_G_;
+    }
+
+    bool lastReached() const {
+        return last_reached_;
+    }
+
+private:
+    static bool isFiniteTransform(
+        const Eigen::Matrix4d& T);
+
+    static bool hasValidBottomRow(
+        const Eigen::Matrix4d& T,
+        double tolerance = 1e-6);
+
+    static bool isValidTransform(
+        const Eigen::Matrix4d& T);
+
+    /**
+     * @brief 将可能有微小数值误差的旋转矩阵投影到 SO(3)。
+     */
+    static Eigen::Matrix3d projectToSO3(
+        const Eigen::Matrix3d& R);
+
+    /**
+     * @brief 计算空间旋转误差，在 G frame 表达。
+     *
+     * R_err = R_des * R_cur^T
+     *
+     * e_R = Log(R_err)^vee
+     */
+    static Eigen::Vector3d rotationError(
+        const Eigen::Matrix3d& R_des,
+        const Eigen::Matrix3d& R_cur);
+
+    static double clampValue(
+        double value,
+        double lower,
+        double upper);
+
+    static device::CartesianVelocity
+    toCartesianVelocity(
+        const Eigen::Matrix<double, 6, 1>& twist);
+
+private:
+    // ---------------- target ----------------
+
+    Eigen::Matrix4d T_G_P_des_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    bool target_valid_{false};
+
+    // ---------------- gains ----------------
+
+    Eigen::Vector3d kp_position_{
+        2.0,
+        2.0,
+        1.5
+    };
+
+    Eigen::Vector3d kp_rotation_{
+        1.5,
+        1.5,
+        1.5
+    };
+
+    // ---------------- velocity limits ----------------
+
+    //
+    // [vx vy vz wx wy wz]
+    //
+    std::array<double, 6> vmax6_{{
+        0.10,
+        0.10,
+        0.05,
+        0.50,
+        0.50,
+        0.50
+    }};
+
+    // ---------------- acceleration limits ----------------
+
+    std::array<double, 6> amax6_{{
+        0.50,
+        0.50,
+        0.30,
+        2.0,
+        2.0,
+        2.0
+    }};
+
+    // ---------------- tolerance ----------------
+
+    std::array<double, 6> tolerance6_{{
+        0.0015,  // x  1.5 mm
+        0.0015,  // y  1.5 mm
+        0.0020,  // z  2.0 mm
+
+        0.05,    // rx ~2.9 deg
+        0.05,    // ry
+        0.05     // rz
+    }};
+
+    // ---------------- axis enable ----------------
+
+    std::array<bool, 6> axis_enabled_{{
+        true,
+        true,
+        true,
+        true,
+        true,
+        true
+    }};
+
+    // ---------------- LPF ----------------
+
+    double twist_lpf_alpha_{1.0};
+
+    // ---------------- command history ----------------
+
+    Eigen::Matrix<double, 6, 1>
+        previous_twist_cmd_G_{
+            Eigen::Matrix<double, 6, 1>::Zero()
+        };
+
+    bool has_previous_twist_{false};
+
+    // ---------------- last output ----------------
+
+    Eigen::Vector3d last_position_error_G_{
+        Eigen::Vector3d::Zero()
+    };
+
+    Eigen::Vector3d last_rotation_error_G_{
+        Eigen::Vector3d::Zero()
+    };
+
+    Eigen::Matrix<double, 6, 1>
+        last_twist_cmd_G_{
+            Eigen::Matrix<double, 6, 1>::Zero()
+        };
+
+    bool last_reached_{false};
+
+    ComputeStatus last_compute_status_{
+        ComputeStatus::TARGET_NOT_SET
+    };
+};
+
+} // namespace cmvr
+
+#endif // CMVR_PBVS_CONTROLLER_H

cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp

@@ -0,0 +1,774 @@
+#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <Eigen/Geometry>
+#include <Eigen/SVD>
+
+namespace cmvr {
+
+PbvsController::PbvsController()
+{
+    resetTwistCommandState();
+}
+
+bool PbvsController::setTargetPose(
+    const Eigen::Matrix4d& T_G_P_des)
+{
+    if (!isValidTransform(T_G_P_des)) {
+        target_valid_ = false;
+        last_compute_status_ =
+            ComputeStatus::INVALID_TARGET;
+        return false;
+    }
+
+    T_G_P_des_ = T_G_P_des;
+
+    //
+    // 视觉位姿可能有微小数值误差。
+    // 强制把 rotation 投影到 SO(3)。
+    //
+    T_G_P_des_.block<3, 3>(0, 0) =
+        projectToSO3(
+            T_G_P_des.block<3, 3>(0, 0));
+
+    target_valid_ = true;
+    last_compute_status_ =
+        ComputeStatus::OK;
+
+    return true;
+}
+
+bool PbvsController::setTargetPose(
+    const Eigen::Vector3d& position_G,
+    const Eigen::Matrix3d& rotation_G_P)
+{
+    if (!position_G.allFinite() ||
+        !rotation_G_P.allFinite()) {
+
+        target_valid_ = false;
+        last_compute_status_ =
+            ComputeStatus::INVALID_TARGET;
+
+        return false;
+    }
+
+    Eigen::Matrix4d T =
+        Eigen::Matrix4d::Identity();
+
+    T.block<3, 3>(0, 0) =
+        projectToSO3(rotation_G_P);
+
+    T.block<3, 1>(0, 3) =
+        position_G;
+
+    return setTargetPose(T);
+}
+
+bool PbvsController::compute(
+    const Eigen::Matrix4d& T_G_P_cur,
+    const double dt,
+    Output& output)
+{
+    output = Output{};
+
+    last_reached_ = false;
+    last_position_error_G_.setZero();
+    last_rotation_error_G_.setZero();
+    last_twist_cmd_G_.setZero();
+
+    if (!target_valid_) {
+        last_compute_status_ =
+            ComputeStatus::TARGET_NOT_SET;
+
+        return false;
+    }
+
+    if (!std::isfinite(dt) ||
+        dt <= 0.0) {
+
+        last_compute_status_ =
+            ComputeStatus::INVALID_DT;
+
+        return false;
+    }
+
+    if (!isValidTransform(T_G_P_cur)) {
+        last_compute_status_ =
+            ComputeStatus::INVALID_CURRENT_POSE;
+
+        return false;
+    }
+
+    // =====================================================
+    // 1. Current pose
+    // =====================================================
+
+    const Eigen::Vector3d p_cur_G =
+        T_G_P_cur.block<3, 1>(0, 3);
+
+    const Eigen::Matrix3d R_cur_G_P =
+        projectToSO3(
+            T_G_P_cur.block<3, 3>(0, 0));
+
+    // =====================================================
+    // 2. Desired pose
+    // =====================================================
+
+    const Eigen::Vector3d p_des_G =
+        T_G_P_des_.block<3, 1>(0, 3);
+
+    const Eigen::Matrix3d R_des_G_P =
+        T_G_P_des_.block<3, 3>(0, 0);
+
+    // =====================================================
+    // 3. Position error
+    //
+    // e_p = p_des - p_cur
+    //
+    // expressed in G frame.
+    // =====================================================
+
+    Eigen::Vector3d e_pos_G =
+        p_des_G -
+        p_cur_G;
+
+    // =====================================================
+    // 4. Rotation error
+    //
+    // R_err =
+    //     R_des * R_cur^T
+    //
+    // e_R =
+    //     Log(R_err)^vee
+    //
+    // e_R is also expressed in G frame.
+    // =====================================================
+
+    Eigen::Vector3d e_rot_G =
+        rotationError(
+            R_des_G_P,
+            R_cur_G_P);
+
+    if (!e_pos_G.allFinite() ||
+        !e_rot_G.allFinite()) {
+
+        last_compute_status_ =
+            ComputeStatus::INVALID_INPUT;
+
+        return false;
+    }
+
+    // =====================================================
+    // 5. Disabled axes
+    // =====================================================
+
+    for (int i = 0; i < 3; ++i) {
+        if (!axis_enabled_[i]) {
+            e_pos_G[i] = 0.0;
+        }
+
+        if (!axis_enabled_[i + 3]) {
+            e_rot_G[i] = 0.0;
+        }
+    }
+
+    last_position_error_G_ =
+        e_pos_G;
+
+    last_rotation_error_G_ =
+        e_rot_G;
+
+    output.position_error_G =
+        e_pos_G;
+
+    output.rotation_error_G =
+        e_rot_G;
+
+    // =====================================================
+    // 6. Reached check
+    // =====================================================
+
+    bool position_reached = true;
+    bool orientation_reached = true;
+
+    for (int i = 0; i < 3; ++i) {
+
+        if (axis_enabled_[i] &&
+            std::abs(e_pos_G[i]) >
+                tolerance6_[i]) {
+
+            position_reached = false;
+        }
+
+        if (axis_enabled_[i + 3] &&
+            std::abs(e_rot_G[i]) >
+                tolerance6_[i + 3]) {
+
+            orientation_reached = false;
+        }
+    }
+
+    const bool reached =
+        position_reached &&
+        orientation_reached;
+
+    output.position_reached =
+        position_reached;
+
+    output.orientation_reached =
+        orientation_reached;
+
+    output.reached =
+        reached;
+
+    last_reached_ =
+        reached;
+
+    // =====================================================
+    // 7. PBVS P control
+    //
+    // v = Kp * e_pos
+    //
+    // w = Kr * e_rot
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_raw_G =
+            Eigen::Matrix<double, 6, 1>::Zero();
+
+    twist_raw_G.head<3>() =
+        kp_position_.cwiseProduct(
+            e_pos_G);
+
+    twist_raw_G.tail<3>() =
+        kp_rotation_.cwiseProduct(
+            e_rot_G);
+
+    // =====================================================
+    // 8. Dead zone
+    //
+    // 某个轴已经进入误差阈值，则这个轴不再主动运动。
+    // =====================================================
+
+    for (int i = 0; i < 6; ++i) {
+
+        if (!axis_enabled_[i]) {
+            twist_raw_G[i] = 0.0;
+            continue;
+        }
+
+        const double error_value =
+            i < 3
+                ? e_pos_G[i]
+                : e_rot_G[i - 3];
+
+        if (std::abs(error_value) <=
+            tolerance6_[i]) {
+
+            twist_raw_G[i] = 0.0;
+        }
+    }
+
+    output.raw_linear_velocity_G =
+        twist_raw_G.head<3>();
+
+    output.raw_angular_velocity_G =
+        twist_raw_G.tail<3>();
+
+    // =====================================================
+    // 9. Velocity limits
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_vel_limited =
+            twist_raw_G;
+
+    for (int i = 0; i < 6; ++i) {
+
+        const double vmax =
+            vmax6_[i];
+
+        if (!std::isfinite(vmax) ||
+            vmax <= 0.0) {
+
+            twist_vel_limited[i] = 0.0;
+            continue;
+        }
+
+        twist_vel_limited[i] =
+            clampValue(
+                twist_vel_limited[i],
+                -vmax,
+                vmax);
+    }
+
+    // =====================================================
+    // 10. Reached:
+    //
+    // 进入完整目标阈值后直接输出 0。
+    //
+    // 上层可以随后调用 RobotArm::stopL()。
+    // =====================================================
+
+    if (reached) {
+
+        previous_twist_cmd_G_.setZero();
+        has_previous_twist_ = true;
+
+        last_twist_cmd_G_.setZero();
+
+        output.linear_velocity_G.setZero();
+        output.angular_velocity_G.setZero();
+
+        output.twist_G =
+            device::CartesianVelocity{};
+
+        output.valid = true;
+
+        last_compute_status_ =
+            ComputeStatus::OK;
+
+        return true;
+    }
+
+    // =====================================================
+    // 11. Acceleration limits
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_acc_limited;
+
+    if (!has_previous_twist_) {
+
+        previous_twist_cmd_G_.setZero();
+        has_previous_twist_ = true;
+    }
+
+    twist_acc_limited =
+        previous_twist_cmd_G_;
+
+    for (int i = 0; i < 6; ++i) {
+
+        if (!axis_enabled_[i]) {
+            twist_acc_limited[i] = 0.0;
+            continue;
+        }
+
+        const double amax =
+            amax6_[i];
+
+        // <=0：不做加速度限制
+        if (!std::isfinite(amax) ||
+            amax <= 0.0) {
+
+            twist_acc_limited[i] =
+                twist_vel_limited[i];
+
+            continue;
+        }
+
+        const double dv_max =
+            amax * dt;
+
+        const double dv_des =
+            twist_vel_limited[i]
+            -
+            previous_twist_cmd_G_[i];
+
+        const double dv =
+            clampValue(
+                dv_des,
+                -dv_max,
+                dv_max);
+
+        twist_acc_limited[i] =
+            previous_twist_cmd_G_[i]
+            +
+            dv;
+    }
+
+    // =====================================================
+    // 12. First-order LPF
+    // =====================================================
+
+    Eigen::Matrix<double, 6, 1>
+        twist_filtered =
+            twist_acc_limited;
+
+    const double alpha =
+        std::clamp(
+            twist_lpf_alpha_,
+            0.0,
+            1.0);
+
+    if (alpha > 0.0 &&
+        alpha < 1.0) {
+
+        twist_filtered =
+            alpha *
+            twist_acc_limited
+            +
+            (1.0 - alpha) *
+            previous_twist_cmd_G_;
+    }
+
+    // =====================================================
+    // 13. Make sure disabled axis is zero
+    // =====================================================
+
+    for (int i = 0; i < 6; ++i) {
+        if (!axis_enabled_[i]) {
+            twist_filtered[i] = 0.0;
+        }
+    }
+
+    // =====================================================
+    // 14. Save state
+    // =====================================================
+
+    previous_twist_cmd_G_ =
+        twist_filtered;
+
+    last_twist_cmd_G_ =
+        twist_filtered;
+
+    // =====================================================
+    // 15. Output
+    // =====================================================
+
+    output.linear_velocity_G =
+        twist_filtered.head<3>();
+
+    output.angular_velocity_G =
+        twist_filtered.tail<3>();
+
+    output.twist_G =
+        toCartesianVelocity(
+            twist_filtered);
+
+    output.valid = true;
+
+    last_compute_status_ =
+        ComputeStatus::OK;
+
+    return true;
+}
+
+bool PbvsController::compute(
+    const Eigen::Matrix4d& T_G_P_cur,
+    const double dt,
+    device::CartesianVelocity& twist_G_out)
+{
+    Output output;
+
+    if (!compute(
+            T_G_P_cur,
+            dt,
+            output)) {
+
+        twist_G_out =
+            device::CartesianVelocity{};
+
+        return false;
+    }
+
+    twist_G_out =
+        output.twist_G;
+
+    return true;
+}
+
+void PbvsController::setPositionGain(
+    const Eigen::Vector3d& kp)
+{
+    for (int i = 0; i < 3; ++i) {
+        if (std::isfinite(kp[i]) &&
+            kp[i] >= 0.0) {
+
+            kp_position_[i] =
+                kp[i];
+        }
+    }
+}
+
+void PbvsController::setRotationGain(
+    const Eigen::Vector3d& kr)
+{
+    for (int i = 0; i < 3; ++i) {
+        if (std::isfinite(kr[i]) &&
+            kr[i] >= 0.0) {
+
+            kp_rotation_[i] =
+                kr[i];
+        }
+    }
+}
+
+void PbvsController::setVelocityLimit6(
+    const std::array<double, 6>& vmax6)
+{
+    for (int i = 0; i < 6; ++i) {
+
+        if (std::isfinite(vmax6[i]) &&
+            vmax6[i] >= 0.0) {
+
+            vmax6_[i] =
+                vmax6[i];
+        }
+    }
+}
+
+void PbvsController::setAccelerationLimit6(
+    const std::array<double, 6>& amax6)
+{
+    for (int i = 0; i < 6; ++i) {
+
+        if (std::isfinite(amax6[i])) {
+            amax6_[i] =
+                amax6[i];
+        }
+    }
+}
+
+void PbvsController::setTolerance6(
+    const std::array<double, 6>& tolerance6)
+{
+    for (int i = 0; i < 6; ++i) {
+
+        if (std::isfinite(tolerance6[i]) &&
+            tolerance6[i] >= 0.0) {
+
+            tolerance6_[i] =
+                tolerance6[i];
+        }
+    }
+}
+
+void PbvsController::setTwistFilterAlpha(
+    const double alpha)
+{
+    if (!std::isfinite(alpha)) {
+        return;
+    }
+
+    twist_lpf_alpha_ =
+        std::clamp(
+            alpha,
+            0.0,
+            1.0);
+}
+
+void PbvsController::setAxisEnabled(
+    const std::array<bool, 6>& enabled)
+{
+    axis_enabled_ =
+        enabled;
+}
+
+void PbvsController::resetTwistCommandState()
+{
+    previous_twist_cmd_G_.setZero();
+    last_twist_cmd_G_.setZero();
+
+    has_previous_twist_ = false;
+}
+
+void PbvsController::reset()
+{
+    T_G_P_des_.setIdentity();
+
+    target_valid_ = false;
+
+    last_position_error_G_.setZero();
+    last_rotation_error_G_.setZero();
+
+    last_reached_ = false;
+
+    resetTwistCommandState();
+
+    last_compute_status_ =
+        ComputeStatus::TARGET_NOT_SET;
+}
+
+const char* PbvsController::statusToString(
+    const ComputeStatus status)
+{
+    switch (status) {
+
+        case ComputeStatus::OK:
+            return "ok";
+
+        case ComputeStatus::TARGET_NOT_SET:
+            return "target_not_set";
+
+        case ComputeStatus::INVALID_INPUT:
+            return "invalid_input";
+
+        case ComputeStatus::INVALID_DT:
+            return "invalid_dt";
+
+        case ComputeStatus::INVALID_TARGET:
+            return "invalid_target";
+
+        case ComputeStatus::INVALID_CURRENT_POSE:
+            return "invalid_current_pose";
+
+        default:
+            return "unknown";
+    }
+}
+
+bool PbvsController::isFiniteTransform(
+    const Eigen::Matrix4d& T)
+{
+    return T.allFinite();
+}
+
+bool PbvsController::hasValidBottomRow(
+    const Eigen::Matrix4d& T,
+    const double tolerance)
+{
+    return
+        std::abs(T(3, 0)) <= tolerance &&
+        std::abs(T(3, 1)) <= tolerance &&
+        std::abs(T(3, 2)) <= tolerance &&
+        std::abs(T(3, 3) - 1.0) <= tolerance;
+}
+
+bool PbvsController::isValidTransform(
+    const Eigen::Matrix4d& T)
+{
+    if (!isFiniteTransform(T)) {
+        return false;
+    }
+
+    if (!hasValidBottomRow(T)) {
+        return false;
+    }
+
+    return true;
+}
+
+Eigen::Matrix3d PbvsController::projectToSO3(
+    const Eigen::Matrix3d& R)
+{
+    if (!R.allFinite()) {
+        return Eigen::Matrix3d::Identity();
+    }
+
+    Eigen::JacobiSVD<Eigen::Matrix3d> svd(
+        R,
+        Eigen::ComputeFullU |
+        Eigen::ComputeFullV);
+
+    Eigen::Matrix3d U =
+        svd.matrixU();
+
+    const Eigen::Matrix3d V =
+        svd.matrixV();
+
+    Eigen::Matrix3d R_projected =
+        U * V.transpose();
+
+    //
+    // 确保 det = +1，而不是 reflection。
+    //
+    if (R_projected.determinant() < 0.0) {
+
+        U.col(2) *= -1.0;
+
+        R_projected =
+            U * V.transpose();
+    }
+
+    return R_projected;
+}
+
+Eigen::Vector3d PbvsController::rotationError(
+    const Eigen::Matrix3d& R_des,
+    const Eigen::Matrix3d& R_cur)
+{
+    const Eigen::Matrix3d R_d =
+        projectToSO3(R_des);
+
+    const Eigen::Matrix3d R_c =
+        projectToSO3(R_cur);
+
+    //
+    // 空间旋转误差，在 G frame 表达。
+    //
+    // 当前为 I，目标绕 +Z 旋转 theta：
+    //
+    // R_err = R_des
+    //
+    // 得到 +theta Z，
+    // 因此角速度方向正确。
+    //
+    const Eigen::Matrix3d R_err =
+        R_d *
+        R_c.transpose();
+
+    Eigen::AngleAxisd aa(
+        R_err);
+
+    const double angle =
+        aa.angle();
+
+    if (!std::isfinite(angle) ||
+        std::abs(angle) <= 1e-12) {
+
+        return Eigen::Vector3d::Zero();
+    }
+
+    const Eigen::Vector3d axis =
+        aa.axis();
+
+    if (!axis.allFinite()) {
+        return Eigen::Vector3d::Zero();
+    }
+
+    return axis * angle;
+}
+
+double PbvsController::clampValue(
+    const double value,
+    const double lower,
+    const double upper)
+{
+    return std::max(
+        lower,
+        std::min(
+            value,
+            upper));
+}
+
+device::CartesianVelocity
+PbvsController::toCartesianVelocity(
+    const Eigen::Matrix<double, 6, 1>& twist)
+{
+    device::CartesianVelocity velocity;
+
+    velocity.vx =
+        twist[0];
+
+    velocity.vy =
+        twist[1];
+
+    velocity.vz =
+        twist[2];
+
+    velocity.wx =
+        twist[3];
+
+    velocity.wy =
+        twist[4];
+
+    velocity.wz =
+        twist[5];
+
+    return velocity;
+}
+
+} // namespace cmvr

cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp

@@ -0,0 +1,61 @@
+#include "gtest/gtest.h"
+
+#include <array>
+
+#include <Eigen/Geometry>
+
+#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
+
+namespace {
+
+TEST(PbvsControllerTest, AppliesConfigurationSetters) {
+    cmvr::PbvsController controller;
+
+    const Eigen::Vector3d position_gain(1.0, 2.0, 3.0);
+    const Eigen::Vector3d rotation_gain(4.0, 5.0, 6.0);
+    const std::array<double, 6> vmax{{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}};
+    const std::array<double, 6> amax{{1.0, 2.0, 3.0, 4.0, 5.0, 6.0}};
+    const std::array<double, 6> tolerance{{0.001, 0.002, 0.003, 0.01, 0.02, 0.03}};
+
+    controller.setPositionGain(position_gain);
+    controller.setRotationGain(rotation_gain);
+    controller.setVelocityLimit6(vmax);
+    controller.setAccelerationLimit6(amax);
+    controller.setTolerance6(tolerance);
+    controller.setTwistFilterAlpha(0.75);
+
+    EXPECT_TRUE(controller.positionGain().isApprox(position_gain));
+    EXPECT_TRUE(controller.rotationGain().isApprox(rotation_gain));
+    EXPECT_EQ(controller.velocityLimit6(), vmax);
+    EXPECT_EQ(controller.accelerationLimit6(), amax);
+    EXPECT_EQ(controller.tolerance6(), tolerance);
+    EXPECT_DOUBLE_EQ(controller.twistFilterAlpha(), 0.75);
+}
+
+TEST(PbvsControllerTest, CommandsTowardPositionAndRotationError) {
+    cmvr::PbvsController controller;
+    controller.setPositionGain(Eigen::Vector3d::Ones());
+    controller.setRotationGain(Eigen::Vector3d::Ones());
+    controller.setVelocityLimit6({{1.0, 1.0, 1.0, 1.0, 1.0, 1.0}});
+    controller.setAccelerationLimit6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
+    controller.setTolerance6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
+    controller.setTwistFilterAlpha(1.0);
+
+    Eigen::Matrix4d target = Eigen::Matrix4d::Identity();
+    target.block<3, 3>(0, 0) =
+        Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ()).toRotationMatrix();
+    target.block<3, 1>(0, 3) = Eigen::Vector3d(0.1, -0.2, 0.3);
+    ASSERT_TRUE(controller.setTargetPose(target));
+
+    cmvr::PbvsController::Output output;
+    ASSERT_TRUE(controller.compute(Eigen::Matrix4d::Identity(), 0.01, output));
+    ASSERT_TRUE(output.valid);
+    EXPECT_GT(output.linear_velocity_G.x(), 0.0);
+    EXPECT_LT(output.linear_velocity_G.y(), 0.0);
+    EXPECT_GT(output.linear_velocity_G.z(), 0.0);
+    EXPECT_NEAR(output.angular_velocity_G.x(), 0.0, 1e-12);
+    EXPECT_NEAR(output.angular_velocity_G.y(), 0.0, 1e-12);
+    EXPECT_GT(output.angular_velocity_G.z(), 0.0);
+}
+
+} // namespace

cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt

@@ -26,3 +26,14 @@ target_link_libraries(ik_solver PUBLIC
 add_library(cmvr_es::ik_solver ALIAS ik_solver)
 
 install(TARGETS ik_solver LIBRARY DESTINATION lib)
+
+add_executable(pinocchio_qp_ik_solver_test
+        pinocchio/src/pinocchio_qp_ik_solver_test.cpp
+)
+
+target_link_libraries(pinocchio_qp_ik_solver_test PRIVATE
+        cmvr_es::ik_solver
+        cmvr_es::proto
+        gtest
+        gtest_main
+)

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h

@@ -45,6 +45,11 @@ public:
                            std::vector<double>& qdot_out,
                            double qdot_abs_max = std::numeric_limits<double>::infinity()) const override;
 
+    // Projects a secondary joint velocity into the Cartesian task null space.
+    static Eigen::VectorXd projectJointLimitAvoidanceToNullspace(
+        const Eigen::MatrixXd& jacobian_base,
+        const Eigen::VectorXd& qdot_avoid);
+
     /// 如你有更严格的速度 / 加速度限位，可以覆盖默认值
     void setVelocityLimits(const Eigen::VectorXd &qd_max);
     void setAccelerationLimits(const Eigen::VectorXd &qdd_max);

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp

@@ -164,7 +164,9 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
 
     const double margin_ratio = positiveOr(config.margin_ratio(), 0.08);
     const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02);
-    Eigen::VectorXd limited = qdot;
+    // Apply one common scale factor instead of changing individual joints.
+    // Per-joint scaling changes J*qdot and can disturb the Cartesian task.
+    double scale = 1.0;
     for (Eigen::Index i = 0; i < q_chain.size(); ++i) {
         const double lower = joint_pos_lower_limits_[i];
         const double upper = joint_pos_upper_limits_[i];
@@ -174,21 +176,15 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
 
         const double span = upper - lower;
         const double margin = std::max(min_margin_rad, margin_ratio * span);
-        if (limited[i] < 0.0 && q_chain[i] < lower + margin) {
+        if (qdot[i] < 0.0 && q_chain[i] < lower + margin) {
             const double ratio = std::clamp((q_chain[i] - lower) / margin, 0.0, 1.0);
-            limited[i] *= ratio;
+            scale = std::min(scale, ratio);
-            if (q_chain[i] <= lower) {
+        } else if (qdot[i] > 0.0 && q_chain[i] > upper - margin) {
-                limited[i] = std::max(0.0, limited[i]);
-            }
-        } else if (limited[i] > 0.0 && q_chain[i] > upper - margin) {
             const double ratio = std::clamp((upper - q_chain[i]) / margin, 0.0, 1.0);
-            limited[i] *= ratio;
+            scale = std::min(scale, ratio);
-            if (q_chain[i] >= upper) {
-                limited[i] = std::min(0.0, limited[i]);
         }
     }
-    }
+    return scale * qdot;
-    return limited;
 }
 
 void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) {

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp

@@ -13,11 +13,66 @@
 #include <pinocchio/spatial/explog.hpp>
 
 #include <algorithm>   // std::clamp, std::max, std::min
+#include <atomic>
 #include <cmath>       // std::sqrt
+#include <cstdint>
 #include <limits>
+#include <sstream>
 #include <unordered_map>
+#include <Eigen/SVD>
 
 namespace cmvr {
+    namespace {
+
+    Eigen::MatrixXd moorePenrosePseudoInverse(const Eigen::MatrixXd& matrix)
+    {
+        if (matrix.rows() == 0 || matrix.cols() == 0 || !matrix.allFinite()) {
+            return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
+        }
+
+        Eigen::JacobiSVD<Eigen::MatrixXd> svd(
+            matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
+        if (svd.info() != Eigen::Success) {
+            return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
+        }
+
+        const Eigen::VectorXd singular_values = svd.singularValues();
+        const double max_singular = singular_values.size() > 0
+                                        ? singular_values.maxCoeff()
+                                        : 0.0;
+        const double tolerance =
+            std::numeric_limits<double>::epsilon() *
+            static_cast<double>(std::max(matrix.rows(), matrix.cols())) *
+            std::max(1.0, max_singular);
+        Eigen::VectorXd inverse_singular = singular_values;
+        for (Eigen::Index i = 0; i < inverse_singular.size(); ++i) {
+            inverse_singular[i] = singular_values[i] > tolerance
+                                      ? 1.0 / singular_values[i]
+                                      : 0.0;
+        }
+
+        const Eigen::Index rank_dimension = singular_values.size();
+        return svd.matrixV().leftCols(rank_dimension) *
+               inverse_singular.asDiagonal() *
+               svd.matrixU().leftCols(rank_dimension).transpose();
+    }
+
+    std::string vectorToString(const Eigen::VectorXd& value)
+    {
+        std::ostringstream stream;
+        stream << '[';
+        for (Eigen::Index i = 0; i < value.size(); ++i) {
+            if (i > 0) {
+                stream << ' ';
+            }
+            stream << value[i];
+        }
+        stream << ']';
+        return stream.str();
+    }
+
+    } // namespace
+
     using Eigen::Matrix4d;
     using Eigen::VectorXd;
     using Eigen::MatrixXd;
@@ -208,6 +263,24 @@ namespace cmvr {
         }
     }
 
+    Eigen::VectorXd PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace(
+        const Eigen::MatrixXd& jacobian_base,
+        const Eigen::VectorXd& qdot_avoid)
+    {
+        if (jacobian_base.cols() != qdot_avoid.size() ||
+            jacobian_base.rows() == 0 || jacobian_base.cols() == 0 ||
+            !jacobian_base.allFinite() || !qdot_avoid.allFinite()) {
+            return Eigen::VectorXd::Zero(qdot_avoid.size());
+        }
+
+        const Eigen::MatrixXd jacobian_pinv =
+            moorePenrosePseudoInverse(jacobian_base);
+        const Eigen::MatrixXd nullspace =
+            Eigen::MatrixXd::Identity(jacobian_base.cols(), jacobian_base.cols()) -
+            jacobian_pinv * jacobian_base;
+        return nullspace * qdot_avoid;
+    }
+
     bool PinocchioQpIKSolver::ik(const Matrix4d &target_pose,
                                  std::vector<double> &joints_angle,
                                  bool is_tcp) {
@@ -389,7 +462,7 @@ namespace cmvr {
         const int dof = chain_v_dof_;
         const auto& avoidance = jointLimitPolicy().avoidance();
         const bool use_joint_limit_avoidance =
-            !jointLimitsDisabled() && avoidance.enable() && avoidance.weight() > 0.0;
+            !jointLimitsDisabled() && avoidance.enable() && avoidance.gain() > 0.0;
         const int avoidance_rows = use_joint_limit_avoidance ? dof : 0;
 
         MatrixXd cost(6 + dof + avoidance_rows, dof);
@@ -406,7 +479,7 @@ namespace cmvr {
         VectorXd upper(dof);
         const Eigen::Map<const VectorXd> q_chain(q_chain_std.data(), dof);
         if (use_joint_limit_avoidance) {
-            const VectorXd qdot_avoid =
+            const VectorXd qdot_avoid_raw =
                 cmvr::kinematics::computeJointLimitAvoidanceVelocity(
                     q_chain,
                     joint_pos_lower_limits_,
@@ -415,10 +488,32 @@ namespace cmvr {
                     positiveOr(avoidance.gain(), 0.2),
                     positiveOr(avoidance.margin_ratio(), 0.15),
                     positiveOr(avoidance.max_push(), 0.25));
-            const double sqrt_weight = std::sqrt(positiveOr(avoidance.weight(), 0.05));
+            const Eigen::MatrixXd jacobian_pinv =
+                moorePenrosePseudoInverse(jacobian_base);
+            const bool jacobian_pinv_valid =
+                jacobian_pinv.rows() == dof && jacobian_pinv.cols() == 6 &&
+                jacobian_pinv.allFinite();
+            Eigen::MatrixXd nullspace = MatrixXd::Zero(dof, dof);
+            if (jacobian_pinv_valid) {
+                nullspace = MatrixXd::Identity(dof, dof) -
+                            jacobian_pinv * jacobian_base;
+            }
+            const VectorXd qdot_avoid_null = nullspace * qdot_avoid_raw;
             cost.middleRows(6 + dof, dof) =
-                sqrt_weight * MatrixXd::Identity(dof, dof);
+                nullspace;
-            target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid;
+            target.segment(6 + dof, dof) = qdot_avoid_null;
+
+            static std::atomic<std::uint64_t> avoidance_debug_counter{0};
+            const auto debug_index =
+                avoidance_debug_counter.fetch_add(1, std::memory_order_relaxed);
+            if (debug_index % 1000 == 0) {
+                CMVR_LOG(DEBUG)
+                    << "[PinocchioQpIKSolver][JOINT_LIMIT_AVOIDANCE]"
+                    << " qdot_avoid_raw=" << vectorToString(qdot_avoid_raw)
+                    << " qdot_avoid_null=" << vectorToString(qdot_avoid_null)
+                    << " norm(J*qdot_avoid_null)="
+                    << (jacobian_base * qdot_avoid_null).norm();
+            }
         }
         for (int i = 0; i < dof; ++i) {
             double limit = std::numeric_limits<double>::infinity();
@@ -452,6 +547,35 @@ namespace cmvr {
                     }
                 }
             }
+
+            const auto& soft_limit = jointLimitPolicy().soft_limit();
+            if (!jointLimitsDisabled() && soft_limit.enable() &&
+                joint_pos_lower_limits_.size() == dof &&
+                joint_pos_upper_limits_.size() == dof) {
+                const double q_min = joint_pos_lower_limits_[i];
+                const double q_max = joint_pos_upper_limits_[i];
+                if (std::isfinite(q_min) && std::isfinite(q_max) && q_max > q_min) {
+                    const double span = q_max - q_min;
+                    const double margin = std::max(
+                        positiveOr(soft_limit.min_margin_rad(), 0.02),
+                        positiveOr(soft_limit.margin_ratio(), 0.08) * span);
+                    if (q_chain[i] < q_min + margin) {
+                        const double ratio = std::clamp(
+                            (q_chain[i] - q_min) / margin, 0.0, 1.0);
+                        lower[i] = std::max(lower[i], -limit * ratio);
+                        if (q_chain[i] <= q_min) {
+                            lower[i] = std::max(0.0, lower[i]);
+                        }
+                    } else if (q_chain[i] > q_max - margin) {
+                        const double ratio = std::clamp(
+                            (q_max - q_chain[i]) / margin, 0.0, 1.0);
+                        upper[i] = std::min(upper[i], limit * ratio);
+                        if (q_chain[i] >= q_max) {
+                            upper[i] = std::min(0.0, upper[i]);
+                        }
+                    }
+                }
+            }
         }
 
         QPSolver solver;
@@ -475,7 +599,6 @@ namespace cmvr {
         if (qdot.size() != dof) {
             return false;
         }
-        qdot = applyJointSoftLimitsToVelocity(q_chain, qdot);
         qdot_out.assign(qdot.data(), qdot.data() + qdot.size());
         return true;
     }

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp

@@ -0,0 +1,104 @@
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h"
+
+#include <filesystem>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "common/io/proto_file_io.h"
+#include "cmvr/config/arm_config/arm_config.pb.h"
+
+namespace cmvr {
+namespace {
+
+std::filesystem::path findProjectRoot()
+{
+    std::filesystem::path current = std::filesystem::current_path();
+    while (!current.empty()) {
+        if (std::filesystem::exists(
+                current / "model/xiaoyan_description/dual_arm.urdf")) {
+            return current;
+        }
+        const auto parent = current.parent_path();
+        if (parent == current) {
+            break;
+        }
+        current = parent;
+    }
+    return {};
+}
+
+config::PinocchioQpIKConfig loadQpConfig(const std::filesystem::path& root)
+{
+    config::ArmRootConfig root_config;
+    const auto config_path =
+        root / "cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt";
+    EXPECT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(config_path.string(), &root_config));
+    EXPECT_GT(root_config.arm().robot_arms_size(), 0);
+    auto solver_config =
+        root_config.arm().robot_arms(0).kinematics().pinocchio_qp_ik_solver();
+    solver_config.set_urdf_path(
+        (root / "model/xiaoyan_description/dual_arm.urdf").string());
+    return solver_config;
+}
+
+TEST(PinocchioQpIKSolverTest, JointLimitAvoidanceIsInCartesianNullspace)
+{
+    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7);
+    jacobian.leftCols(6).setIdentity();
+    jacobian.col(6) << 0.3, -0.2, 0.4, -0.1, 0.25, 0.15;
+    Eigen::VectorXd qdot_avoid(7);
+    qdot_avoid << 0.4, -0.3, 0.2, 0.1, -0.5, 0.6, -0.7;
+
+    const Eigen::VectorXd qdot_null =
+        PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace(
+            jacobian, qdot_avoid);
+
+    ASSERT_EQ(qdot_null.size(), 7);
+    EXPECT_LT((jacobian * qdot_null).norm(), 1e-12);
+    EXPECT_GT(qdot_null.norm(), 0.0);
+}
+
+TEST(PinocchioQpIKSolverTest, AvoidanceDoesNotDisturbReachableCartesianTwist)
+{
+    const auto root = findProjectRoot();
+    ASSERT_FALSE(root.empty());
+
+    auto disabled_config = loadQpConfig(root);
+    disabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(false);
+    auto enabled_config = disabled_config;
+    enabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(true);
+
+    PinocchioQpIKSolver solver_disabled(disabled_config);
+    PinocchioQpIKSolver solver_enabled(enabled_config);
+    ASSERT_TRUE(solver_disabled.init());
+    ASSERT_TRUE(solver_enabled.init());
+
+    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7);
+    jacobian.leftCols(6).setIdentity();
+    Eigen::Matrix<double, 6, 1> target_twist;
+    target_twist << 0.15, -0.10, 0.08, 0.05, -0.04, 0.03;
+
+    // The last joint is inside its configured soft-limit margin, while the
+    // first six columns fully span the Cartesian task.
+    const std::vector<double> q_chain = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.56};
+    std::vector<double> qdot_disabled;
+    std::vector<double> qdot_enabled;
+    ASSERT_TRUE(solver_disabled.solveVelocityBase(
+        jacobian, target_twist, q_chain, qdot_disabled, 10.0));
+    ASSERT_TRUE(solver_enabled.solveVelocityBase(
+        jacobian, target_twist, q_chain, qdot_enabled, 10.0));
+
+    const Eigen::Map<const Eigen::VectorXd> qdot_disabled_eigen(
+        qdot_disabled.data(), static_cast<Eigen::Index>(qdot_disabled.size()));
+    const Eigen::Map<const Eigen::VectorXd> qdot_enabled_eigen(
+        qdot_enabled.data(), static_cast<Eigen::Index>(qdot_enabled.size()));
+    const Eigen::VectorXd achieved_disabled = jacobian * qdot_disabled_eigen;
+    const Eigen::VectorXd achieved_enabled = jacobian * qdot_enabled_eigen;
+
+    EXPECT_LT((achieved_enabled - achieved_disabled).norm(), 1e-6);
+    EXPECT_LT((achieved_enabled - target_twist).norm(), 5e-5);
+}
+
+} // namespace
+} // namespace cmvr

cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt

@@ -14,3 +14,14 @@ target_link_libraries(arm_motion
 add_library(cmvr_es::arm_motion ALIAS arm_motion)
 add_library(cmvr_es::algorithms::arm_motion ALIAS arm_motion)
 install(TARGETS arm_motion LIBRARY DESTINATION lib)
+
+add_executable(toppra_joint_motion_planner_test
+        joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp
+)
+
+target_link_libraries(toppra_joint_motion_planner_test
+        PRIVATE
+        cmvr_es::algorithms::arm_motion
+        gtest
+        gtest_main
+)

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h

@@ -99,6 +99,11 @@ private:
     bool speedl_line_check_active_{false};
     bool speedl_line_deviation_warned_{false};
     bool speedl_line_direction_warned_{false};
+
+    // speedL 是否已经进入停止阶段。
+    // 停止阶段不要每 1 ms 用 measured twist 重新点燃 Cartesian planner。
+    bool speedl_stop_active_{false};
+
     bool speedl_configured_{false};
 };
 

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp

@@ -127,6 +127,7 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne
     speedl_line_check_active_ = false;
     speedl_line_deviation_warned_ = false;
     speedl_line_direction_warned_ = false;
+    speedl_stop_active_ = false;
     speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0);
     speedl_configured_ = true;
     return true;
@@ -894,15 +895,43 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         return false;
     }
 
-    const Eigen::Matrix<double, 6, 1> target_twist = common::math::velocityToVector(target_velocity);
+    const Eigen::Matrix<double, 6, 1> target_twist =
-    const bool is_stop_command = target_twist.squaredNorm() <= 1e-12;
+        common::math::velocityToVector(target_velocity);
+
+    const bool is_stop_command =
+        target_twist.squaredNorm() <= 1e-12;
+
     if (is_stop_command) {
-        twist_limiter_.synchronize(measured_twist_base, dt, true);
+        if (!speedl_stop_active_) {
-    } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) {
+            // 只在 stop 边沿执行一次。
-        twist_limiter_.initialize(Eigen::Matrix<double, 6, 1>::Zero());
+            //
+            // 非常重要：
+            // 不再调用
+            // twist_limiter_.synchronize(measured_twist_base, dt, true);
+            //
+            // 停止应当从“上一拍已经发送出去的 command twist”
+            // 连续规划到 0，而不是每 1 ms 被 measured twist 重新点燃。
+            speedl_stop_active_ = true;
+
+            twist_limiter_.stop();
         }
-    twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame));
+    } else {
-    speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool);
+        // 收到新的非零 speedL，退出停止状态。
+        speedl_stop_active_ = false;
+
+        // 从静止开始一个新的 speedL command。
+        if (speedl_command_twist_base_.squaredNorm() <= 1e-12) {
+            twist_limiter_.initialize(
+                Eigen::Matrix<double, 6, 1>::Zero());
+        }
+
+        twist_limiter_.setTargetTwist(
+            target_twist,
+            common::math::toPlannerFrame(frame));
+    }
+
+    speedl_command_twist_base_ =
+        twist_limiter_.update(dt, base_R_tool);
     if (!updateAndValidateSpeedLLineDeviation_(q_measured,
                                                is_stop_command,
                                                speedl_command_twist_base_.head<3>())) {
@@ -932,7 +961,11 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         qdot = applyJointAccelerationLimits_(qdot, reference, dt);
     }
     const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
-    if (!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
+    // During a stop, the limiter intentionally commands a near-zero residual
+    // twist while the measured arm can still be moving in a different direction.
+    // Direction and speed-ratio checks are not meaningful for that transient.
+    if (!is_stop_command &&
+        !validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
                                                      achieved_twist_base,
                                                      toEigenVector(q_measured),
                                                      qdot)) {

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h

@@ -1,18 +1,15 @@
 #ifndef CMVR_ES_JOINT_MOTION_PLANNER_H
 #define CMVR_ES_JOINT_MOTION_PLANNER_H
 
+#include <algorithm>
+#include <cmath>
 #include <vector>
 
+#include "common/base/logging/logger.h"
 #include "common/types/arm/arm_types.h"
 
 namespace cmvr::device {
 
-struct JointTrajectorySample {
-    double t{0.0};
-    std::vector<double> position;
-    std::vector<double> velocity;
-};
-
 class JointMotionPlanner {
 public:
     virtual ~JointMotionPlanner() = default;
@@ -23,9 +20,137 @@ public:
                            const JointPositionCommand& target,
                            const MotionOptions& options,
                            double speed_scaling,
-                           std::vector<JointTrajectorySample>& samples) = 0;
+                           JointTrajectory& trajectory) = 0;
+
+    virtual bool planReplay(const std::vector<double>& current_position,
+                            const JointTrajectory& recorded_trajectory,
+                            const MotionOptions& options,
+                            JointTrajectory& replay_trajectory) = 0;
+
+    bool validateJointTrajectory(const JointTrajectory& trajectory,
+                                 std::size_t expected_dof,
+                                 const MotionOptions& limits) const;
 };
 
+inline bool JointMotionPlanner::validateJointTrajectory(
+    const JointTrajectory& trajectory,
+    const std::size_t expected_dof,
+    const MotionOptions& limits) const
+{
+    if (trajectory.size() < 2 || expected_dof == 0) {
+        CMVR_LOG(ERROR) << "[JointMotionPlanner] trajectory must contain at least "
+                           "two points and have a non-zero DOF";
+        return false;
+    }
+    if (!std::isfinite(limits.velocity) || limits.velocity <= 0.0 ||
+        !std::isfinite(limits.acceleration) || limits.acceleration <= 0.0) {
+        CMVR_LOG(ERROR) << "[JointMotionPlanner] velocity or acceleration limit is invalid";
+        return false;
+    }
+    if (!limits.joint_velocity_limits.empty() &&
+        limits.joint_velocity_limits.size() != expected_dof) {
+        CMVR_LOG(ERROR) << "[JointMotionPlanner] joint velocity limit count does not match DOF";
+        return false;
+    }
+
+    constexpr double kVelocityTolerance = 1e-6;
+    constexpr double kAccelerationTolerance = 1e-3;
+    double maximum_velocity = 0.0;
+    double maximum_acceleration = 0.0;
+    double maximum_position_velocity = 0.0;
+    double maximum_position_acceleration = 0.0;
+    double maximum_jerk = 0.0;
+    std::vector<double> previous_position_velocity(expected_dof, 0.0);
+    std::vector<double> previous_acceleration(expected_dof, 0.0);
+
+    for (std::size_t i = 0; i < trajectory.size(); ++i) {
+        const auto& point = trajectory[i];
+        if (!std::isfinite(point.time_s) ||
+            point.position.size() != expected_dof ||
+            point.velocity.size() != expected_dof) {
+            CMVR_LOG(ERROR) << "[JointMotionPlanner] invalid trajectory point at index=" << i;
+            return false;
+        }
+
+        double dt = 0.0;
+        if (i > 0) {
+            dt = point.time_s - trajectory[i - 1].time_s;
+            if (!std::isfinite(dt) || dt <= 0.0) {
+                CMVR_LOG(ERROR) << "[JointMotionPlanner] trajectory time is not increasing at index="
+                                << i;
+                return false;
+            }
+        }
+
+        for (std::size_t joint = 0; joint < expected_dof; ++joint) {
+            if (!std::isfinite(point.position[joint]) ||
+                !std::isfinite(point.velocity[joint])) {
+                CMVR_LOG(ERROR) << "[JointMotionPlanner] non-finite trajectory value at point="
+                                << i << ", joint=" << joint;
+                return false;
+            }
+
+            const double velocity = std::abs(point.velocity[joint]);
+            const double velocity_limit = limits.joint_velocity_limits.empty()
+                                              ? limits.velocity
+                                              : limits.joint_velocity_limits[joint];
+            if (!std::isfinite(velocity_limit) || velocity_limit <= 0.0) {
+                CMVR_LOG(ERROR) << "[JointMotionPlanner] invalid velocity limit for joint="
+                                << joint;
+                return false;
+            }
+            maximum_velocity = std::max(maximum_velocity, velocity);
+            if (velocity > velocity_limit + kVelocityTolerance) {
+                CMVR_LOG(ERROR) << "[JointMotionPlanner] velocity limit exceeded at point="
+                                << i << ", joint=" << joint
+                                << ", actual=" << velocity
+                                << ", limit=" << velocity_limit;
+                return false;
+            }
+
+            if (i > 0) {
+                const double position_velocity =
+                    (point.position[joint] - trajectory[i - 1].position[joint]) / dt;
+                const double acceleration =
+                    (point.velocity[joint] - trajectory[i - 1].velocity[joint]) / dt;
+                maximum_position_velocity = std::max(
+                    maximum_position_velocity, std::abs(position_velocity));
+                maximum_acceleration = std::max(
+                    maximum_acceleration, std::abs(acceleration));
+                if (std::abs(acceleration) >
+                    limits.acceleration + kAccelerationTolerance) {
+                    CMVR_LOG(ERROR) << "[JointMotionPlanner] acceleration limit exceeded at point="
+                                    << i << ", joint=" << joint
+                                    << ", actual=" << std::abs(acceleration)
+                                    << ", limit=" << limits.acceleration;
+                    return false;
+                }
+                if (i > 1) {
+                    maximum_position_acceleration = std::max(
+                        maximum_position_acceleration,
+                        std::abs(position_velocity -
+                                 previous_position_velocity[joint]) / dt);
+                    maximum_jerk = std::max(
+                        maximum_jerk,
+                        std::abs(acceleration - previous_acceleration[joint]) / dt);
+                }
+                previous_position_velocity[joint] = position_velocity;
+                previous_acceleration[joint] = acceleration;
+            }
+        }
+    }
+
+    CMVR_LOG(INFO) << "[JointMotionPlanner] trajectory validated"
+                   << ", points=" << trajectory.size()
+                   << ", max_velocity_rad_s=" << maximum_velocity
+                   << ", max_discrete_acceleration_rad_s2=" << maximum_acceleration
+                   << ", max_position_velocity_rad_s=" << maximum_position_velocity
+                   << ", max_position_acceleration_rad_s2="
+                   << maximum_position_acceleration
+                   << ", max_discrete_jerk_rad_s3=" << maximum_jerk;
+    return true;
+}
+
 } // namespace cmvr::device
 
 #endif // CMVR_ES_JOINT_MOTION_PLANNER_H

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h

@@ -21,9 +21,19 @@ public:
                    const JointPositionCommand& target,
                    const MotionOptions& options,
                    double speed_scaling,
-                   std::vector<JointTrajectorySample>& samples) override;
+                   JointTrajectory& trajectory) override;
+
+    bool planReplay(const std::vector<double>& current_position,
+                    const JointTrajectory& recorded_trajectory,
+                    const MotionOptions& options,
+                    JointTrajectory& replay_trajectory) override;
 
 private:
+    bool sampleTrajectory_(
+        const std::shared_ptr<cmvr::JointTrajectoryPlanner>& planner,
+        const cmvr::TrajPtr& raw_trajectory,
+        JointTrajectory& trajectory) const;
+
     std::shared_ptr<cmvr::JointTrajectoryPlanner> planner_;
     cmvr::PathType path_type_{cmvr::PathType::Quintic};
     double sample_period_s_{0.001};

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp

@@ -1,6 +1,9 @@
 #include "algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h"
 
+#include <cmath>
+
 #include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
+#include "common/base/logging/logger.h"
 
 namespace cmvr::device {
 
@@ -39,36 +42,215 @@ bool ToppraJointMotionPlanner::init()
     return true;
 }
 
+bool ToppraJointMotionPlanner::sampleTrajectory_(
+    const std::shared_ptr<cmvr::JointTrajectoryPlanner>& planner,
+    const cmvr::TrajPtr& raw_trajectory,
+    JointTrajectory& trajectory) const
+{
+    const auto raw_samples = planner->sampleTrajectory(
+        raw_trajectory, sample_period_s_);
+    if (raw_samples.size() < 2) {
+        CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner] trajectory sampling returned fewer than "
+                           "two points: count="
+                        << raw_samples.size();
+        return false;
+    }
+    trajectory.clear();
+    trajectory.reserve(raw_samples.size());
+    for (std::size_t i = 0; i < raw_samples.size(); ++i) {
+        const auto& sample = raw_samples[i];
+        if (!std::isfinite(sample.t) || !sample.q.allFinite() ||
+            !sample.qd.allFinite()) {
+            CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner] sampled trajectory contains "
+                               "a non-finite value at point="
+                            << i;
+            trajectory.clear();
+            return false;
+        }
+        JointTrajectoryPoint point;
+        point.time_s = sample.t;
+        point.position = toStdVector(sample.q);
+        point.velocity = toStdVector(sample.qd);
+        trajectory.push_back(std::move(point));
+    }
+    return true;
+}
+
 bool ToppraJointMotionPlanner::planMoveJ(const std::vector<double>& start,
                                          const JointPositionCommand& target,
                                          const MotionOptions& options,
                                          const double speed_scaling,
-                                         std::vector<JointTrajectorySample>& samples)
+                                         JointTrajectory& trajectory)
 {
-    samples.clear();
+    trajectory.clear();
     if (!planner_ || start.empty() || start.size() != target.position.size() ||
         options.velocity <= 0.0 || options.acceleration <= 0.0) {
         return false;
     }
 
-    cmvr::TrajPtr trajectory;
+    cmvr::TrajPtr raw_trajectory;
     planner_->setPathType(path_type_);
     planner_->setGridSizes(grid_size_, high_grid_size_);
     planner_->setSymmetricLimits(
         std::vector<double>(start.size(), options.velocity * speed_scaling),
         std::vector<double>(start.size(), options.acceleration));
-    if (!planner_->plan(start, target.position, trajectory)) {
+    if (!planner_->plan(start, target.position, raw_trajectory)) {
         return false;
     }
 
-    const auto raw_samples = planner_->sampleTrajectory(trajectory, sample_period_s_);
+    return sampleTrajectory_(planner_, raw_trajectory, trajectory);
-    samples.reserve(raw_samples.size());
+}
-    for (const auto& sample : raw_samples) {
+
-        JointTrajectorySample dst;
+bool ToppraJointMotionPlanner::planReplay(
-        dst.t = sample.t;
+    const std::vector<double>& current_position,
-        dst.position = toStdVector(sample.q);
+    const JointTrajectory& recorded_trajectory,
-        dst.velocity = toStdVector(sample.qd);
+    const MotionOptions& options,
-        samples.push_back(std::move(dst));
+    JointTrajectory& replay_trajectory)
+{
+    replay_trajectory.clear();
+    if (current_position.empty() || recorded_trajectory.size() < 2 ||
+        options.velocity <= 0.0 ||
+        options.acceleration <= 0.0 || !std::isfinite(options.velocity) ||
+        !std::isfinite(options.acceleration)) {
+        CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid trajectory or options";
+        return false;
+    }
+
+    const std::size_t dof = current_position.size();
+    if (!options.joint_velocity_limits.empty() &&
+        options.joint_velocity_limits.size() != dof) {
+        CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid DOF or joint limits";
+        return false;
+    }
+    for (const double position : current_position) {
+        if (!std::isfinite(position)) {
+            CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] non-finite current position";
+            return false;
+        }
+    }
+    for (std::size_t i = 0; i < recorded_trajectory.size(); ++i) {
+        const auto& point = recorded_trajectory[i];
+        if (!std::isfinite(point.time_s) || point.position.size() != dof ||
+            (i > 0 && point.time_s <= recorded_trajectory[i - 1].time_s)) {
+            CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid recorded point: "
+                            << i;
+            return false;
+        }
+        for (std::size_t joint = 0; joint < dof; ++joint) {
+            if (!std::isfinite(point.position[joint])) {
+                CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] non-finite recorded point: "
+                                << i;
+                return false;
+            }
+        }
+    }
+
+    std::vector<double> velocity_limits = options.joint_velocity_limits;
+    if (velocity_limits.empty()) {
+        velocity_limits.assign(dof, options.velocity);
+    }
+    for (std::size_t joint = 0; joint < velocity_limits.size(); ++joint) {
+        const double limit = velocity_limits[joint];
+        if (!std::isfinite(limit) || limit <= 0.0) {
+            CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid joint velocity limit";
+            return false;
+        }
+    }
+
+    const double ramp_duration_s = std::max(
+        sample_period_s_, options.velocity / options.acceleration);
+    replay_trajectory.reserve(recorded_trajectory.size() + 2);
+    replay_trajectory.push_back(JointTrajectoryPoint{
+        0.0, current_position, std::vector<double>(dof, 0.0)});
+
+    double replay_time_s = ramp_duration_s;
+    replay_trajectory.push_back(JointTrajectoryPoint{
+        replay_time_s,
+        recorded_trajectory.back().position,
+        std::vector<double>(dof, 0.0)});
+    for (std::size_t i = recorded_trajectory.size() - 1; i > 0; --i) {
+        replay_time_s += recorded_trajectory[i].time_s -
+                         recorded_trajectory[i - 1].time_s;
+        replay_trajectory.push_back(JointTrajectoryPoint{
+            replay_time_s,
+            recorded_trajectory[i - 1].position,
+            std::vector<double>(dof, 0.0)});
+    }
+    replay_time_s += ramp_duration_s;
+    replay_trajectory.push_back(JointTrajectoryPoint{
+        replay_time_s,
+        recorded_trajectory.front().position,
+        std::vector<double>(dof, 0.0)});
+
+    const auto update_velocities = [&] {
+        for (auto& point : replay_trajectory) {
+            std::fill(point.velocity.begin(), point.velocity.end(), 0.0);
+        }
+        for (std::size_t i = 1; i + 1 < replay_trajectory.size(); ++i) {
+            const double dt = replay_trajectory[i + 1].time_s -
+                              replay_trajectory[i - 1].time_s;
+            for (std::size_t joint = 0; joint < dof; ++joint) {
+                replay_trajectory[i].velocity[joint] =
+                    (replay_trajectory[i + 1].position[joint] -
+                     replay_trajectory[i - 1].position[joint]) / dt;
+            }
+        }
+    };
+
+    for (int iteration = 0; iteration < 3; ++iteration) {
+        update_velocities();
+        double required_scale = 1.0;
+        std::vector<double> previous_position_velocity(dof, 0.0);
+        for (std::size_t i = 0; i < replay_trajectory.size(); ++i) {
+            const auto& point = replay_trajectory[i];
+            for (std::size_t joint = 0; joint < dof; ++joint) {
+                required_scale = std::max(
+                    required_scale,
+                    std::abs(point.velocity[joint]) / velocity_limits[joint]);
+                if (i == 0) {
+                    continue;
+                }
+
+                const double dt = point.time_s -
+                                  replay_trajectory[i - 1].time_s;
+                const double position_velocity =
+                    (point.position[joint] -
+                     replay_trajectory[i - 1].position[joint]) / dt;
+                const double acceleration =
+                    (point.velocity[joint] -
+                     replay_trajectory[i - 1].velocity[joint]) / dt;
+                required_scale = std::max(
+                    required_scale,
+                    std::abs(position_velocity) / velocity_limits[joint]);
+                required_scale = std::max(
+                    required_scale,
+                    std::sqrt(std::abs(acceleration) /
+                              options.acceleration));
+                if (i > 1) {
+                    const double position_acceleration =
+                        (position_velocity -
+                         previous_position_velocity[joint]) / dt;
+                    required_scale = std::max(
+                        required_scale,
+                        std::sqrt(std::abs(position_acceleration) /
+                                  options.acceleration));
+                }
+                previous_position_velocity[joint] = position_velocity;
+            }
+        }
+
+        if (required_scale <= 1.0 + 1e-9) {
+            break;
+        }
+        required_scale *= 1.001;
+        for (auto& point : replay_trajectory) {
+            point.time_s *= required_scale;
+        }
+    }
+    update_velocities();
+    if (!validateJointTrajectory(replay_trajectory, dof, options)) {
+        replay_trajectory.clear();
+        return false;
     }
     return true;
 }

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp

@@ -0,0 +1,139 @@
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "joint_motion/toppra/include/toppra_joint_motion_planner.h"
+
+namespace cmvr::device {
+namespace {
+
+constexpr std::size_t kDof = 7;
+
+JointTrajectory makeRecordedTrajectory(const std::size_t point_count)
+{
+    JointTrajectory trajectory;
+    trajectory.reserve(point_count);
+    for (std::size_t i = 0; i < point_count; ++i) {
+        const double s = static_cast<double>(i) /
+                         static_cast<double>(point_count - 1);
+        JointTrajectoryPoint point;
+        point.time_s = static_cast<double>(i) * 0.002;
+        point.position = {
+            0.40 * s,
+            -0.25 * s + 0.03 * std::sin(3.141592653589793 * s),
+            0.20 * s * s,
+            0.30 * std::sin(1.5707963267948966 * s),
+            -0.12 * s,
+            0.15 * s,
+            -0.08 * std::sin(3.141592653589793 * s),
+        };
+        point.velocity.assign(kDof, 0.0);
+        trajectory.push_back(std::move(point));
+    }
+    return trajectory;
+}
+
+double maximumPositionError(const std::vector<double>& lhs,
+                            const std::vector<double>& rhs)
+{
+    if (lhs.size() != rhs.size()) {
+        return std::numeric_limits<double>::infinity();
+    }
+    double maximum = 0.0;
+    for (std::size_t i = 0; i < lhs.size(); ++i) {
+        maximum = std::max(maximum, std::abs(lhs[i] - rhs[i]));
+    }
+    return maximum;
+}
+
+TEST(ToppraJointMotionPlannerTest, PlansBoundedReverseReplay)
+{
+    ToppraJointMotionPlanner planner(
+        cmvr::PathType::Quintic, 0.001, 150, 300);
+    ASSERT_TRUE(planner.init());
+
+    const JointTrajectory recorded = makeRecordedTrajectory(300);
+    MotionOptions options;
+    options.velocity = 0.15;
+    options.acceleration = 5.0;
+
+    JointTrajectory replay;
+    ASSERT_TRUE(planner.planReplay(
+        recorded.back().position, recorded, options, replay));
+    ASSERT_EQ(replay.size(), recorded.size() + 2);
+    EXPECT_LT(maximumPositionError(
+                  replay.front().position, recorded.back().position),
+              1e-9);
+    EXPECT_LT(maximumPositionError(
+                  replay.back().position, recorded.front().position),
+              1e-9);
+    for (std::size_t i = 0; i < recorded.size(); ++i) {
+        EXPECT_LT(maximumPositionError(
+                      replay[i + 1].position,
+                      recorded[recorded.size() - 1 - i].position),
+                  1e-9);
+    }
+
+    double maximum_velocity = 0.0;
+    double maximum_acceleration = 0.0;
+    for (std::size_t i = 0; i < replay.size(); ++i) {
+        ASSERT_EQ(replay[i].position.size(), kDof);
+        ASSERT_EQ(replay[i].velocity.size(), kDof);
+        for (std::size_t joint = 0; joint < kDof; ++joint) {
+            maximum_velocity = std::max(
+                maximum_velocity, std::abs(replay[i].velocity[joint]));
+            if (i > 0) {
+                const double dt = replay[i].time_s - replay[i - 1].time_s;
+                ASSERT_GT(dt, 0.0);
+                maximum_acceleration = std::max(
+                    maximum_acceleration,
+                    std::abs(replay[i].velocity[joint] -
+                             replay[i - 1].velocity[joint]) / dt);
+            }
+        }
+    }
+    EXPECT_LE(maximum_velocity, options.velocity + 1e-6);
+    EXPECT_LE(maximum_acceleration, options.acceleration + 1e-3);
+}
+
+TEST(ToppraJointMotionPlannerTest, RejectsNonIncreasingRecordedTime)
+{
+    ToppraJointMotionPlanner planner(
+        cmvr::PathType::Quintic, 0.001, 150, 300);
+    ASSERT_TRUE(planner.init());
+
+    JointTrajectory recorded = makeRecordedTrajectory(10);
+    recorded[5].time_s = recorded[4].time_s;
+    MotionOptions options;
+    options.velocity = 0.15;
+    options.acceleration = 5.0;
+
+    JointTrajectory replay;
+    EXPECT_FALSE(planner.planReplay(
+        recorded.back().position, recorded, options, replay));
+    EXPECT_TRUE(replay.empty());
+}
+
+TEST(ToppraJointMotionPlannerTest, ValidationRejectsInvalidOutputTrajectory)
+{
+    ToppraJointMotionPlanner planner(
+        cmvr::PathType::Quintic, 0.001, 150, 300);
+    MotionOptions options;
+    options.velocity = 0.15;
+    options.acceleration = 5.0;
+
+    JointTrajectory trajectory = makeRecordedTrajectory(10);
+    trajectory[5].velocity[2] = options.velocity + 0.01;
+    EXPECT_FALSE(planner.validateJointTrajectory(trajectory, kDof, options));
+
+    trajectory[5].velocity[2] = 0.0;
+    trajectory[5].time_s = trajectory[4].time_s;
+    EXPECT_FALSE(planner.validateJointTrajectory(trajectory, kDof, options));
+}
+
+} // namespace
+} // namespace cmvr::device

cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt

@@ -21,3 +21,25 @@ target_link_libraries(base_motion PUBLIC
 
 add_library(cmvr_es::base_motion ALIAS base_motion)
 install(TARGETS base_motion LIBRARY DESTINATION lib)
+
+add_executable(toppra_multi_waypoint_test
+        joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp
+)
+
+target_link_libraries(toppra_multi_waypoint_test
+        PRIVATE
+        cmvr_es::base_motion
+        gtest
+        gtest_main
+)
+
+add_executable(s_curve_velocity_planner_stop_test
+        motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp
+)
+
+target_link_libraries(s_curve_velocity_planner_stop_test
+        PRIVATE
+        cmvr_es::base_motion
+        gtest
+        gtest_main
+)

cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp

@@ -201,7 +201,12 @@ void CartesianTwistLimiter::setTargetTwist(const Twist& target_twist, CartesianF
 
 void CartesianTwistLimiter::stop()
 {
+  // target_twist_input_.setZero();
   target_twist_input_.setZero();
+  target_twist_base_.setZero();
+
+  linear_norm_planner_.setTargetVelocity(0.0);
+  angular_norm_planner_.setTargetVelocity(0.0);
 }
 
 void CartesianTwistLimiter::emergencyStop(double emergency_acceleration,

cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h

@@ -109,37 +109,18 @@ namespace cmvr {
         static void sanitizeVsq(toppra::Vector &v);
 
 
-        // centripetal 弦长（alpha=0.5），生成严格递增 S
+        // Joint-space chord length keeps the parameterization independent of
+        // how densely the same geometric path is sampled.
         static std::vector<toppra::value_type>
-        makeS_centripetal(const std::vector<Eigen::VectorXd> &q) {
+        makeSChordLength(const std::vector<Eigen::VectorXd> &q) {
             const size_t M = q.size();
             std::vector<toppra::value_type> S(M, 0.0);
-            auto chord = [](const Eigen::VectorXd &a, const Eigen::VectorXd &b) {
-                double d = (a - b).norm();
-                return std::pow(std::max(d, 1e-16), 0.5);
-            };
             for (size_t i = 1; i < M; ++i) {
-                S[i] = S[i - 1] + chord(q[i], q[i - 1]);
+                S[i] = S[i - 1] + (q[i] - q[i - 1]).norm();
-                if (S[i] <= S[i - 1]) S[i] = S[i - 1] + 1e-12;
             }
             return S;
         }
 
-        // 等距参数（简单稳妥）
-        static inline std::vector<toppra::value_type> makeS_equal(size_t M) {
-            std::vector<toppra::value_type> S(M);
-            for (size_t i = 0; i < M; ++i) S[i] = static_cast<toppra::value_type>(i);
-            return S;
-        }
-
-        // 或：先用centripetal，再整体归一化到跨度≈(M-1)，并设置每段最小ds
-        static inline void normalize_and_floor_S(std::vector<toppra::value_type> &S, double ds_min = 0.2) {
-            for (size_t i = 1; i < S.size(); ++i) S[i] -= S[0];
-            double L = S.back();
-            if (L > 0) for (auto &x: S) x *= (S.size() - 1) / L;
-            for (size_t i = 1; i < S.size(); ++i) if (S[i] - S[i - 1] < ds_min) S[i] = S[i - 1] + ds_min;
-        }
-
         // Catmull–Rom（centripetal）估计结点几何速度 v（端点=0）
         static std::vector<Eigen::VectorXd>
         estimateVelsCatmull(const std::vector<Eigen::VectorXd> &q,
@@ -159,14 +140,16 @@ namespace cmvr {
         // 对内点几何速度限幅，抑制过冲（k∈[0.5,1.0]）
         static void clampNodeVels(std::vector<Eigen::VectorXd> &v,
                                   const std::vector<Eigen::VectorXd> &q,
+                                  const std::vector<toppra::value_type> &S,
                                   double k = 1.0) {
             const size_t M = q.size();
             if (M <= 2) return;
             for (size_t i = 1; i + 1 < M; ++i) {
-                double d0 = (q[i] - q[i - 1]).norm();
+                const double ds0 = std::max<double>(S[i] - S[i - 1], 1e-12);
-                double d1 = (q[i + 1] - q[i]).norm();
+                const double ds1 = std::max<double>(S[i + 1] - S[i], 1e-12);
-                double d = std::max(std::min(d0, d1), 1e-12);
+                const double slope0 = (q[i] - q[i - 1]).norm() / ds0;
-                double vmax = k * d;
+                const double slope1 = (q[i + 1] - q[i]).norm() / ds1;
+                const double vmax = k * std::min(slope0, slope1);
                 double n = v[i].norm();
                 if (n > vmax) v[i] *= (vmax / n);
             }

cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp

@@ -5,10 +5,117 @@
 #include <toppra/toppra.hpp>
 #include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
 
+#include <algorithm>
+#include <cmath>
 #include <fstream>
 #include <iomanip>
 
 namespace cmvr {
+    namespace {
+
+    class TimeScaledTrajectory final : public ITrajectory {
+    public:
+        TimeScaledTrajectory(TrajPtr source, const double scale)
+            : source_(std::move(source)), scale_(scale), source_interval_(source_->timeInterval())
+        {
+        }
+
+        toppra::Bound timeInterval() const override
+        {
+            toppra::Bound interval;
+            interval << source_interval_[0],
+                source_interval_[0] +
+                    (source_interval_[1] - source_interval_[0]) * scale_;
+            return interval;
+        }
+
+        Eigen::VectorXd q(const double t) const override
+        {
+            return source_->q(sourceTime_(t));
+        }
+
+        Eigen::VectorXd qd(const double t) const override
+        {
+            return source_->qd(sourceTime_(t)) / scale_;
+        }
+
+        Eigen::VectorXd qdd(const double t) const override
+        {
+            return source_->qdd(sourceTime_(t)) / (scale_ * scale_);
+        }
+
+    private:
+        double sourceTime_(const double output_time) const
+        {
+            return std::clamp(
+                source_interval_[0] +
+                    (output_time - source_interval_[0]) / scale_,
+                source_interval_[0],
+                source_interval_[1]);
+        }
+
+        TrajPtr source_;
+        double scale_{1.0};
+        toppra::Bound source_interval_;
+    };
+
+    bool enforceSampledLimits(const TrajPtr& source,
+                              const std::vector<double>& velocity_limits,
+                              const std::vector<double>& acceleration_limits,
+                              const std::size_t waypoint_count,
+                              TrajPtr& output)
+    {
+        if (!source || velocity_limits.empty() ||
+            velocity_limits.size() != acceleration_limits.size()) {
+            return false;
+        }
+        const auto interval = source->timeInterval();
+        const double duration = interval[1] - interval[0];
+        if (!std::isfinite(duration) || duration <= 0.0) {
+            return false;
+        }
+
+        const std::size_t time_samples = static_cast<std::size_t>(
+            std::ceil(duration / 0.001)) + 1;
+        const std::size_t path_samples = waypoint_count * 20;
+        const std::size_t sample_count = std::clamp<std::size_t>(
+            std::max({std::size_t{1000}, time_samples, path_samples}),
+            std::size_t{1000},
+            std::size_t{200000});
+
+        double required_scale = 1.0;
+        for (std::size_t sample = 0; sample < sample_count; ++sample) {
+            const double ratio = static_cast<double>(sample) /
+                                 static_cast<double>(sample_count - 1);
+            const double time = interval[0] + duration * ratio;
+            const Eigen::VectorXd velocity = source->qd(time);
+            const Eigen::VectorXd acceleration = source->qdd(time);
+            if (!velocity.allFinite() || !acceleration.allFinite() ||
+                velocity.size() != static_cast<Eigen::Index>(velocity_limits.size()) ||
+                acceleration.size() !=
+                    static_cast<Eigen::Index>(acceleration_limits.size())) {
+                return false;
+            }
+            for (Eigen::Index joint = 0; joint < velocity.size(); ++joint) {
+                const std::size_t index = static_cast<std::size_t>(joint);
+                required_scale = std::max(
+                    required_scale,
+                    std::abs(velocity[joint]) / velocity_limits[index]);
+                required_scale = std::max(
+                    required_scale,
+                    std::sqrt(std::abs(acceleration[joint]) /
+                              acceleration_limits[index]));
+            }
+        }
+
+        constexpr double kNumericalMargin = 1.001;
+        output = std::make_shared<TimeScaledTrajectory>(
+            source, required_scale * kNumericalMargin);
+        return true;
+    }
+
+    } // namespace
+
     // ===== ConstAccelTraj =====
     ConstAccelTraj::ConstAccelTraj(std::shared_ptr<toppra::parametrizer::ConstAccel> p)
         : impl_(std::move(p)) {
@@ -54,22 +161,39 @@ namespace cmvr {
     bool ToppraJointTrajectoryPlanner::plan(const std::vector<std::vector<double>>& waypoints,
                              TrajPtr& traj_out) {
         traj_out.reset();
-        const size_t M = waypoints.size();
+        if (waypoints.size() < 2) return false;
-        if (M < 2) return false;
         const size_t DoF = waypoints.front().size();
-        for (const auto& w : waypoints) if (w.size()!=DoF) return false;
+        if (DoF == 0) return false;
+        for (const auto& w : waypoints) {
+            if (w.size() != DoF) return false;
+            for (const double value : w) {
+                if (!std::isfinite(value)) return false;
+            }
+        }
         if (!ensureLimitsSized(DoF)) return false;
+        for (size_t joint = 0; joint < DoF; ++joint) {
+            if (!std::isfinite(v_max_[joint]) || v_max_[joint] <= 0.0 ||
+                !std::isfinite(a_max_[joint]) || a_max_[joint] <= 0.0) {
+                return false;
+            }
+        }
 
-        // 组装
+        std::vector<Eigen::VectorXd> q;
-        std::vector<Eigen::VectorXd> q; q.reserve(M);
+        q.reserve(waypoints.size());
-        for (const auto& w : waypoints)
+        constexpr double kDuplicateDistance = 1e-10;
-            q.emplace_back(Eigen::Map<const Eigen::VectorXd>(w.data(), DoF));
+        for (const auto& waypoint : waypoints) {
+            Eigen::VectorXd value = Eigen::Map<const Eigen::VectorXd>(
+                waypoint.data(), static_cast<Eigen::Index>(DoF));
+            if (q.empty() || (value - q.back()).norm() > kDuplicateDistance) {
+                q.push_back(std::move(value));
+            }
+        }
+        if (q.size() < 2) return false;
+        const size_t M = q.size();
 
-        // 生成 S
+        const std::vector<toppra::value_type> S = M == 2
-//        std::vector<toppra::value_type> S = (M==2) ? std::vector<toppra::value_type>{0.0,1.0}
+            ? std::vector<toppra::value_type>{0.0, 1.0}
-//                                                   : makeS_centripetal(q);
+            : makeSChordLength(q);
-        std::vector<toppra::value_type> S = (M==2) ? std::vector<toppra::value_type>{0.0,1.0}
-                                                   : makeS_equal(M);
 
         // 几何路径
         auto path = buildPathUnified(q, S);
@@ -86,8 +210,23 @@ namespace cmvr {
 
         // TOPPRA
         toppra::algorithm::TOPPRA algo{constraints, path};
-        auto solve_once = [&](int N)->bool{
+        auto solve_once = [&](const int requested_intervals)->bool{
-            algo.setN(N);
+            const int segment_count = static_cast<int>(M - 1);
+            const int subdivisions = std::max(
+                1, (requested_intervals + segment_count - 1) / segment_count);
+            toppra::Vector grid(segment_count * subdivisions + 1);
+            Eigen::Index index = 0;
+            for (int segment = 0; segment < segment_count; ++segment) {
+                const double start = S[static_cast<size_t>(segment)];
+                const double length = S[static_cast<size_t>(segment + 1)] - start;
+                for (int subdivision = 0; subdivision < subdivisions; ++subdivision) {
+                    grid[index++] = start + length *
+                        static_cast<double>(subdivision) /
+                        static_cast<double>(subdivisions);
+                }
+            }
+            grid[index] = S.back();
+            algo.setGridpoints(grid);
             algo.solver(std::make_shared<toppra::solver::Seidel>());
             return algo.computePathParametrization(0.0, 0.0) == toppra::ReturnCode::OK;
         };
@@ -99,20 +238,21 @@ namespace cmvr {
         toppra::Vector grid = data.gridpoints;
         toppra::Vector vsq  = data.parametrization;
 
+        TrajPtr candidate;
         auto ca = std::make_shared<toppra::parametrizer::ConstAccel>(path, grid, vsq);
         if (ca->validate()) {
-            traj_out = std::make_shared<ConstAccelTraj>(std::move(ca));
+            candidate = std::make_shared<ConstAccelTraj>(std::move(ca));
-            return true;
+        } else {
-        }
             sanitizeVsq(vsq);
             try {
-            traj_out = std::make_shared<SplineTraj>(path, grid, vsq);
+                candidate = std::make_shared<SplineTraj>(path, grid, vsq);
-            (void) traj_out->timeInterval();
+                (void) candidate->timeInterval();
-            return true;
             } catch (...) {
                 return false;
             }
         }
+        return enforceSampledLimits(candidate, v_max_, a_max_, M, traj_out);
+    }
 
 
     bool ToppraJointTrajectoryPlanner::plan(const std::vector<double>& start_joints,
@@ -225,7 +365,7 @@ namespace cmvr {
             double ds = std::max<double>(S[k+1]-S[k], 1e-12);
             toppra::Matrix seg(2, DoF);
             Eigen::RowVectorXd A1 = ((q[k+1]-q[k])/ds).transpose();
-            Eigen::RowVectorXd A0 = (q[k] - A1.transpose()*S[k]).transpose();
+            Eigen::RowVectorXd A0 = q[k].transpose();
             seg.row(0)=A1; seg.row(1)=A0;
             segs.emplace_back(std::move(seg));
         }
@@ -237,7 +377,7 @@ namespace cmvr {
     ToppraJointTrajectoryPlanner::buildCubicHermiteMulti(const std::vector<Eigen::VectorXd>& q,
                                           const std::vector<toppra::value_type>& S) {
         auto v = estimateVelsCatmull(q, S);
-        clampNodeVels(v, q, /*k=*/1.0);
+        clampNodeVels(v, q, S, /*k=*/1.0);
         toppra::Vectors pos(q.begin(), q.end());
         toppra::Vectors vel(v.begin(), v.end());
         auto herm = toppra::PiecewisePolyPath::CubicHermiteSpline(pos, vel, S);
@@ -282,7 +422,7 @@ namespace cmvr {
                                        const std::vector<toppra::value_type>& S) {
         const size_t M = q.size(), DoF = q[0].size();
         auto v = estimateVelsCatmull(q, S);
-        clampNodeVels(v, q, /*k=*/1.0);
+        clampNodeVels(v, q, S, /*k=*/1.0);
         auto a = estimateAccelsSecondDiff(q, S);
 
         toppra::Matrices segs; segs.reserve(M-1);
@@ -301,11 +441,11 @@ namespace cmvr {
             Eigen::VectorXd C5 = (  6.0*dq - (3.0*A1 + 0.5*(a0*ds*ds)) - (3.0*(v1*ds) - 0.5*(a1*ds*ds)) );
 
             toppra::Matrix seg(6, DoF);
-            seg.row(0)=C5.transpose();
+            seg.row(0)=(C5 / std::pow(ds, 5)).transpose();
-            seg.row(1)=C4.transpose();
+            seg.row(1)=(C4 / std::pow(ds, 4)).transpose();
-            seg.row(2)=C3.transpose();
+            seg.row(2)=(C3 / std::pow(ds, 3)).transpose();
-            seg.row(3)=A2.transpose();
+            seg.row(3)=(a0 / 2.0).transpose();
-            seg.row(4)=A1.transpose();
+            seg.row(4)=v0.transpose();
             seg.row(5)=A0.transpose();
             segs.emplace_back(std::move(seg));
         }

cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp

@@ -0,0 +1,228 @@
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstddef>
+#include <iostream>
+#include <limits>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
+
+namespace cmvr {
+namespace {
+
+constexpr std::size_t kDof = 7;
+constexpr double kVelocityLimit = 0.15;
+constexpr double kAccelerationLimit = 0.3;
+constexpr double kSamplePeriodS = 0.002;
+
+std::vector<std::vector<double>> makeSmoothWaypoints(const std::size_t count)
+{
+    constexpr double kPi = 3.14159265358979323846;
+    std::vector<std::vector<double>> waypoints;
+    waypoints.reserve(count);
+    for (std::size_t i = 0; i < count; ++i) {
+        const double s = static_cast<double>(i) /
+                         static_cast<double>(count - 1);
+        std::vector<double> q(kDof, 0.0);
+        q[0] = 0.40 * s + 0.03 * std::sin(2.0 * kPi * s);
+        q[1] = -0.25 * s + 0.04 * std::sin(kPi * s);
+        q[2] = 0.20 * s * s;
+        q[3] = 0.30 * std::sin(0.5 * kPi * s);
+        q[4] = -0.12 * s + 0.02 * std::sin(3.0 * kPi * s);
+        q[5] = 0.15 * s;
+        q[6] = -0.08 * std::sin(kPi * s);
+        waypoints.push_back(std::move(q));
+    }
+    return waypoints;
+}
+
+double maxAbs(const Eigen::VectorXd& value)
+{
+    double result = 0.0;
+    for (Eigen::Index i = 0; i < value.size(); ++i) {
+        result = std::max(result, std::abs(value[i]));
+    }
+    return result;
+}
+
+double positionError(const Eigen::VectorXd& actual,
+                     const std::vector<double>& expected)
+{
+    if (actual.size() != static_cast<Eigen::Index>(expected.size())) {
+        return std::numeric_limits<double>::infinity();
+    }
+    double squared_error = 0.0;
+    for (Eigen::Index i = 0; i < actual.size(); ++i) {
+        const double error = actual[i] - expected[static_cast<std::size_t>(i)];
+        squared_error += error * error;
+    }
+    return std::sqrt(squared_error);
+}
+
+struct PlanMetrics {
+    bool success{false};
+    double planning_ms{0.0};
+    double duration_s{0.0};
+    double max_velocity{0.0};
+    double max_acceleration{0.0};
+    double max_waypoint_error{0.0};
+    double start_error{0.0};
+    double end_error{0.0};
+    std::size_t sample_count{0};
+};
+
+PlanMetrics planAndMeasure(const std::vector<std::vector<double>>& waypoints,
+                           const PathType path_type = PathType::Linear)
+{
+    PlanMetrics metrics;
+    ToppraJointTrajectoryPlanner planner(path_type);
+    planner.setSymmetricLimits(
+        std::vector<double>(kDof, kVelocityLimit),
+        std::vector<double>(kDof, kAccelerationLimit));
+    planner.setGridSizes(150, 300);
+
+    TrajPtr trajectory;
+    const auto start = std::chrono::steady_clock::now();
+    metrics.success = planner.plan(waypoints, trajectory);
+    metrics.planning_ms = std::chrono::duration<double, std::milli>(
+        std::chrono::steady_clock::now() - start).count();
+    if (!metrics.success || !trajectory) {
+        return metrics;
+    }
+
+    const auto interval = trajectory->timeInterval();
+    metrics.duration_s = interval[1] - interval[0];
+    const auto samples = planner.sampleTrajectory(trajectory, kSamplePeriodS);
+    metrics.sample_count = samples.size();
+    if (samples.empty()) {
+        metrics.success = false;
+        return metrics;
+    }
+    metrics.start_error = positionError(samples.front().q, waypoints.front());
+    metrics.end_error = positionError(samples.back().q, waypoints.back());
+
+    for (const auto& sample : samples) {
+        if (!std::isfinite(sample.t) || !sample.q.allFinite() ||
+            !sample.qd.allFinite() || !sample.qdd.allFinite()) {
+            metrics.success = false;
+            return metrics;
+        }
+        metrics.max_velocity = std::max(metrics.max_velocity, maxAbs(sample.qd));
+        metrics.max_acceleration = std::max(
+            metrics.max_acceleration, maxAbs(sample.qdd));
+    }
+
+    std::size_t sample_index = 0;
+    for (const auto& waypoint : waypoints) {
+        while (sample_index + 1 < samples.size() &&
+               positionError(samples[sample_index + 1].q, waypoint) <=
+                   positionError(samples[sample_index].q, waypoint)) {
+            ++sample_index;
+        }
+        metrics.max_waypoint_error = std::max(
+            metrics.max_waypoint_error,
+            positionError(samples[sample_index].q, waypoint));
+    }
+    return metrics;
+}
+
+const char* pathTypeName(const PathType path_type)
+{
+    switch (path_type) {
+        case PathType::Linear: return "Linear";
+        case PathType::CubicHermite: return "CubicHermite";
+        case PathType::Quintic: return "Quintic";
+        case PathType::Natural: return "Natural";
+    }
+    return "Unknown";
+}
+
+void printMetrics(const std::size_t waypoint_count, const PlanMetrics& metrics)
+{
+    std::cout << "[ToppraMultiWaypointTest] waypoints=" << waypoint_count
+              << ", success=" << metrics.success
+              << ", planning_ms=" << metrics.planning_ms
+              << ", duration_s=" << metrics.duration_s
+              << ", samples=" << metrics.sample_count
+              << ", max_qd=" << metrics.max_velocity
+              << ", max_qdd=" << metrics.max_acceleration
+              << ", max_waypoint_error=" << metrics.max_waypoint_error
+              << ", start_error=" << metrics.start_error
+              << ", end_error=" << metrics.end_error
+              << std::endl;
+}
+
+TEST(ToppraMultiWaypointTest, SmoothSevenDofPathScalesToThousandsOfWaypoints)
+{
+    double reference_duration_s = 0.0;
+    for (const std::size_t count : {10U, 100U, 300U, 1000U, 3000U}) {
+        const auto metrics = planAndMeasure(makeSmoothWaypoints(count));
+        printMetrics(count, metrics);
+        ASSERT_TRUE(metrics.success) << "waypoint_count=" << count;
+        EXPECT_GT(metrics.duration_s, 0.0) << "waypoint_count=" << count;
+        EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6)
+            << "waypoint_count=" << count;
+        EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5)
+            << "waypoint_count=" << count;
+        EXPECT_LT(metrics.max_waypoint_error, 0.002)
+            << "waypoint_count=" << count;
+        if (reference_duration_s == 0.0) {
+            reference_duration_s = metrics.duration_s;
+        } else {
+            EXPECT_NEAR(metrics.duration_s, reference_duration_s,
+                        reference_duration_s * 0.10)
+                << "waypoint_count=" << count;
+        }
+    }
+}
+
+TEST(ToppraMultiWaypointTest, RepeatedWaypointsRemainPlannable)
+{
+    const auto smooth = makeSmoothWaypoints(300);
+    std::vector<std::vector<double>> repeated;
+    repeated.reserve(smooth.size() * 2);
+    for (const auto& waypoint : smooth) {
+        repeated.push_back(waypoint);
+        repeated.push_back(waypoint);
+    }
+
+    const auto metrics = planAndMeasure(repeated);
+    printMetrics(repeated.size(), metrics);
+    EXPECT_TRUE(metrics.success);
+    if (metrics.success) {
+        EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6);
+        EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5);
+        EXPECT_LT(metrics.max_waypoint_error, 0.002);
+    }
+}
+
+TEST(ToppraMultiWaypointTest, CompareInterpolationModesAtThreeHundredWaypoints)
+{
+    const auto waypoints = makeSmoothWaypoints(300);
+    for (const auto path_type : {
+             PathType::CubicHermite,
+             PathType::Quintic,
+             PathType::Natural}) {
+        const auto metrics = planAndMeasure(waypoints, path_type);
+        std::cout << "[ToppraMultiWaypointTest] path_type="
+                  << pathTypeName(path_type) << std::endl;
+        printMetrics(waypoints.size(), metrics);
+        EXPECT_TRUE(metrics.success) << pathTypeName(path_type);
+        if (metrics.success) {
+            EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6)
+                << pathTypeName(path_type);
+            EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5)
+                << pathTypeName(path_type);
+            EXPECT_LT(metrics.max_waypoint_error, 0.002)
+                << pathTypeName(path_type);
+            EXPECT_LT(metrics.start_error, 1e-9) << pathTypeName(path_type);
+            EXPECT_LT(metrics.end_error, 1e-9) << pathTypeName(path_type);
+        }
+    }
+}
+
+} // namespace
+} // namespace cmvr

cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp

@@ -118,9 +118,25 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity,
                                                    double acceleration)
 {
   const double measured_velocity = clamp(velocity, -max_velocity_, max_velocity_);
-  const double measured_acceleration =
+  double measured_acceleration =
       clamp(acceleration, -max_acceleration_, max_acceleration_);
 
+  // When the target is zero this planner is also used for Cartesian speed
+  // magnitudes. A magnitude is non-negative, while the finite-difference
+  // derivative of the measured magnitude is signed. Feeding a large negative
+  // measured acceleration into a signed 1-D velocity planner can generate a
+  // profile that crosses through zero and becomes negative before returning to
+  // zero. The twist limiter then multiplies that negative "norm" by the
+  // current direction, which reverses and amplifies the Cartesian command.
+  //
+  // For feedback resynchronization during a stop, synchronize the measured
+  // speed only and restart the stop profile with zero scalar acceleration.
+  // This avoids noise-sensitive stop replans and preserves a non-overshooting
+  // deceleration profile for speed-magnitude users.
+  if (std::abs(state_.target_velocity) <= VELOCITY_THRESHOLD) {
+    measured_acceleration = 0.0;
+  }
+
   if (!state_.has_active_profile &&
       std::abs(measured_velocity - state_.target_velocity) <= VELOCITY_THRESHOLD) {
     state_.velocity = state_.target_velocity;

cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp

@@ -0,0 +1,77 @@
+#include <algorithm>
+
+#include <gtest/gtest.h>
+
+#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_velocity_planner.h"
+
+namespace cmvr {
+namespace {
+
+void finishActiveProfile(SCurveVelocityPlanner1D& planner, double dt)
+{
+  for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) {
+    planner.update(dt);
+  }
+  ASSERT_FALSE(planner.hasActiveProfile());
+}
+
+TEST(SCurveVelocityPlannerStopTest, FeedbackResyncDoesNotReverseSpeedMagnitude)
+{
+  SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0);
+  constexpr double kDt = 0.001;
+  constexpr double kInitialSpeed = 0.04;
+
+  planner.initialize(kInitialSpeed, 0.0);
+  planner.setTargetVelocity(0.0);
+  finishActiveProfile(planner, kDt);
+
+  // Reproduce the speedL stop feedback case: the command profile has already
+  // reached zero, but the measured TCP still has residual speed. A 1 kHz
+  // finite difference may report a large negative scalar acceleration.
+  planner.synchronizeAndReplan(kInitialSpeed, -3.0);
+
+  double max_speed = planner.getVelocity();
+  double min_speed = planner.getVelocity();
+  for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) {
+    const double speed = planner.update(kDt);
+    max_speed = std::max(max_speed, speed);
+    min_speed = std::min(min_speed, speed);
+  }
+
+  EXPECT_GE(min_speed, -1e-9);
+  EXPECT_LE(max_speed, kInitialSpeed + 1e-9);
+  EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9);
+}
+
+TEST(SCurveVelocityPlannerStopTest, LargerMeasuredDecelerationDoesNotIncreaseStopSpeed)
+{
+  constexpr double kDt = 0.001;
+  constexpr double kInitialSpeed = 0.04;
+  const double measured_accelerations[] = {-0.5, -1.0, -2.0, -3.0};
+
+  for (const double measured_acceleration : measured_accelerations) {
+    SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0);
+    planner.initialize(kInitialSpeed, 0.0);
+    planner.setTargetVelocity(0.0);
+    finishActiveProfile(planner, kDt);
+
+    planner.synchronizeAndReplan(kInitialSpeed, measured_acceleration);
+
+    double max_speed = planner.getVelocity();
+    double min_speed = planner.getVelocity();
+    for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) {
+      const double speed = planner.update(kDt);
+      max_speed = std::max(max_speed, speed);
+      min_speed = std::min(min_speed, speed);
+    }
+
+    EXPECT_GE(min_speed, -1e-9) << "measured_acceleration=" << measured_acceleration;
+    EXPECT_LE(max_speed, kInitialSpeed + 1e-9)
+        << "measured_acceleration=" << measured_acceleration;
+    EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9)
+        << "measured_acceleration=" << measured_acceleration;
+  }
+}
+
+} // namespace
+} // namespace cmvr

cmvr-es/algorithms/perception/CMakeLists.txt

@@ -4,6 +4,7 @@ find_package(OpenCV REQUIRED)
 add_library(perception SHARED
         apriltag/src/tag_relative_target_3d.cpp
         apriltag/src/apriltag_perception.cpp
+        apriltag/src/tag_relative_tcp_pose.cpp
 )
 
 target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h

@@ -84,6 +84,10 @@ public:
 
         // Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
         Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
+
+        // Semantic alias used by visualization and downstream consumers:
+        // `T_C_Tag` maps points in this tag frame into camera frame C.
+        const Eigen::Matrix4d& T_C_Tag() const { return T_c_t; }
     };
 
     struct FrameCache {

cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h

@@ -0,0 +1,289 @@
+#pragma once
+
+#ifndef CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H
+#define CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H
+
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include "algorithms/perception/apriltag/include/apriltag_perception.h"
+
+namespace cmvr::perception {
+
+/**
+ * @brief 根据同一台相机同时观测到的 Screen Tag 和 Hand Tag，
+ *        计算 TCP 相对于 Screen Tag 的位姿。
+ *
+ * 坐标系：
+ *
+ *   C : 固定外部相机坐标系
+ *   G : Screen Tag 坐标系，同时作为屏幕参考坐标系
+ *   H : Hand Tag 坐标系
+ *   P : TCP / 触控点坐标系
+ *
+ * 已知：
+ *
+ *   T_C_G : Screen Tag -> Camera
+ *   T_C_H : Hand Tag   -> Camera
+ *   T_H_P : TCP        -> Hand Tag
+ *
+ * 其中 T_H_P 是外部传入的一次标定结果。
+ *
+ * 计算：
+ *
+ *   T_G_H = inverse(T_C_G) * T_C_H
+ *
+ *   T_G_P = T_G_H * T_H_P
+ *
+ * 即：
+ *
+ *   T_G_P = inverse(T_C_G) * T_C_H * T_H_P
+ *
+ * 最终：
+ *
+ *   p_G_P = T_G_P.block<3, 1>(0, 3)
+ *
+ * 得到 TCP 原点在 Screen Tag 坐标系下的位置。
+ *
+ * 注意：
+ * - 本类不主动抓相机图像；
+ * - 本类不主动调用 AprilTagPerception::update()；
+ * - 上层应保证当前 perception 缓存来自同一帧；
+ * - Screen Tag 和 Hand Tag 必须同时在当前帧可见。
+ */
+class TagRelativeTcpPose {
+public:
+    enum class Status {
+        OK = 0,
+
+        NO_PERCEPTION,
+
+        INVALID_SCREEN_TAG_ID,
+
+        INVALID_HAND_TAG_ID,
+
+        SAME_TAG_ID,
+
+        SCREEN_TAG_NOT_FOUND,
+
+        HAND_TAG_NOT_FOUND,
+
+        INVALID_T_C_G,
+
+        INVALID_T_C_H,
+
+        INVALID_T_H_P,
+
+        INVALID_T_G_H,
+
+        INVALID_T_G_P,
+
+        INVALID_TCP_POSITION
+    };
+
+public:
+    explicit TagRelativeTcpPose(
+        const std::shared_ptr<AprilTagPerception>& perception = nullptr);
+
+    /**
+     * @brief 设置 AprilTag 感知前端。
+     *
+     * 本类只读取感知缓存，不主动 update。
+     */
+    void setPerception(
+        const std::shared_ptr<AprilTagPerception>& perception);
+
+    const std::shared_ptr<AprilTagPerception>& perception() const {
+        return perception_;
+    }
+
+    /**
+     * @brief 设置 Screen Tag ID。
+     */
+    void setScreenTagId(int id);
+
+    /**
+     * @brief 设置 Hand Tag ID。
+     */
+    void setHandTagId(int id);
+
+    int screenTagId() const {
+        return screen_tag_id_;
+    }
+
+    int handTagId() const {
+        return hand_tag_id_;
+    }
+
+    /**
+     * @brief 使用当前 AprilTagPerception 缓存计算 TCP 相对 Screen Tag 的位姿。
+     *
+     * 核心公式：
+     *
+     *   T_G_P =
+     *       inverse(T_C_G)
+     *       * T_C_H
+     *       * T_H_P
+     *
+     * @param T_H_P
+     *        TCP(P) 相对于 Hand Tag(H) 的固定齐次变换。
+     *
+     *        坐标变换语义：
+     *
+     *          p_H = T_H_P * p_P
+     *
+     *        即：
+     *
+     *          ^H T_P
+     *
+     * @return 成功返回 true。
+     */
+    bool update(
+        const Eigen::Matrix4d& T_H_P);
+
+    /**
+     * @brief 当前结果是否有效。
+     */
+    bool valid() const {
+        return valid_;
+    }
+
+    /**
+     * @brief 最近一次 update() 的状态。
+     */
+    Status lastStatus() const {
+        return last_status_;
+    }
+
+    static const char* statusToString(Status status);
+
+    /**
+     * @brief 当前 Screen Tag 在 Camera 中的位姿。
+     *
+     * ^C T_G
+     */
+    const Eigen::Matrix4d& T_C_G() const {
+        return T_C_G_;
+    }
+
+    /**
+     * @brief 当前 Hand Tag 在 Camera 中的位姿。
+     *
+     * ^C T_H
+     */
+    const Eigen::Matrix4d& T_C_H() const {
+        return T_C_H_;
+    }
+
+    /**
+     * @brief Hand Tag 相对于 Screen Tag 的位姿。
+     *
+     * ^G T_H
+     */
+    const Eigen::Matrix4d& T_G_H() const {
+        return T_G_H_;
+    }
+
+    /**
+     * @brief TCP 相对于 Screen Tag 的完整 6DoF 位姿。
+     *
+     * ^G T_P
+     */
+    const Eigen::Matrix4d& T_G_P() const {
+        return T_G_P_;
+    }
+
+    /**
+     * @brief TCP 原点在 Screen Tag 坐标系中的位置。
+     *
+     * P_P^G =
+     *
+     * [ x_P ]
+     * [ y_P ]
+     * [ z_P ]
+     */
+    const Eigen::Vector3d& tcpPositionInScreenTag() const {
+        return p_G_P_;
+    }
+
+    /**
+     * @brief TCP 相对于 Screen Tag 的旋转矩阵。
+     *
+     * R_G_P
+     */
+    Eigen::Matrix3d tcpRotationInScreenTag() const {
+        return T_G_P_.block<3, 3>(0, 0);
+    }
+
+    /**
+     * @brief 清空当前结果。
+     */
+    void clear();
+
+private:
+    /**
+     * @brief 检查 4x4 矩阵元素是否全部有限。
+     */
+    static bool isFiniteTransform(
+        const Eigen::Matrix4d& T);
+
+    /**
+     * @brief 基础检查齐次矩阵最后一行。
+     */
+    static bool hasValidHomogeneousBottomRow(
+        const Eigen::Matrix4d& T,
+        double tolerance = 1e-6);
+
+    /**
+     * @brief 判断一个矩阵是否可以作为基本齐次变换使用。
+     *
+     * 当前只检查：
+     * - 所有元素 finite
+     * - 最后一行约等于 [0 0 0 1]
+     *
+     * 暂时不强制检查 rotation orthonormal，
+     * 避免视觉估计中的微小数值误差导致误判。
+     */
+    static bool isValidTransform(
+        const Eigen::Matrix4d& T);
+
+private:
+    std::shared_ptr<AprilTagPerception> perception_{nullptr};
+
+    int screen_tag_id_{-1};
+    int hand_tag_id_{-1};
+
+    // 当前外部相机观测
+    Eigen::Matrix4d T_C_G_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    Eigen::Matrix4d T_C_H_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    // 相对变换
+    Eigen::Matrix4d T_G_H_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    Eigen::Matrix4d T_G_P_{
+        Eigen::Matrix4d::Identity()
+    };
+
+    // TCP 原点在 Screen Tag 坐标系的位置
+    Eigen::Vector3d p_G_P_{
+        Eigen::Vector3d::Zero()
+    };
+
+    bool valid_{false};
+
+    Status last_status_{
+        Status::NO_PERCEPTION
+    };
+};
+
+} // namespace cmvr::perception
+
+#endif // CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H

cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp

@@ -0,0 +1,315 @@
+#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h"
+
+#include <cmath>
+
+namespace cmvr::perception {
+
+TagRelativeTcpPose::TagRelativeTcpPose(
+    const std::shared_ptr<AprilTagPerception>& perception)
+    : perception_(perception)
+{
+}
+
+void TagRelativeTcpPose::setPerception(
+    const std::shared_ptr<AprilTagPerception>& perception)
+{
+    perception_ = perception;
+    clear();
+
+    if (!perception_) {
+        last_status_ = Status::NO_PERCEPTION;
+    }
+}
+
+void TagRelativeTcpPose::setScreenTagId(
+    const int id)
+{
+    screen_tag_id_ = id;
+    valid_ = false;
+}
+
+void TagRelativeTcpPose::setHandTagId(
+    const int id)
+{
+    hand_tag_id_ = id;
+    valid_ = false;
+}
+
+bool TagRelativeTcpPose::update(
+    const Eigen::Matrix4d& T_H_P)
+{
+    valid_ = false;
+
+    /*
+     * 1. 检查 perception
+     */
+    if (!perception_) {
+        last_status_ = Status::NO_PERCEPTION;
+        return false;
+    }
+
+    /*
+     * 2. 检查 Tag ID
+     */
+    if (screen_tag_id_ < 0) {
+        last_status_ = Status::INVALID_SCREEN_TAG_ID;
+        return false;
+    }
+
+    if (hand_tag_id_ < 0) {
+        last_status_ = Status::INVALID_HAND_TAG_ID;
+        return false;
+    }
+
+    if (screen_tag_id_ == hand_tag_id_) {
+        last_status_ = Status::SAME_TAG_ID;
+        return false;
+    }
+
+    /*
+     * 3. 检查外部传入的：
+     *
+     *      ^H T_P
+     *
+     * Hand Tag -> TCP 固定标定矩阵。
+     */
+    if (!isValidTransform(T_H_P)) {
+        last_status_ = Status::INVALID_T_H_P;
+        return false;
+    }
+
+    /*
+     * 4. 从 AprilTagPerception 当前缓存取 Screen Tag。
+     *
+     * AprilTagPerception 已经通过 ViSP / PnP 得到：
+     *
+     *      ^C T_G
+     */
+    const auto* screen_tag =
+        perception_->findTag(screen_tag_id_);
+
+    if (!screen_tag) {
+        last_status_ =
+            Status::SCREEN_TAG_NOT_FOUND;
+        return false;
+    }
+
+    /*
+     * 5. 取 Hand Tag：
+     *
+     *      ^C T_H
+     */
+    const auto* hand_tag =
+        perception_->findTag(hand_tag_id_);
+
+    if (!hand_tag) {
+        last_status_ =
+            Status::HAND_TAG_NOT_FOUND;
+        return false;
+    }
+
+    /*
+     * 6. 保存当前帧两个原始视觉变换。
+     *
+     * AprilTagPerception::Tag::T_c_t
+     *
+     * 定义是：
+     *
+     *      Tag -> Camera
+     *
+     * 因此：
+     *
+     * screen tag:
+     *
+     *      ^C T_G
+     *
+     * hand tag:
+     *
+     *      ^C T_H
+     */
+    T_C_G_ = screen_tag->T_c_t;
+    T_C_H_ = hand_tag->T_c_t;
+
+    if (!isValidTransform(T_C_G_)) {
+        last_status_ = Status::INVALID_T_C_G;
+        return false;
+    }
+
+    if (!isValidTransform(T_C_H_)) {
+        last_status_ = Status::INVALID_T_C_H;
+        return false;
+    }
+
+    /*
+     * 7. 求 Hand Tag 相对于 Screen Tag 的位姿。
+     *
+     * 已知：
+     *
+     *      ^C T_G
+     *      ^C T_H
+     *
+     * 因此：
+     *
+     *      ^G T_H
+     *
+     *    = (^C T_G)^-1 * ^C T_H
+     */
+    T_G_H_ =
+        T_C_G_.inverse() *
+        T_C_H_;
+
+    if (!isValidTransform(T_G_H_)) {
+        last_status_ = Status::INVALID_T_G_H;
+        return false;
+    }
+
+    /*
+     * 8. 求 TCP 相对于 Screen Tag 的位姿。
+     *
+     * 已知：
+     *
+     *      ^G T_H
+     *      ^H T_P
+     *
+     * 因此：
+     *
+     *      ^G T_P
+     *
+     *    = ^G T_H * ^H T_P
+     *
+     *    = (^C T_G)^-1
+     *      * ^C T_H
+     *      * ^H T_P
+     */
+    T_G_P_ =
+        T_G_H_ *
+        T_H_P;
+
+    if (!isValidTransform(T_G_P_)) {
+        last_status_ = Status::INVALID_T_G_P;
+        return false;
+    }
+
+    /*
+     * 9. 提取 TCP 原点在 Screen Tag
+     *    坐标系 G 下的位置。
+     *
+     *      P_P^G =
+     *
+     *      [ x_P ]
+     *      [ y_P ]
+     *      [ z_P ]
+     *
+     */
+    p_G_P_ =
+        T_G_P_.block<3, 1>(0, 3);
+
+    if (!p_G_P_.allFinite()) {
+        last_status_ =
+            Status::INVALID_TCP_POSITION;
+        return false;
+    }
+
+    /*
+     * 10. 当前帧结果有效。
+     */
+    valid_ = true;
+    last_status_ = Status::OK;
+
+    return true;
+}
+
+void TagRelativeTcpPose::clear()
+{
+    T_C_G_.setIdentity();
+    T_C_H_.setIdentity();
+
+    T_G_H_.setIdentity();
+    T_G_P_.setIdentity();
+
+    p_G_P_.setZero();
+
+    valid_ = false;
+}
+
+const char* TagRelativeTcpPose::statusToString(
+    const Status status)
+{
+    switch (status) {
+
+        case Status::OK:
+            return "ok";
+
+        case Status::NO_PERCEPTION:
+            return "no_perception";
+
+        case Status::INVALID_SCREEN_TAG_ID:
+            return "invalid_screen_tag_id";
+
+        case Status::INVALID_HAND_TAG_ID:
+            return "invalid_hand_tag_id";
+
+        case Status::SAME_TAG_ID:
+            return "same_tag_id";
+
+        case Status::SCREEN_TAG_NOT_FOUND:
+            return "screen_tag_not_found";
+
+        case Status::HAND_TAG_NOT_FOUND:
+            return "hand_tag_not_found";
+
+        case Status::INVALID_T_C_G:
+            return "invalid_T_C_G";
+
+        case Status::INVALID_T_C_H:
+            return "invalid_T_C_H";
+
+        case Status::INVALID_T_H_P:
+            return "invalid_T_H_P";
+
+        case Status::INVALID_T_G_H:
+            return "invalid_T_G_H";
+
+        case Status::INVALID_T_G_P:
+            return "invalid_T_G_P";
+
+        case Status::INVALID_TCP_POSITION:
+            return "invalid_tcp_position";
+
+        default:
+            return "unknown";
+    }
+}
+
+bool TagRelativeTcpPose::isFiniteTransform(
+    const Eigen::Matrix4d& T)
+{
+    return T.allFinite();
+}
+
+bool TagRelativeTcpPose::hasValidHomogeneousBottomRow(
+    const Eigen::Matrix4d& T,
+    const double tolerance)
+{
+    return
+        std::abs(T(3, 0)) <= tolerance &&
+        std::abs(T(3, 1)) <= tolerance &&
+        std::abs(T(3, 2)) <= tolerance &&
+        std::abs(T(3, 3) - 1.0) <= tolerance;
+}
+
+bool TagRelativeTcpPose::isValidTransform(
+    const Eigen::Matrix4d& T)
+{
+    if (!isFiniteTransform(T)) {
+        return false;
+    }
+
+    if (!hasValidHomogeneousBottomRow(T)) {
+        return false;
+    }
+
+    return true;
+}
+
+} // namespace cmvr::perception

cmvr-es/common/media/ffmpeg/camera_capture.cpp

@@ -62,14 +62,14 @@ int CameraCapture::initialize(const Config& config) {
 }
 
 int CameraCapture::init_device() {
-    AVInputFormat* input_fmt = nullptr;
+    // AVInputFormat* input_fmt = nullptr;
     std::string device_path;
 
 #ifdef _WIN32
-    input_fmt = av_find_input_format("dshow");
+    auto input_fmt = av_find_input_format("dshow");
     device_path = "video=" + config_.device_name;
 #else
-    input_fmt = av_find_input_format("v4l2");
+    auto input_fmt = av_find_input_format("v4l2");
     device_path = config_.device_name;
 #endif
 

cmvr-es/common/media/media_frame.h

@@ -0,0 +1,244 @@
+#ifndef CMVR_ES_COMMON_MEDIA_MEDIA_FRAME_H
+#define CMVR_ES_COMMON_MEDIA_MEDIA_FRAME_H
+
+#pragma once
+
+#include <cstddef>
+#include <cstdint>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace cmvr::media {
+
+enum class MediaKind : uint8_t {
+    UNKNOWN = 0,
+    VIDEO = 1,
+    AUDIO = 2
+};
+
+enum class Codec : uint8_t {
+    UNKNOWN = 0,
+    H264 = 1,
+    H265 = 2,
+    OPUS = 3,
+    PCM_S16LE = 4,
+    AAC = 5
+};
+
+enum class PayloadFormat : uint8_t {
+    UNKNOWN = 0,
+    ANNEX_B = 1,
+    AVCC = 2,
+    RAW = 3,
+    OPUS_PACKET = 4,
+    AAC_ADTS = 5
+};
+
+struct Rational {
+    int32_t numerator{0};
+    int32_t denominator{1};
+
+    constexpr bool valid() const noexcept {
+        return numerator > 0 && denominator > 0;
+    }
+};
+
+constexpr bool operator==(const Rational lhs, const Rational rhs) noexcept {
+    return lhs.numerator == rhs.numerator && lhs.denominator == rhs.denominator;
+}
+
+constexpr bool operator!=(const Rational lhs, const Rational rhs) noexcept {
+    return !(lhs == rhs);
+}
+
+// A TrackDescriptor is immutable after construction. Reconfiguration is represented by
+// publishing a new descriptor with a larger generation and attaching it to later frames.
+class TrackDescriptor final {
+public:
+    struct Config {
+        std::string id;
+        std::string source_id;
+        MediaKind kind{MediaKind::UNKNOWN};
+        Codec codec{Codec::UNKNOWN};
+        PayloadFormat payload_format{PayloadFormat::UNKNOWN};
+        Rational time_base{};
+        uint32_t width{0};
+        uint32_t height{0};
+        uint32_t sample_rate{0};
+        uint32_t channels{0};
+        uint32_t nominal_rate{0};
+        float fx{0.0F};
+        float fy{0.0F};
+        float cx{0.0F};
+        float cy{0.0F};
+        std::vector<float> distortion;
+        uint64_t generation{1};
+        std::vector<uint8_t> codec_config;
+    };
+
+    explicit TrackDescriptor(Config config)
+        : id(std::move(config.id)),
+          source_id(std::move(config.source_id)),
+          kind(config.kind),
+          codec(config.codec),
+          payload_format(config.payload_format),
+          time_base(config.time_base),
+          width(config.width),
+          height(config.height),
+          sample_rate(config.sample_rate),
+          channels(config.channels),
+          nominal_rate(config.nominal_rate),
+          fx(config.fx),
+          fy(config.fy),
+          cx(config.cx),
+          cy(config.cy),
+          distortion(std::move(config.distortion)),
+          generation(config.generation),
+          codec_config(std::move(config.codec_config)) {
+        if (id.empty()) {
+            throw std::invalid_argument("TrackDescriptor id must not be empty");
+        }
+        if (source_id.empty()) {
+            throw std::invalid_argument("TrackDescriptor source_id must not be empty");
+        }
+        if (kind == MediaKind::UNKNOWN) {
+            throw std::invalid_argument("TrackDescriptor kind must not be UNKNOWN");
+        }
+        if (!time_base.valid()) {
+            throw std::invalid_argument("TrackDescriptor time_base is invalid");
+        }
+        if (generation == 0) {
+            throw std::invalid_argument("TrackDescriptor generation must be greater than zero");
+        }
+    }
+
+    const std::string id;
+    const std::string source_id;
+    const MediaKind kind;
+    const Codec codec;
+    const PayloadFormat payload_format;
+    const Rational time_base;
+    const uint32_t width;
+    const uint32_t height;
+    const uint32_t sample_rate;
+    const uint32_t channels;
+    const uint32_t nominal_rate;
+    const float fx;
+    const float fy;
+    const float cx;
+    const float cy;
+    const std::vector<float> distortion;
+    const uint64_t generation;
+    const std::vector<uint8_t> codec_config;
+};
+
+using TrackDescriptorPtr = std::shared_ptr<const TrackDescriptor>;
+
+inline bool equivalentTrackDescriptor(
+    const TrackDescriptor& lhs,
+    const TrackDescriptor& rhs) noexcept {
+    return lhs.id == rhs.id &&
+           lhs.source_id == rhs.source_id &&
+           lhs.kind == rhs.kind &&
+           lhs.codec == rhs.codec &&
+           lhs.payload_format == rhs.payload_format &&
+           lhs.time_base == rhs.time_base &&
+           lhs.width == rhs.width &&
+           lhs.height == rhs.height &&
+           lhs.sample_rate == rhs.sample_rate &&
+           lhs.channels == rhs.channels &&
+           lhs.nominal_rate == rhs.nominal_rate &&
+           lhs.fx == rhs.fx &&
+           lhs.fy == rhs.fy &&
+           lhs.cx == rhs.cx &&
+           lhs.cy == rhs.cy &&
+           lhs.distortion == rhs.distortion &&
+           lhs.generation == rhs.generation &&
+           lhs.codec_config == rhs.codec_config;
+}
+
+// MediaFrame and its payload are immutable and therefore safe to share across all protocol
+// adapters and consumers without copying. PTS/DTS use TrackDescriptor::time_base;
+// capture_time_ns is monotonic for pacing, while capture_utc_ns is optional wall time.
+class MediaFrame final {
+public:
+    using Payload = std::vector<uint8_t>;
+    using PayloadPtr = std::shared_ptr<const Payload>;
+
+    struct Config {
+        TrackDescriptorPtr descriptor;
+        Payload payload;
+        uint64_t sequence{0};
+        // Opaque producer-native timing/counter values. Their units and epoch
+        // are source-defined; zero means that the source did not provide them.
+        uint64_t source_timestamp{0};
+        uint64_t source_frame_number{0};
+        int64_t pts{0};
+        int64_t dts{0};
+        int64_t duration{0};
+        uint64_t capture_time_ns{0};
+        int64_t capture_utc_ns{0};
+        bool key_frame{false};
+        bool discontinuity{false};
+    };
+
+    explicit MediaFrame(Config config)
+        : descriptor(std::move(config.descriptor)),
+          payload(std::make_shared<const Payload>(std::move(config.payload))),
+          sequence(config.sequence),
+          source_timestamp(config.source_timestamp),
+          source_frame_number(config.source_frame_number),
+          pts(config.pts),
+          dts(config.dts),
+          duration(config.duration),
+          capture_time_ns(config.capture_time_ns),
+          capture_utc_ns(config.capture_utc_ns),
+          key_frame(config.key_frame),
+          discontinuity(config.discontinuity) {
+        if (!descriptor) {
+            throw std::invalid_argument("MediaFrame descriptor must not be null");
+        }
+    }
+
+    const uint8_t* data() const noexcept {
+        return payload->empty() ? nullptr : payload->data();
+    }
+
+    size_t size() const noexcept {
+        return payload->size();
+    }
+
+    bool empty() const noexcept {
+        return payload->empty();
+    }
+
+    const TrackDescriptorPtr descriptor;
+    const PayloadPtr payload;
+    const uint64_t sequence;
+    const uint64_t source_timestamp;
+    const uint64_t source_frame_number;
+    const int64_t pts;
+    const int64_t dts;
+    const int64_t duration;
+    const uint64_t capture_time_ns;
+    const int64_t capture_utc_ns;
+    const bool key_frame;
+    const bool discontinuity;
+};
+
+using MediaFramePtr = std::shared_ptr<const MediaFrame>;
+
+inline TrackDescriptorPtr makeTrackDescriptor(TrackDescriptor::Config config) {
+    return std::make_shared<const TrackDescriptor>(std::move(config));
+}
+
+inline MediaFramePtr makeMediaFrame(MediaFrame::Config config) {
+    return std::make_shared<const MediaFrame>(std::move(config));
+}
+
+} // namespace cmvr::media
+
+#endif // CMVR_ES_COMMON_MEDIA_MEDIA_FRAME_H

cmvr-es/config/devices/camera/camera.pb.txt

@@ -83,8 +83,8 @@ camera {
         stream_mode: STREAM_MODE_RGBD
       }
       encoder {
-        width: 480
+        width: 1280
-        height: 320
+        height: 720
         fps: 30
         codec: "H264"
         enable_stream_timestamp: true
@@ -92,7 +92,7 @@ camera {
       }
       consume_new_frame_only: false
       viewer_pip {
-        enable: false
+        enable: true
         left: -10
         bottom: 10
         width: 320
@@ -101,57 +101,7 @@ camera {
     }
   }
 
-  cameras {
-    id: "mujoco_external_touch_cam"
-    mujoco {
-      world_id: "mujoco_world"
-      camera_name: "external_touch_cam"
-      render {
-        width: 1280
-        height: 720
-        fps: 30
-        stream_mode: STREAM_MODE_RGBD
-      }
-      encoder {
-        width: 480
-        height: 320
-        fps: 30
-        codec: "H264"
-        enable_stream_timestamp: true
-        buffer_size: 30
-      }
-      consume_new_frame_only: false
-      viewer_pip {
-        enable: false
-        left: 10
-        bottom: 10
-        width: 320
-        height: 180
-      }
-    }
-  }
 
-  cameras {
-    id: "left_eye_cam"
-    uvc {
-      usb: "/dev/uvc_left_camera"
-      camera_mode: CAMERA_MODE_VIDEO
-      capture {
-        width: 640
-        height: 480
-        fps: 30
-        stream_mode: STREAM_MODE_RGB
-      }
-      encoder {
-        width: 640
-        height: 480
-        fps: 30
-        codec: "H265"
-        enable_stream_timestamp: true
-        buffer_size: 30
-      }
-    }
-  }
 
   cameras {
     id: "cam5"
@@ -176,4 +126,89 @@ camera {
       sync: false
     }
   }
+
+  cameras {
+      id: "hikvision_cam"
+      hikvision {
+        ip: "192.168.192.64"
+        port: 8000
+        username: "admin"
+        password: "okwy1688"
+        channel: 1
+        stream_type: 0
+        link_mode: 0
+        width: 1920
+        height: 1080
+        fps: 25
+        codec: "H264"
+        camera_mode: CAMERA_MODE_VIDEO
+        stream_mode: STREAM_MODE_RGB
+        buffer_size: 30
+      }
+    }
+   cameras {
+        id: "hikvision_thermal_cam"
+        hikvision {
+          ip: "192.168.192.65"
+          port: 8000
+          username: "admin"
+          password: "okwy1688"
+          channel: 1
+          stream_type: 0
+          link_mode: 0
+          width: 384
+          height: 288
+          fps: 50
+          codec: "H264"
+          camera_mode: CAMERA_MODE_VIDEO
+          stream_mode: STREAM_MODE_RGB
+          buffer_size: 30
+        }
+      }
+
+   cameras {
+       id: "real_cam1"
+       realsense {
+         serialNumber: "332522076896"
+         camera_mode: CAMERA_MODE_VIDEO
+         capture {
+           width: 640
+           height: 480
+           fps: 30
+           stream_mode: STREAM_MODE_RGBD
+         }
+         encoder {
+           width: 640
+           height: 480
+           fps: 30
+           codec: "h265_qsv"
+           enable_stream_timestamp: true
+           buffer_size: 30
+         }
+         align_mode: ALIGN_MODE_COLOR
+         sync: false
+       }
+     }
+
+     cameras {
+         id: "usb_cam1"
+         uvc {
+           usb: "/dev/video0"
+           camera_mode: CAMERA_MODE_VIDEO
+           capture {
+             width: 640
+             height: 480
+             fps: 30
+             stream_mode: STREAM_MODE_RGB
+           }
+           encoder {
+             width: 640
+             height: 480
+             fps: 30
+             codec: "h265_qsv"
+             enable_stream_timestamp: true
+             buffer_size: 30
+           }
+         }
+       }
 }

cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp

@@ -385,7 +385,7 @@ Result AuboArm::stopMotion()
         }
         auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
         if (robot_interface) {
-            robot_interface->getMotionControl()->stopMove();
+            robot_interface->getMotionControl()->stopMove(true, true);
         }
         busy_.store(false);
         return Result::success();

cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h

@@ -131,6 +131,7 @@ private:
     std::shared_ptr<JointMotionPlanner> joint_planner_{nullptr};
     std::shared_ptr<CartesianMotionPlanner> cartesian_planner_{nullptr};
     std::unique_ptr<CartesianVelocityController> cartesian_velocity_controller_{nullptr};
+    double cartesian_stop_timeout_s_{2.0};
 
     // MoveJ post-trajectory settling criteria.  These defaults preserve the
     // historical behavior when the optional arm configuration fields are absent.

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp

@@ -1013,8 +1013,7 @@ Result MotorRobotArm::stopCartesianMotionAndWait_()
         }
 
         const auto deadline = std::chrono::steady_clock::now() +
-                              std::chrono::duration<double>(
+                              std::chrono::duration<double>(cartesian_stop_timeout_s_);
-                                  cartesian_velocity_controller_->stopTimeoutS());
         while (cartesian_velocity_controller_->busy() &&
                std::chrono::steady_clock::now() < deadline) {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
@@ -1135,6 +1134,9 @@ bool MotorRobotArm::configureAlgorithms_()
                         << cartesian_controller_config.stop_timeout_s();
         return false;
     }
+    if (cartesian_controller_config.has_stop_timeout_s()) {
+        cartesian_stop_timeout_s_ = cartesian_controller_config.stop_timeout_s();
+    }
 
     joint_planner_ = JointMotionPlannerFactory::create(cfg_.motion().move_j());
     if (!joint_planner_) {
@@ -1248,10 +1250,6 @@ CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityController
     result.stop_acceleration =
         config.stop_acceleration() > 0.0 ? config.stop_acceleration()
                                          : result.stop_acceleration;
-    if (config.has_stop_timeout_s() && std::isfinite(config.stop_timeout_s()) &&
-        config.stop_timeout_s() > 0.0) {
-        result.stop_timeout_s = config.stop_timeout_s();
-    }
     return result;
 }
 

cmvr-es/devices/camera/CMakeLists.txt

@@ -3,7 +3,7 @@ add_subdirectory(common)
 add_subdirectory(uvc_camera)
 add_subdirectory(realsense_camera)
 add_subdirectory(mujoco_camera)
-
+add_subdirectory(hikvision_camera)
 add_library(camera INTERFACE)
 
 target_include_directories(camera INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
@@ -14,6 +14,7 @@ target_link_libraries(camera
         cmvr_es::device::realsense_camera
         cmvr_es::device::mujoco_camera
         cmvr_es::device::camera_stream_encoder
+        cmvr_es::device::hikvision_camera
         cmvr_es::proto
 )
 

cmvr-es/devices/camera/abstract_camera.h

@@ -2,8 +2,13 @@
 #define CMVR_ES_ABSTRACT_CAMERA_H
 #pragma once
 
-#include <opencv2/opencv.hpp>
+#include <cstdint>
+#include <chrono>
 #include <mutex>
+#include <string>
+#include <vector>
+
+#include <opencv2/opencv.hpp>
 #include "../abstract_device.h"
 #include <Eigen/Core>
 #include "cmvr/config/camera_config/camera_config.pb.h"
@@ -14,11 +19,11 @@ namespace cmvr::device {
 
     struct Rs2Intrinsics
     {
-        float cx{0.0F};
+        float   cx;
-        float cy{0.0F};
+        float   cy;
-        float fx{0.0F};
+        float   fx;
-        float fy{0.0F};
+        float   fy;
-        float coeffs[5]{};
+        float   coeffs[5];
 
     };
     struct StreamFrameData
@@ -62,6 +67,21 @@ namespace cmvr::device {
         uint32_t codec_config_generation = 0;
         std::vector<uint8_t> codec_config;
     };
+
+    enum class PtzCommand {
+        TiltUp,
+        TiltDown,
+        PanLeft,
+        PanRight,
+        UpLeft,
+        UpRight,
+        DownLeft,
+        DownRight,
+        ZoomIn,
+        ZoomOut,
+        PanAuto
+    };
+
     class AbstractCamera : public AbstractDevice {
     public:
         // 录制状态
@@ -75,7 +95,25 @@ namespace cmvr::device {
         ~AbstractCamera() override = default;
 
         DeviceKind kind() const noexcept override { return DeviceKind::Camera; }
-        inline void getState(CameraState &state) {state = state_;}
+        // Every implementation must take the same lock used by its state_
+        // writers; CameraState contains std::string and cannot be snapshotted
+        // safely while another thread mutates it.
+        virtual void getState(CameraState &state) = 0;
+        DeviceHealthSnapshot healthSnapshot() override {
+            CameraState state{};
+            getState(state);
+
+            DeviceHealthSnapshot health;
+            health.error_message = state.error_message;
+            if (state.is_error) {
+                health.state = DeviceHealthState::Fault;
+            } else if (!state.error_message.empty()) {
+                health.state = DeviceHealthState::Degraded;
+            } else if (state.is_initialized) {
+                health.state = DeviceHealthState::Healthy;
+            }
+            return health;
+        }
         virtual void getRGBImage(cv::Mat &color, Rs2Intrinsics& intrinsics) {}
         virtual void getDepthImage(cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
         virtual void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
@@ -84,13 +122,18 @@ namespace cmvr::device {
         virtual void pauseRecording() {}
         virtual void resumeRecording() {}
         virtual void getEncodedFrame(StreamFrameData& frame_data, size_t& index) {}
+        virtual bool waitEncodedFrame(
+            StreamFrameData& frame_data,
+            size_t& index,
+            std::chrono::milliseconds timeout) {
+            (void)timeout;
+            getEncodedFrame(frame_data, index);
+            return !frame_data.rgbFrame.empty() || !frame_data.depthFrame.empty();
+        }
         virtual bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) {
             return false;
         }
 
-        // Video overlay is a presentation-only snapshot.  It is deliberately
-        // kept on the camera so the encoding thread can consume it without
-        // coupling the camera to AprilTag or task code.
         void setStreamOverlay(const CameraStreamOverlay& overlay) {
             std::lock_guard<std::mutex> lock(stream_overlay_mutex_);
             stream_overlay_ = overlay;
@@ -103,6 +146,13 @@ namespace cmvr::device {
 
         virtual bool startStreaming() {return true;}
         virtual void stopStreaming() {}
+        virtual bool controlPtz(PtzCommand command, bool stop, int speed) {
+            (void)command;
+            (void)stop;
+            (void)speed;
+            return false;
+        }
+        virtual bool requestKeyFrame() { return false; }
         virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
     protected:
         CameraState state_{};

cmvr-es/devices/camera/camera_factory.h

@@ -9,6 +9,7 @@
 #include "common/base/logging/logger.h"
 #include "devices/camera/abstract_camera.h"
 #include "devices/camera/mujoco_camera/include/mujoco_camera.h"
+#include "devices/camera/hikvision_camera/include/hikvision_camera.h"
 #include "devices/camera/realsense_camera/include/realsense_camera.h"
 #include "devices/camera/uvc_camera/include/uvc_camera.h"
 
@@ -41,6 +42,10 @@ public:
                 return nullptr;
             }
 
+            case config::CameraDeviceConfig::kHikvision:
+                return std::make_shared<HikvisionCamera>(
+                    backendWithId_(cfg.id(), cfg.hikvision()));
+
             case config::CameraDeviceConfig::kMujoco:
                 return std::make_shared<MujocoCamera>(
                     backendWithId_(cfg.id(), cfg.mujoco()));

cmvr-es/devices/camera/common/include/camera_stream_encoder.h

@@ -7,12 +7,9 @@
 #include <opencv2/opencv.hpp>
 
 #include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
-#include "devices/camera/common/include/camera_stream_overlay.h"
 
 namespace cmvr::device {
 
-struct Rs2Intrinsics;
-
 struct FfmpegEncoderInfo {
     std::string codec_name;
     int width = 0;
@@ -22,6 +19,7 @@ struct FfmpegEncoderInfo {
     bool bRunning = false;
     AVCodecContext* codec_context = nullptr;
     AVFrame* frame = nullptr;
+    AVFrame* transfer_frame = nullptr;
     AVPacket* packet = nullptr;
     SwsContext* sws_context = nullptr;
 
@@ -30,23 +28,8 @@ struct FfmpegEncoderInfo {
 
 struct CameraStreamEncodeOptions {
     bool draw_timestamp = false;
-    CameraStreamOverlay overlay;
 };
 
-// Draw a frame whose pose is expressed as ^C T_Frame onto a BGR/BGRA image.
-// The image is modified in place and no camera/perception state is touched.
-void drawCoordinateFrame(cv::Mat& image,
-                         const Eigen::Matrix4d& T_C_Frame,
-                         const Rs2Intrinsics& intrinsics,
-                         double axis_length_m,
-                         const std::string& label);
-
-Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
-                              int source_width,
-                              int source_height,
-                              int target_width,
-                              int target_height);
-
 class CameraStreamEncoder {
 public:
     static bool init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
@@ -59,12 +42,10 @@ public:
                        const cv::Mat& frame,
                        std::vector<uint8_t>& encoded_frame,
                        bool& is_key,
-                       const Rs2Intrinsics& intrinsics,
                        const CameraStreamEncodeOptions& options = {});
 
-    // Compatibility overload for callers that only need timestamp drawing.
     static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
-                       const cv::Mat& frame,
+                       const AVFrame* frame,
                        std::vector<uint8_t>& encoded_frame,
                        bool& is_key,
                        const CameraStreamEncodeOptions& options = {});

cmvr-es/devices/camera/common/include/camera_stream_overlay.h

@@ -7,8 +7,6 @@
 
 namespace cmvr::device {
 
-// A pose snapshot used only by the encoded video overlay.  The transform is
-// ^C T_Frame: it maps points in the named frame into the camera frame.
 struct CoordinateFrameOverlay {
     Eigen::Matrix4d T_C_Frame{Eigen::Matrix4d::Identity()};
     std::string label;

cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp

@@ -1,16 +1,16 @@
 #include "devices/camera/common/include/camera_stream_encoder.h"
 
 #include <chrono>
-#include <cmath>
 #include <ctime>
 #include <iomanip>
 #include <sstream>
 
 #include <libavutil/opt.h>
+#include <libavutil/pixdesc.h>
+#include <libavutil/hwcontext.h>
 #include <opencv2/imgproc.hpp>
 
 #include "common/base/logging/logger.h"
-#include "devices/camera/abstract_camera.h"
 
 namespace cmvr::device {
 namespace {
@@ -48,43 +48,15 @@ void drawTimeStamp(cv::Mat& image)
     cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness);
 }
 
-bool projectPoint(const Eigen::Vector3d& point,
-                 const Rs2Intrinsics& intrinsics,
-                 cv::Point& pixel)
-{
-    if (!point.allFinite() || point.z() <= 1e-9 ||
-        !std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
-        !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
-        intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) {
-        return false;
-    }
-    const double u = static_cast<double>(intrinsics.fx) * point.x() / point.z() +
-                     static_cast<double>(intrinsics.cx);
-    const double v = static_cast<double>(intrinsics.fy) * point.y() / point.z() +
-                     static_cast<double>(intrinsics.cy);
-    if (!std::isfinite(u) || !std::isfinite(v)) {
-        return false;
-    }
-    pixel = cv::Point(cvRound(u), cvRound(v));
-    return true;
-}
-
-void drawOutlinedText(cv::Mat& image,
-                      const std::string& text,
-                      const cv::Point& origin,
-                      const cv::Scalar& color)
-{
-    constexpr int font_face = cv::FONT_HERSHEY_SIMPLEX;
-    constexpr double font_scale = 0.55;
-    constexpr int thickness = 1;
-    cv::putText(image, text, origin, font_face, font_scale,
-                cv::Scalar(0, 0, 0), thickness + 2, cv::LINE_AA);
-    cv::putText(image, text, origin, font_face, font_scale,
-                color, thickness, cv::LINE_AA);
-}
-
 const AVCodec* findEncoder(const std::string& codec_name)
 {
+    if (codec_name == "h264_qsv" || codec_name == "H264_QSV") {
+        return avcodec_find_encoder_by_name("h264_qsv");
+    }
+    if (codec_name == "hevc_qsv" || codec_name == "h265_qsv" ||
+        codec_name == "HEVC_QSV" || codec_name == "H265_QSV") {
+        return avcodec_find_encoder_by_name("hevc_qsv");
+    }
     if (codec_name == "h264" || codec_name == "H264") {
         const AVCodec* codec = avcodec_find_encoder_by_name("libx264");
         return codec ? codec : avcodec_find_encoder(AV_CODEC_ID_H264);
@@ -110,85 +82,67 @@ AVPixelFormat sourcePixelFormat(const cv::Mat& frame)
     return AV_PIX_FMT_NONE;
 }
 
+bool encodePreparedFrame(FfmpegEncoderInfo& encoder,
+                         std::vector<uint8_t>& encoded_frame,
+                         bool& is_key)
+{
+    encoder.frame->pts = encoder.frame_pts++;
+    int ret = avcodec_send_frame(encoder.codec_context, encoder.frame);
+    if (ret < 0) {
+        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+        av_strerror(ret, errbuf, sizeof(errbuf));
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] error sending frame to encoder: " << errbuf;
+        return false;
+    }
+
+    while (true) {
+        ret = avcodec_receive_packet(encoder.codec_context, encoder.packet);
+        if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
+            break;
+        }
+        if (ret < 0) {
+            char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+            av_strerror(ret, errbuf, sizeof(errbuf));
+            CMVR_LOG(ERROR) << "[CameraStreamEncoder] error receiving packet from encoder: " << errbuf;
+            return false;
+        }
+
+        if (encoder.packet->flags & AV_PKT_FLAG_KEY) {
+            is_key = true;
+        }
+        encoded_frame.reserve(encoded_frame.size() + encoder.packet->size);
+        encoded_frame.insert(encoded_frame.end(),
+                             encoder.packet->data,
+                             encoder.packet->data + encoder.packet->size);
+        av_packet_unref(encoder.packet);
+    }
+
+    if (encoded_frame.size() < 4) {
+        return false;
+    }
+
+    const bool has_start_code =
+        (encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 1) ||
+        (encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 0 && encoded_frame[3] == 1);
+    if (!has_start_code) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] invalid frame: no NALU start code";
+        return false;
+    }
+    return true;
+}
+
 } // namespace
 
-void drawCoordinateFrame(cv::Mat& image,
-                         const Eigen::Matrix4d& T_C_Frame,
-                         const Rs2Intrinsics& intrinsics,
-                         const double axis_length_m,
-                         const std::string& label)
-{
-    if (image.empty() || image.channels() < 3 || !T_C_Frame.allFinite() ||
-        !std::isfinite(axis_length_m) || axis_length_m <= 0.0) {
-        return;
-    }
-
-    const Eigen::Vector4d origin_h(0.0, 0.0, 0.0, 1.0);
-    const Eigen::Vector4d x_h(axis_length_m, 0.0, 0.0, 1.0);
-    const Eigen::Vector4d y_h(0.0, axis_length_m, 0.0, 1.0);
-    const Eigen::Vector4d z_h(0.0, 0.0, axis_length_m, 1.0);
-    const Eigen::Vector3d origin = (T_C_Frame * origin_h).head<3>();
-    const Eigen::Vector3d x = (T_C_Frame * x_h).head<3>();
-    const Eigen::Vector3d y = (T_C_Frame * y_h).head<3>();
-    const Eigen::Vector3d z = (T_C_Frame * z_h).head<3>();
-
-    cv::Point origin_px;
-    if (!projectPoint(origin, intrinsics, origin_px)) {
-        return;
-    }
-
-    cv::Point x_px;
-    cv::Point y_px;
-    cv::Point z_px;
-    constexpr int thickness = 2;
-    if (projectPoint(x, intrinsics, x_px)) {
-        cv::arrowedLine(image, origin_px, x_px, cv::Scalar(0, 0, 255), thickness,
-                        cv::LINE_AA, 0, 0.15);
-        drawOutlinedText(image, "X", x_px + cv::Point(4, -4), cv::Scalar(0, 0, 255));
-    }
-    if (projectPoint(y, intrinsics, y_px)) {
-        cv::arrowedLine(image, origin_px, y_px, cv::Scalar(0, 255, 0), thickness,
-                        cv::LINE_AA, 0, 0.15);
-        drawOutlinedText(image, "Y", y_px + cv::Point(4, -4), cv::Scalar(0, 255, 0));
-    }
-    if (projectPoint(z, intrinsics, z_px)) {
-        cv::arrowedLine(image, origin_px, z_px, cv::Scalar(255, 0, 0), thickness,
-                        cv::LINE_AA, 0, 0.15);
-        drawOutlinedText(image, "Z", z_px + cv::Point(4, -4), cv::Scalar(255, 0, 0));
-    }
-
-    cv::drawMarker(image, origin_px, cv::Scalar(255, 255, 255), cv::MARKER_CROSS, 9, 1,
-                   cv::LINE_AA);
-    if (!label.empty()) {
-        drawOutlinedText(image, label, origin_px + cv::Point(7, -7),
-                         cv::Scalar(255, 255, 255));
-    }
-}
-
-Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
-                              const int source_width,
-                              const int source_height,
-                              const int target_width,
-                              const int target_height)
-{
-    Rs2Intrinsics scaled = intrinsics;
-    if (source_width > 0 && source_height > 0 && target_width > 0 && target_height > 0) {
-        const float sx = static_cast<float>(target_width) / static_cast<float>(source_width);
-        const float sy = static_cast<float>(target_height) / static_cast<float>(source_height);
-        scaled.fx *= sx;
-        scaled.cx *= sx;
-        scaled.fy *= sy;
-        scaled.cy *= sy;
-    }
-    return scaled;
-}
-
 FfmpegEncoderInfo::~FfmpegEncoderInfo()
 {
     if (frame) {
         av_frame_free(&frame);
         frame = nullptr;
     }
+    if (transfer_frame) {
+        av_frame_free(&transfer_frame);
+        transfer_frame = nullptr;
+    }
     if (packet) {
         av_packet_free(&packet);
         packet = nullptr;
@@ -237,7 +191,17 @@ bool CameraStreamEncoder::init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
     ctx->max_b_frames = 0;
     ctx->gop_size = 10;
 
-    if (codec->id == AV_CODEC_ID_H264) {
+    const bool is_qsv = codec->name &&
+        (std::string(codec->name) == "h264_qsv" || std::string(codec->name) == "hevc_qsv");
+
+    if (is_qsv) {
+        // Feed system-memory NV12 frames to oneVPL/QSV. The QSV encoder
+        // performs the upload and hardware encoding internally.
+        ctx->pix_fmt = AV_PIX_FMT_NV12;
+        ctx->bit_rate = 4'000'000;
+        av_opt_set_int(ctx->priv_data, "async_depth", 1, 0);
+        av_opt_set_int(ctx->priv_data, "repeat_pps", 1, 0);
+    } else if (codec->id == AV_CODEC_ID_H264) {
         av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
         av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
         av_opt_set(ctx->priv_data, "profile", "baseline", 0);
@@ -252,12 +216,14 @@ bool CameraStreamEncoder::init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
         av_opt_set(ctx->priv_data, "no-open-gop", "1", 0);
     }
 
+    if (!is_qsv) {
         const AVPixelFormat* pix_fmts = codec->pix_fmts;
         if (!pix_fmts) {
             ctx->pix_fmt = AV_PIX_FMT_YUV420P;
         } else {
             ctx->pix_fmt = pix_fmts[0];
         }
+    }
 
     const int open_ret = avcodec_open2(ctx, codec, nullptr);
     if (open_ret < 0) {
@@ -287,9 +253,11 @@ bool CameraStreamEncoder::init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
         return false;
     }
 
-    CMVR_LOG(INFO) << "[CameraStreamEncoder] initialized " << codec_name
+    const char* pixel_format_name = av_get_pix_fmt_name(ctx->pix_fmt);
+    CMVR_LOG(INFO) << "[CameraStreamEncoder] initialized " << codec->name
                    << " encoder, size=" << width << "x" << height
-                   << ", fps=" << fps;
+                   << ", fps=" << fps
+                   << ", pixel_format=" << (pixel_format_name ? pixel_format_name : "unknown");
     return true;
 }
 
@@ -297,7 +265,6 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
                                  const cv::Mat& frame,
                                  std::vector<uint8_t>& encoded_frame,
                                  bool& is_key,
-                                 const Rs2Intrinsics& intrinsics,
                                  const CameraStreamEncodeOptions& options)
 {
     encoded_frame.clear();
@@ -314,28 +281,10 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
     }
 
     cv::Mat frame_to_encode = frame;
-    if (options.draw_timestamp || options.overlay.draw_coordinate_frames) {
-        frame_to_encode = frame.clone();
     if (options.draw_timestamp) {
+        frame_to_encode = frame.clone();
         drawTimeStamp(frame_to_encode);
     }
-        if (options.overlay.draw_coordinate_frames) {
-            for (const auto& coordinate_frame : options.overlay.coordinate_frames) {
-                if (!coordinate_frame.valid) {
-                    continue;
-                }
-                std::string label = coordinate_frame.label;
-                if (coordinate_frame.tag_id >= 0) {
-                    label += " #" + std::to_string(coordinate_frame.tag_id);
-                }
-                drawCoordinateFrame(frame_to_encode,
-                                    coordinate_frame.T_C_Frame,
-                                    intrinsics,
-                                    options.overlay.coordinate_axis_length_m,
-                                    label);
-            }
-        }
-    }
 
     const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode);
     if (src_pix_fmt == AV_PIX_FMT_NONE) {
@@ -343,10 +292,8 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
         return false;
     }
 
-    if (encoder->sws_context) {
+    encoder->sws_context = sws_getCachedContext(encoder->sws_context,
-        sws_freeContext(encoder->sws_context);
+                                                frame_to_encode.cols,
-    }
-    encoder->sws_context = sws_getContext(frame_to_encode.cols,
                                                 frame_to_encode.rows,
                                                 src_pix_fmt,
                                                 encoder->codec_context->width,
@@ -361,6 +308,14 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
         return false;
     }
 
+    const int writable_ret = av_frame_make_writable(encoder->frame);
+    if (writable_ret < 0) {
+        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+        av_strerror(writable_ret, errbuf, sizeof(errbuf));
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] frame is not writable: " << errbuf;
+        return false;
+    }
+
     const uint8_t* src_data[AV_NUM_DATA_POINTERS] = {frame_to_encode.data};
     int src_linesize[AV_NUM_DATA_POINTERS] = {static_cast<int>(frame_to_encode.step)};
 
@@ -376,59 +331,125 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
         return false;
     }
 
-    encoder->frame->pts = encoder->frame_pts++;
+    return encodePreparedFrame(*encoder, encoded_frame, is_key);
-    int ret = avcodec_send_frame(encoder->codec_context, encoder->frame);
-    if (ret < 0) {
-        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
-        av_strerror(ret, errbuf, sizeof(errbuf));
-        CMVR_LOG(ERROR) << "[CameraStreamEncoder] error sending frame to encoder: " << errbuf;
-        return false;
-    }
-
-    while (true) {
-        ret = avcodec_receive_packet(encoder->codec_context, encoder->packet);
-        if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
-            break;
-        }
-        if (ret < 0) {
-            char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
-            av_strerror(ret, errbuf, sizeof(errbuf));
-            CMVR_LOG(ERROR) << "[CameraStreamEncoder] error receiving packet from encoder: " << errbuf;
-            break;
-        }
-
-        if (encoder->packet->flags & AV_PKT_FLAG_KEY) {
-            is_key = true;
-        }
-        encoded_frame.reserve(encoded_frame.size() + encoder->packet->size);
-        encoded_frame.insert(encoded_frame.end(),
-                             encoder->packet->data,
-                             encoder->packet->data + encoder->packet->size);
-        av_packet_unref(encoder->packet);
-    }
-
-    if (encoded_frame.size() < 4) {
-        return false;
-    }
-
-    const bool has_start_code =
-        (encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 1) ||
-        (encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 0 && encoded_frame[3] == 1);
-    if (!has_start_code) {
-        CMVR_LOG(ERROR) << "[CameraStreamEncoder] invalid frame: no NALU start code";
-        return false;
-    }
-    return true;
 }
 
 bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
-                                 const cv::Mat& frame,
+                                 const AVFrame* frame,
                                  std::vector<uint8_t>& encoded_frame,
                                  bool& is_key,
                                  const CameraStreamEncodeOptions& options)
 {
-    Rs2Intrinsics intrinsics{};
+    encoded_frame.clear();
-    return encode(encoder, frame, encoded_frame, is_key, intrinsics, options);
+    is_key = false;
+    if (!encoder || !encoder->codec_context || !encoder->frame || !encoder->packet || !frame) {
+        return false;
+    }
+
+    const AVFrame* source_frame = frame;
+    if (frame->hw_frames_ctx) {
+        if (!encoder->transfer_frame) {
+            encoder->transfer_frame = av_frame_alloc();
+        }
+        if (!encoder->transfer_frame) {
+            return false;
+        }
+        av_frame_unref(encoder->transfer_frame);
+        const int transfer_ret = av_hwframe_transfer_data(encoder->transfer_frame, frame, 0);
+        if (transfer_ret < 0) {
+            char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+            av_strerror(transfer_ret, errbuf, sizeof(errbuf));
+            CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to download hardware frame: " << errbuf;
+            return false;
+        }
+        const int props_ret = av_frame_copy_props(encoder->transfer_frame, frame);
+        if (props_ret < 0) {
+            char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+            av_strerror(props_ret, errbuf, sizeof(errbuf));
+            CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to copy hardware frame properties: " << errbuf;
+            return false;
+        }
+        source_frame = encoder->transfer_frame;
+    }
+
+    const auto source_format = static_cast<AVPixelFormat>(source_frame->format);
+    if (source_format == AV_PIX_FMT_NONE) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] source frame has no pixel format";
+        return false;
+    }
+
+    const int writable_ret = av_frame_make_writable(encoder->frame);
+    if (writable_ret < 0) {
+        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+        av_strerror(writable_ret, errbuf, sizeof(errbuf));
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] frame is not writable: " << errbuf;
+        return false;
+    }
+
+    int copy_ret = 0;
+    if (source_frame->width == encoder->codec_context->width
+        && source_frame->height == encoder->codec_context->height
+        && source_format == encoder->codec_context->pix_fmt) {
+        copy_ret = av_frame_copy(encoder->frame, source_frame);
+    } else {
+        encoder->sws_context = sws_getCachedContext(
+            encoder->sws_context,
+            source_frame->width,
+            source_frame->height,
+            source_format,
+            encoder->codec_context->width,
+            encoder->codec_context->height,
+            encoder->codec_context->pix_fmt,
+            SWS_BILINEAR,
+            nullptr,
+            nullptr,
+            nullptr);
+        if (!encoder->sws_context) {
+            CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to create native frame conversion context";
+            return false;
+        }
+        const int scaled_rows = sws_scale(
+            encoder->sws_context,
+            source_frame->data,
+            source_frame->linesize,
+            0,
+            source_frame->height,
+            encoder->frame->data,
+            encoder->frame->linesize);
+        copy_ret = scaled_rows == encoder->codec_context->height
+            ? 0
+            : scaled_rows < 0 ? scaled_rows : AVERROR_INVALIDDATA;
+    }
+    if (copy_ret < 0) {
+        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+        av_strerror(copy_ret, errbuf, sizeof(errbuf));
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to copy native frame: " << errbuf;
+        return false;
+    }
+
+    if (options.draw_timestamp) {
+        const auto output_format = static_cast<AVPixelFormat>(encoder->frame->format);
+        if (output_format != AV_PIX_FMT_NV12 && output_format != AV_PIX_FMT_YUV420P) {
+            const char* format_name = av_get_pix_fmt_name(output_format);
+            CMVR_LOG(ERROR) << "[CameraStreamEncoder] cannot draw timestamp on pixel format "
+                            << (format_name ? format_name : "unknown");
+            return false;
+        }
+        if (!encoder->frame->data[0]
+            || encoder->frame->linesize[0] < encoder->frame->width) {
+            CMVR_LOG(ERROR) << "[CameraStreamEncoder] invalid luma plane for timestamp";
+            return false;
+        }
+        cv::Mat luma_plane(
+            encoder->frame->height,
+            encoder->frame->width,
+            CV_8UC1,
+            encoder->frame->data[0],
+            static_cast<size_t>(encoder->frame->linesize[0]));
+        drawTimeStamp(luma_plane);
+    }
+
+    return encodePreparedFrame(*encoder, encoded_frame, is_key);
 }
 
 } // namespace cmvr::device

cmvr-es/devices/camera/hikvision_camera/CMakeLists.txt

@@ -0,0 +1,122 @@
+add_library(hikvision_camera SHARED src/hikvision_camera.cpp)
+
+target_include_directories(hikvision_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+set(HIKVISION_SDK_ROOT "${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/hikvision_sdk/v6.1.11.5")
+set(HIKVISION_SDK_LIB_DIR "${HIKVISION_SDK_ROOT}/lib")
+set(HIKVISION_SDK_COM_DIR "${HIKVISION_SDK_LIB_DIR}/HCNetSDKCom")
+
+file(GLOB HIKVISION_SDK_LIBS CONFIGURE_DEPENDS
+        "${HIKVISION_SDK_LIB_DIR}/*.so"
+        "${HIKVISION_SDK_LIB_DIR}/*.so.*"
+)
+file(GLOB HIKVISION_SDK_COM_LIBS CONFIGURE_DEPENDS
+        "${HIKVISION_SDK_COM_DIR}/*.so"
+        "${HIKVISION_SDK_COM_DIR}/*.so.*"
+)
+set(HIKVISION_HCNETSDK_LIB "${HIKVISION_SDK_LIB_DIR}/libhcnetsdk.so")
+
+if(NOT HIKVISION_SDK_LIBS)
+    message(FATAL_ERROR "Hikvision SDK libraries not found: ${HIKVISION_SDK_LIB_DIR}")
+endif()
+if(NOT HIKVISION_SDK_COM_LIBS)
+    message(FATAL_ERROR "Hikvision SDK component libraries not found: ${HIKVISION_SDK_COM_DIR}")
+endif()
+if(NOT EXISTS "${HIKVISION_HCNETSDK_LIB}")
+    message(FATAL_ERROR "Hikvision HCNetSDK library not found: ${HIKVISION_HCNETSDK_LIB}")
+endif()
+
+target_include_directories(hikvision_camera PRIVATE "${HIKVISION_SDK_ROOT}/include")
+target_link_directories(hikvision_camera PRIVATE "${HIKVISION_SDK_LIB_DIR}" "${HIKVISION_SDK_COM_DIR}")
+
+set(HIKVISION_JSONCPP_LINK jsoncpp)
+if(TARGET jsoncpp)
+    get_target_property(HIKVISION_JSONCPP_IMPORTED_LOCATION jsoncpp IMPORTED_LOCATION)
+    get_target_property(HIKVISION_JSONCPP_IMPORTED_LOCATION_RELEASE jsoncpp IMPORTED_LOCATION_RELEASE)
+    get_target_property(HIKVISION_JSONCPP_INTERFACE_INCLUDES jsoncpp INTERFACE_INCLUDE_DIRECTORIES)
+    message(STATUS "[hikvision_camera] jsoncpp target: jsoncpp")
+    message(STATUS "[hikvision_camera] jsoncpp IMPORTED_LOCATION: ${HIKVISION_JSONCPP_IMPORTED_LOCATION}")
+    message(STATUS "[hikvision_camera] jsoncpp IMPORTED_LOCATION_RELEASE: ${HIKVISION_JSONCPP_IMPORTED_LOCATION_RELEASE}")
+    message(STATUS "[hikvision_camera] jsoncpp INTERFACE_INCLUDE_DIRECTORIES: ${HIKVISION_JSONCPP_INTERFACE_INCLUDES}")
+else()
+    find_library(HIKVISION_JSONCPP_LIBRARY NAMES jsoncpp)
+    if(HIKVISION_JSONCPP_LIBRARY)
+        set(HIKVISION_JSONCPP_LINK "${HIKVISION_JSONCPP_LIBRARY}")
+        message(STATUS "[hikvision_camera] jsoncpp library: ${HIKVISION_JSONCPP_LIBRARY}")
+    else()
+        message(WARNING "[hikvision_camera] jsoncpp library not found by CMake; linker will resolve -ljsoncpp")
+    endif()
+endif()
+
+find_path(HIKVISION_JSONCPP_INCLUDE_DIR NAMES json/json.h PATH_SUFFIXES jsoncpp)
+message(STATUS "[hikvision_camera] jsoncpp include dir: ${HIKVISION_JSONCPP_INCLUDE_DIR}")
+if(HIKVISION_JSONCPP_INCLUDE_DIR)
+    target_include_directories(hikvision_camera PRIVATE "${HIKVISION_JSONCPP_INCLUDE_DIR}")
+endif()
+
+target_link_libraries(hikvision_camera
+        PUBLIC
+        glog
+        opencv_core
+        cmvr_es::proto
+        PRIVATE
+        "${HIKVISION_HCNETSDK_LIB}"
+        ${HIKVISION_JSONCPP_LINK}
+        pthread
+)
+
+set_target_properties(hikvision_camera PROPERTIES
+        BUILD_RPATH "${HIKVISION_SDK_LIB_DIR};${HIKVISION_SDK_COM_DIR}"
+        INSTALL_RPATH "$ORIGIN;$ORIGIN/HCNetSDKCom"
+)
+
+add_library(cmvr_es::device::hikvision_camera ALIAS hikvision_camera)
+install(TARGETS hikvision_camera LIBRARY DESTINATION lib)
+install(FILES ${HIKVISION_SDK_LIBS} DESTINATION lib)
+install(DIRECTORY "${HIKVISION_SDK_COM_DIR}" DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(hikvision_camera_callback_test
+            tests/hikvision_camera_callback_test.cpp
+            src/hikvision_camera.cpp
+    )
+    target_include_directories(hikvision_camera_callback_test
+            PRIVATE
+            ${CMAKE_CURRENT_SOURCE_DIR}
+            "${HIKVISION_SDK_ROOT}/include"
+    )
+    if(HIKVISION_JSONCPP_INCLUDE_DIR)
+        target_include_directories(
+                hikvision_camera_callback_test
+                PRIVATE
+                "${HIKVISION_JSONCPP_INCLUDE_DIR}"
+        )
+    endif()
+    target_compile_definitions(hikvision_camera_callback_test
+            PRIVATE
+            HIKVISION_SDK_LIB_DIR="${HIKVISION_SDK_LIB_DIR}/"
+    )
+    # The test supplies a small in-process HCNetSDK fake, so it exercises the
+    # callback/stop ordering without requiring a camera or loading hcnetsdk.
+    target_link_libraries(hikvision_camera_callback_test
+            PRIVATE
+            glog
+            opencv_core
+            cmvr_es::proto
+            ${HIKVISION_JSONCPP_LINK}
+            pthread
+    )
+    add_test(
+            NAME hikvision_camera_callback_test
+            COMMAND hikvision_camera_callback_test
+    )
+    set_tests_properties(hikvision_camera_callback_test PROPERTIES TIMEOUT 10)
+    if(UNIX AND NOT APPLE)
+        get_property(_hikvision_test_library_dirs DIRECTORY PROPERTY LINK_DIRECTORIES)
+        list(PREPEND _hikvision_test_library_dirs "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime")
+        list(JOIN _hikvision_test_library_dirs ":" _hikvision_test_library_path)
+        set_tests_properties(hikvision_camera_callback_test PROPERTIES
+                ENVIRONMENT "LD_LIBRARY_PATH=${_hikvision_test_library_path}"
+        )
+    endif()
+endif()

cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h

@@ -0,0 +1,117 @@
+#ifndef CMVR_ES_HIKVISION_CAMERA_H
+#define CMVR_ES_HIKVISION_CAMERA_H
+
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "common/base/ring_buffer.h"
+#include "devices/camera/abstract_camera.h"
+
+namespace cmvr::device {
+
+class HikvisionCamera final : public AbstractCamera {
+public:
+    explicit HikvisionCamera(const cmvr::config::HikvisionCameraConfig& camera);
+    ~HikvisionCamera() override;
+
+    std::string typeName() const override { return "HikvisionCamera"; }
+    bool init() override;
+    bool start() override;
+    bool stop() override;
+    void getState(CameraState& state) override;
+
+    void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
+    void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
+    void getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
+
+    void startRecording(const std::string& video_path) override;
+    void stopRecording() override;
+    void pauseRecording() override;
+    void resumeRecording() override;
+    void getEncodedFrame(StreamFrameData& frame_data, size_t& index) override;
+    bool waitEncodedFrame(StreamFrameData& frame_data, size_t& index, std::chrono::milliseconds timeout) override;
+    bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
+    bool startStreaming() override;
+    void stopStreaming() override;
+    bool controlPtz(PtzCommand command, bool stop, int speed) override;
+    bool executeJsonCommand(const std::string& request_json, std::string& response_json) override;
+    bool requestKeyFrame() override;
+
+    void onEsData(long real_handle,
+                  unsigned int packet_type,
+                  unsigned char* buffer,
+                  unsigned int buffer_size,
+                  unsigned int width,
+                  unsigned int height,
+                  uint64_t source_timestamp,
+                  uint64_t source_frame_number,
+                  unsigned int source_frame_rate,
+                  unsigned int source_packet_mode);
+
+private:
+    bool initSdk_();
+    void releaseSdk_();
+    bool login_();
+    bool startPreview_();
+    void stopPreview_();
+    bool requestKeyFrame_();
+    void stopRecordingUnlocked_();
+    void fillIntrinsics_(Rs2Intrinsics& intrinsics) const;
+    void setError_(const std::string& message);
+    std::string sdkError_(const std::string& action) const;
+    void pushEncodedFrame_(const unsigned char* buffer,
+                           unsigned int buffer_size,
+                           bool is_key_frame,
+                           unsigned int width,
+                           unsigned int height,
+                           uint64_t source_timestamp,
+                           uint64_t source_frame_number,
+                           unsigned int source_frame_rate,
+                           unsigned int source_packet_mode);
+    void resetStreamState_();
+
+    config::HikvisionCameraConfig camera_;
+    std::string ip_;
+    std::string username_;
+    std::string password_;
+    std::string sdk_path_;
+
+    int port_ = 8000;
+    int channel_ = 1;
+    int stream_type_ = 0;
+    int link_mode_ = 0;
+    int fps_ = 25;
+    int width_ = 0;
+    int height_ = 0;
+    size_t buffer_size_ = 30;
+    std::string codec_ = "H264";
+
+    int user_id_ = -1;
+    int real_handle_ = -1;
+    bool sdk_acquired_ = false;
+
+    std::shared_ptr<SPMCRingBuffer<StreamFrameData>> stream_frame_buffer_;
+    std::string current_video_path_;
+
+    mutable std::mutex ctrl_mtx_;
+    // SDK callbacks must never take ctrl_mtx_: NET_DVR_StopRealPlay may wait
+    // for an in-flight callback while stop() owns that mutex. This mutex is the
+    // single synchronization domain for callback publication and stream state.
+    mutable std::mutex callback_mtx_;
+    bool callback_publishing_enabled_ = false;
+    long callback_preview_handle_ = -1;
+    std::vector<uint8_t> es_stream_header_;
+    bool has_es_stream_header_ = false;
+    bool awaiting_key_frame_ = false;
+    int stream_count_ = 0;
+    uint64_t stream_epoch_ = 0;
+    uint64_t stream_sequence_ = 0;
+    uint32_t codec_config_generation_ = 0;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_HIKVISION_CAMERA_H

cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp

@@ -0,0 +1,961 @@
+#include "../include/hikvision_camera.h"
+
+#include <algorithm>
+#include <atomic>
+#include <cctype>
+#include <chrono>
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include <filesystem>
+#include <limits.h>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#if defined(__linux__)
+#include <unistd.h>
+#endif
+
+#include "HCNetSDK.h"
+#include "common/base/logging/logger.h"
+#include "json/json.h"
+
+namespace {
+
+std::mutex g_sdk_mutex;
+int g_sdk_ref_count = 0;
+bool g_sdk_initialized = false;
+std::atomic<int> g_ignored_data_type_log_count{0};
+std::atomic<int> g_es_video_log_count{0};
+std::atomic<int> g_unexpected_packet_mode_log_count{0};
+
+constexpr DWORD kHikvisionPacketFileHeader = 0;
+constexpr DWORD kHikvisionPacketVideoIFrame = 1;
+constexpr DWORD kHikvisionPacketVideoBFrame = 2;
+constexpr DWORD kHikvisionPacketVideoPFrame = 3;
+
+std::string normalizeSdkPath(std::string path)
+{
+    if (path.empty()) {
+        return path;
+    }
+    const char last = path.back();
+    if (last != '/' && last != '\\') {
+        path.push_back('/');
+    }
+    return path;
+}
+
+std::string defaultRuntimeSdkPath()
+{
+#if defined(__linux__)
+    char executable_path[PATH_MAX] = {0};
+    const ssize_t count = readlink("/proc/self/exe", executable_path, PATH_MAX - 1);
+    if (count > 0) {
+        executable_path[count] = '\0';
+        const auto lib_path =
+            std::filesystem::path(executable_path).parent_path() / ".." / "lib";
+        return normalizeSdkPath(std::filesystem::weakly_canonical(lib_path).string());
+    }
+#endif
+    return normalizeSdkPath((std::filesystem::current_path() / ".." / "lib").string());
+}
+
+void copyCString(char* dest, size_t dest_size, const std::string& source)
+{
+    if (dest_size == 0) {
+        return;
+    }
+    std::snprintf(dest, dest_size, "%s", source.c_str());
+}
+
+DWORD toHikvisionPtzCommand(cmvr::device::PtzCommand command)
+{
+    switch (command) {
+        case cmvr::device::PtzCommand::TiltUp:
+            return TILT_UP;
+        case cmvr::device::PtzCommand::TiltDown:
+            return TILT_DOWN;
+        case cmvr::device::PtzCommand::PanLeft:
+            return PAN_LEFT;
+        case cmvr::device::PtzCommand::PanRight:
+            return PAN_RIGHT;
+        case cmvr::device::PtzCommand::UpLeft:
+            return UP_LEFT;
+        case cmvr::device::PtzCommand::UpRight:
+            return UP_RIGHT;
+        case cmvr::device::PtzCommand::DownLeft:
+            return DOWN_LEFT;
+        case cmvr::device::PtzCommand::DownRight:
+            return DOWN_RIGHT;
+        case cmvr::device::PtzCommand::ZoomIn:
+            return ZOOM_IN;
+        case cmvr::device::PtzCommand::ZoomOut:
+            return ZOOM_OUT;
+        case cmvr::device::PtzCommand::PanAuto:
+            return PAN_AUTO;
+    }
+    return 0;
+}
+
+DWORD normalizePtzSpeed(int speed)
+{
+    if (speed <= 0) {
+        return 4;
+    }
+    if (speed > 7) {
+        return 7;
+    }
+    return static_cast<DWORD>(speed);
+}
+
+std::string lowerString(std::string value)
+{
+    std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) {
+        return static_cast<char>(std::tolower(c));
+    });
+    return value;
+}
+
+std::string jsonEscape(const std::string& value)
+{
+    std::ostringstream out;
+    for (const char c : value) {
+        switch (c) {
+            case '\\':
+                out << "\\\\";
+                break;
+            case '"':
+                out << "\\\"";
+                break;
+            case '\n':
+                out << "\\n";
+                break;
+            case '\r':
+                out << "\\r";
+                break;
+            case '\t':
+                out << "\\t";
+                break;
+            default:
+                out << c;
+                break;
+        }
+    }
+    return out.str();
+}
+
+std::string jsonStringField(const Json::Value& value, const char* name)
+{
+    const Json::Value* member = value.find(name, name + std::strlen(name));
+    return member && member->isString() ? member->asString() : "";
+}
+
+bool jsonBoolField(const Json::Value& value, const char* name, bool& out)
+{
+    const Json::Value* member = value.find(name, name + std::strlen(name));
+    if (!member || !member->isBool()) {
+        return false;
+    }
+    out = member->asBool();
+    return true;
+}
+
+int jsonIntField(const Json::Value& value, const char* name, int default_value)
+{
+    const Json::Value* member = value.find(name, name + std::strlen(name));
+    return member && member->isInt() ? member->asInt() : default_value;
+}
+
+bool parseJsonCommand(const std::string& request_json, Json::Value& root, std::string& error)
+{
+    Json::CharReaderBuilder builder;
+    std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
+    return reader->parse(
+        request_json.data(), request_json.data() + request_json.size(), &root, &error);
+}
+
+bool parsePtzCommandName(const std::string& command, cmvr::device::PtzCommand& out)
+{
+    const std::string normalized = lowerString(command);
+    if (normalized == "tilt_up" || normalized == "up") {
+        out = cmvr::device::PtzCommand::TiltUp;
+    } else if (normalized == "tilt_down" || normalized == "down") {
+        out = cmvr::device::PtzCommand::TiltDown;
+    } else if (normalized == "pan_left" || normalized == "left") {
+        out = cmvr::device::PtzCommand::PanLeft;
+    } else if (normalized == "pan_right" || normalized == "right") {
+        out = cmvr::device::PtzCommand::PanRight;
+    } else if (normalized == "up_left") {
+        out = cmvr::device::PtzCommand::UpLeft;
+    } else if (normalized == "up_right") {
+        out = cmvr::device::PtzCommand::UpRight;
+    } else if (normalized == "down_left") {
+        out = cmvr::device::PtzCommand::DownLeft;
+    } else if (normalized == "down_right") {
+        out = cmvr::device::PtzCommand::DownRight;
+    } else if (normalized == "zoom_in") {
+        out = cmvr::device::PtzCommand::ZoomIn;
+    } else if (normalized == "zoom_out") {
+        out = cmvr::device::PtzCommand::ZoomOut;
+    } else if (normalized == "pan_auto" || normalized == "auto") {
+        out = cmvr::device::PtzCommand::PanAuto;
+    } else {
+        return false;
+    }
+    return true;
+}
+
+std::string makeJsonResult(bool success, const std::string& error_message = "")
+{
+    std::ostringstream out;
+    out << "{\"success\":" << (success ? "true" : "false");
+    if (!error_message.empty()) {
+        out << ",\"error_message\":\"" << jsonEscape(error_message) << "\"";
+    }
+    out << "}";
+    return out.str();
+}
+
+void CALLBACK hikvisionEsRealPlayCallback(
+    LONG real_handle, NET_DVR_PACKET_INFO_EX* packet_info, void* user)
+{
+    auto* camera = static_cast<cmvr::device::HikvisionCamera*>(user);
+    if (!camera || !packet_info) {
+        return;
+    }
+    const uint64_t source_timestamp =
+        (static_cast<uint64_t>(packet_info->dwTimeStampHigh) << 32U) |
+        static_cast<uint64_t>(packet_info->dwTimeStamp);
+    camera->onEsData(real_handle,
+                     packet_info->dwPacketType,
+                     packet_info->pPacketBuffer,
+                     packet_info->dwPacketSize,
+                     packet_info->wWidth,
+                     packet_info->wHeight,
+                     source_timestamp,
+                     packet_info->dwFrameNum,
+                     packet_info->dwFrameRate,
+                     packet_info->dwPacketMode);
+}
+
+} // namespace
+
+namespace cmvr::device {
+
+HikvisionCamera::HikvisionCamera(const config::HikvisionCameraConfig& camera)
+    : camera_(camera)
+{
+    id_ = camera_.id();
+    ip_ = camera_.ip();
+    username_ = camera_.username().empty() ? "admin" : camera_.username();
+    password_ = camera_.password();
+    port_ = camera_.port() > 0 ? camera_.port() : 8000;
+    channel_ = camera_.channel() > 0 ? camera_.channel() : 1;
+    stream_type_ = camera_.stream_type() >= 0 ? camera_.stream_type() : 0;
+    link_mode_ = camera_.link_mode() >= 0 ? camera_.link_mode() : 0;
+    fps_ = camera_.fps() > 0 ? camera_.fps() : 25;
+    width_ = camera_.width();
+    height_ = camera_.height();
+    buffer_size_ = camera_.buffer_size() > 0 ? static_cast<size_t>(camera_.buffer_size()) : 30;
+    codec_ = camera_.codec().empty() ? "H264" : camera_.codec();
+    sdk_path_ = normalizeSdkPath(
+        camera_.sdk_path().empty() ? defaultRuntimeSdkPath() : camera_.sdk_path());
+    // Keep the shared_ptr itself immutable after construction. Readers and the
+    // SDK callback may use it concurrently; the ring buffer owns its locking.
+    stream_frame_buffer_ = std::make_shared<SPMCRingBuffer<StreamFrameData>>(buffer_size_);
+
+    state_.fps = fps_;
+    state_.width = width_;
+    state_.height = height_;
+
+    if (id_.empty()) {
+        setError_("[HikvisionCamera] camera id is empty");
+    } else if (ip_.empty()) {
+        setError_("[HikvisionCamera] ip is empty");
+    } else if (password_.empty()) {
+        setError_("[HikvisionCamera] password is empty");
+    }
+}
+
+HikvisionCamera::~HikvisionCamera()
+{
+    stop();
+}
+
+bool HikvisionCamera::init()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+    state_.is_initialized = false;
+
+    if (ip_.empty() || username_.empty() || password_.empty()) {
+        setError_("ip, username or password is empty");
+        return false;
+    }
+
+    stream_frame_buffer_->clear();
+    resetStreamState_();
+
+    if (!initSdk_()) {
+        return false;
+    }
+
+    state_.is_initialized = true;
+    state_.is_error = false;
+    CMVR_LOG(INFO) << "[HikvisionCamera] initialized: id=" << id_
+                   << ", ip=" << ip_
+                   << ", channel=" << channel_
+                   << ", stream_type=" << stream_type_;
+    return true;
+}
+
+bool HikvisionCamera::start()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+
+    if (!state_.is_initialized) {
+        setError_("camera not initialized");
+        return false;
+    }
+    if (state_.is_opened) {
+        return true;
+    }
+    if (!login_()) {
+        return false;
+    }
+    if (!startPreview_()) {
+        if (user_id_ >= 0) {
+            NET_DVR_Logout(user_id_);
+            user_id_ = -1;
+        }
+        return false;
+    }
+
+    state_.is_opened = true;
+    return true;
+}
+
+bool HikvisionCamera::stop()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+    if (state_.is_recording) {
+        stopRecordingUnlocked_();
+    }
+    state_.is_streaming = false;
+    stream_count_ = 0;
+    resetStreamState_();
+    stopPreview_();
+    if (user_id_ >= 0) {
+        NET_DVR_Logout(user_id_);
+        user_id_ = -1;
+    }
+    state_.is_opened = false;
+    releaseSdk_();
+    return true;
+}
+
+void HikvisionCamera::getState(CameraState& state)
+{
+    std::lock_guard lock(ctrl_mtx_);
+    state = state_;
+}
+
+void HikvisionCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+    fillIntrinsics_(intrinsics);
+    setError_("getRGBImage unsupported: HikvisionCamera only forwards stream data");
+    color.release();
+}
+
+void HikvisionCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics&)
+{
+    std::lock_guard lock(ctrl_mtx_);
+    setError_("getDepthImage unsupported usage");
+    depth.release();
+}
+
+void HikvisionCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
+{
+    getRGBImage(color, intrinsics);
+    depth.release();
+}
+
+void HikvisionCamera::startRecording(const std::string& video_path)
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+
+    if (!state_.is_opened || real_handle_ < 0) {
+        setError_("camera not opened");
+        return;
+    }
+    if (state_.is_recording) {
+        setError_("already recording");
+        return;
+    }
+    if (video_path.empty()) {
+        setError_("video path is empty");
+        return;
+    }
+
+    current_video_path_ = video_path;
+    if (!NET_DVR_SaveRealData(real_handle_, const_cast<char*>(current_video_path_.c_str()))) {
+        setError_(sdkError_("NET_DVR_SaveRealData"));
+        current_video_path_.clear();
+        return;
+    }
+    state_.is_recording = true;
+}
+
+void HikvisionCamera::stopRecording()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+    stopRecordingUnlocked_();
+}
+
+void HikvisionCamera::stopRecordingUnlocked_()
+{
+    clear_error_();
+
+    if (!state_.is_recording) {
+        return;
+    }
+    if (real_handle_ >= 0 && !NET_DVR_StopSaveRealData(real_handle_)) {
+        setError_(sdkError_("NET_DVR_StopSaveRealData"));
+        return;
+    }
+    state_.is_recording = false;
+    current_video_path_.clear();
+}
+
+void HikvisionCamera::pauseRecording()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    setError_("pauseRecording unsupported by Hikvision SDK recording");
+}
+
+void HikvisionCamera::resumeRecording()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    setError_("resumeRecording unsupported by Hikvision SDK recording");
+}
+
+void HikvisionCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index)
+{
+    if (!stream_frame_buffer_) {
+        return;
+    }
+    auto frame = stream_frame_buffer_->pop(index);
+    if (frame) {
+        frame_data = std::move(*frame);
+    }
+}
+
+bool HikvisionCamera::waitEncodedFrame(
+    StreamFrameData& frame_data,
+    size_t& index,
+    const std::chrono::milliseconds timeout)
+{
+    if (!stream_frame_buffer_) {
+        return false;
+    }
+    auto frame = stream_frame_buffer_->waitPop(index, timeout);
+    if (!frame) {
+        return false;
+    }
+    frame_data = std::move(*frame);
+    return !frame_data.rgbFrame.empty() || !frame_data.depthFrame.empty();
+}
+
+bool HikvisionCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index)
+{
+    if (!stream_frame_buffer_) {
+        return false;
+    }
+
+    auto frame = stream_frame_buffer_->getLatest(next_index);
+    if (!frame.has_value()) {
+        return false;
+    }
+
+    frame_data = frame.value();
+    return true;
+}
+
+bool HikvisionCamera::startStreaming()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+
+    if (!state_.is_opened) {
+        setError_("camera not opened");
+        return false;
+    }
+
+    const bool first_stream = stream_count_ == 0;
+    if (first_stream) {
+        if (stream_frame_buffer_) {
+            stream_frame_buffer_->clear();
+        }
+        std::lock_guard callback_lock(callback_mtx_);
+        if (callback_preview_handle_ < 0 ||
+            callback_preview_handle_ != static_cast<long>(real_handle_)) {
+            setError_("preview callback is not active");
+            return false;
+        }
+        ++stream_epoch_;
+        stream_sequence_ = 0;
+        ++codec_config_generation_;
+        awaiting_key_frame_ = true;
+        callback_publishing_enabled_ = true;
+    }
+    state_.is_streaming = true;
+    ++stream_count_;
+    if (first_stream && !requestKeyFrame_()) {
+        // Keep waiting for the next natural I-frame. Publishing P/B frames
+        // immediately would make a newly attached decoder start corrupted.
+        CMVR_LOG(WARNING) << "[HikvisionCamera] key-frame request failed; "
+                          << "waiting for the next natural I-frame";
+    }
+    return true;
+}
+
+void HikvisionCamera::stopStreaming()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    if (stream_count_ > 0) {
+        --stream_count_;
+    }
+    if (stream_count_ == 0) {
+        // Wait for an already-running callback to finish its publication, then
+        // prevent both queued and future callbacks from publishing.  No frame
+        // can be pushed after this critical section has completed.
+        std::lock_guard callback_lock(callback_mtx_);
+        callback_publishing_enabled_ = false;
+        awaiting_key_frame_ = false;
+        state_.is_streaming = false;
+    }
+}
+
+bool HikvisionCamera::controlPtz(PtzCommand command, bool stop, int speed)
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+
+    if (!state_.is_opened || user_id_ < 0) {
+        setError_("camera not opened");
+        return false;
+    }
+
+    const DWORD sdk_command = toHikvisionPtzCommand(command);
+    if (sdk_command == 0) {
+        setError_("unsupported PTZ command");
+        return false;
+    }
+
+    const DWORD sdk_stop = stop ? 1 : 0;
+    const DWORD sdk_speed = normalizePtzSpeed(speed);
+    if (!NET_DVR_PTZControlWithSpeed_Other(user_id_, channel_, sdk_command, sdk_stop, sdk_speed)) {
+        setError_(sdkError_("NET_DVR_PTZControlWithSpeed_Other"));
+        return false;
+    }
+
+    return true;
+}
+
+bool HikvisionCamera::requestKeyFrame()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+    if (!state_.is_opened || user_id_ < 0) {
+        setError_("camera not opened");
+        return false;
+    }
+    if (!requestKeyFrame_()) {
+        setError_(sdkError_(
+            stream_type_ == 0 ? "NET_DVR_MakeKeyFrame" : "NET_DVR_MakeKeyFrameSub"));
+        return false;
+    }
+    return true;
+}
+
+bool HikvisionCamera::requestKeyFrame_()
+{
+    if (user_id_ < 0) {
+        CMVR_LOG(WARNING) << "[HikvisionCamera] skip key frame request: camera not logged in";
+        return false;
+    }
+
+    const bool is_sub_stream = stream_type_ != 0;
+    const BOOL success = is_sub_stream
+        ? NET_DVR_MakeKeyFrameSub(user_id_, channel_)
+        : NET_DVR_MakeKeyFrame(user_id_, channel_);
+    if (!success) {
+        CMVR_LOG(WARNING) << "[HikvisionCamera] "
+                          << (is_sub_stream ? "NET_DVR_MakeKeyFrameSub" : "NET_DVR_MakeKeyFrame")
+                          << " failed, error_code=" << NET_DVR_GetLastError();
+        return false;
+    }
+
+    CMVR_LOG(INFO) << "[HikvisionCamera] requested key frame"
+                   << ", channel=" << channel_
+                   << ", stream_type=" << stream_type_;
+    return true;
+}
+
+bool HikvisionCamera::executeJsonCommand(const std::string& request_json, std::string& response_json)
+{
+    Json::Value root;
+    std::string parse_error;
+    if (!parseJsonCommand(request_json, root, parse_error) || !root.isObject()) {
+        response_json = makeJsonResult(false, "invalid json: " + parse_error);
+        return false;
+    }
+
+    const std::string command_type = lowerString(
+        !jsonStringField(root, "command").empty() ? jsonStringField(root, "command") :
+        !jsonStringField(root, "type").empty() ? jsonStringField(root, "type") :
+        jsonStringField(root, "action"));
+    if (command_type != "ptz") {
+        response_json = makeJsonResult(false, "unsupported json command: " + command_type);
+        return false;
+    }
+
+    const std::string ptz_command_name =
+        !jsonStringField(root, "direction").empty() ? jsonStringField(root, "direction") :
+        !jsonStringField(root, "ptz_command").empty() ? jsonStringField(root, "ptz_command") :
+        jsonStringField(root, "operation");
+
+    PtzCommand ptz_command{};
+    if (!parsePtzCommandName(ptz_command_name, ptz_command)) {
+        response_json = makeJsonResult(false, "invalid ptz command: " + ptz_command_name);
+        return false;
+    }
+
+    bool stop = false;
+    if (!jsonBoolField(root, "stop", stop)) {
+        const std::string ptz_action = lowerString(jsonStringField(root, "ptz_action"));
+        const std::string action = lowerString(jsonStringField(root, "action"));
+        stop = ptz_action == "stop" || action == "stop";
+    }
+
+    const int speed = jsonIntField(root, "speed", 0);
+    if (!controlPtz(ptz_command, stop, speed)) {
+        CameraState state;
+        getState(state);
+        response_json = makeJsonResult(
+            false,
+            state.error_message.empty() ? "failed to control PTZ" : state.error_message);
+        return false;
+    }
+
+    std::ostringstream result;
+    result << "{\"success\":true"
+           << ",\"command\":\"ptz\""
+           << ",\"direction\":\"" << jsonEscape(ptz_command_name) << "\""
+           << ",\"stop\":" << (stop ? "true" : "false")
+           << ",\"speed\":" << static_cast<int>(normalizePtzSpeed(speed))
+           << "}";
+    response_json = result.str();
+    return true;
+}
+
+void HikvisionCamera::onEsData(
+    const long real_handle,
+    const unsigned int packet_type,
+    unsigned char* buffer,
+    const unsigned int buffer_size,
+    const unsigned int packet_width,
+    const unsigned int packet_height,
+    const uint64_t source_timestamp,
+    const uint64_t source_frame_number,
+    const unsigned int source_frame_rate,
+    const unsigned int source_packet_mode)
+{
+    if (!buffer || buffer_size == 0) {
+        return;
+    }
+
+    // Never take ctrl_mtx_ from an SDK callback. NET_DVR_StopRealPlay may wait
+    // for this callback while stop() owns ctrl_mtx_. Holding callback_mtx_
+    // through publication also preserves the ring's single-producer contract.
+    std::lock_guard callback_lock(callback_mtx_);
+    if (callback_preview_handle_ != real_handle || !stream_frame_buffer_) {
+        return;
+    }
+
+    if (packet_type == kHikvisionPacketFileHeader) {
+        const bool changed =
+            !has_es_stream_header_ ||
+            es_stream_header_.size() != buffer_size ||
+            !std::equal(es_stream_header_.begin(), es_stream_header_.end(), buffer);
+        if (changed) {
+            es_stream_header_.assign(buffer, buffer + buffer_size);
+            has_es_stream_header_ = true;
+            ++codec_config_generation_;
+            if (callback_publishing_enabled_) {
+                awaiting_key_frame_ = true;
+            }
+            CMVR_LOG(INFO) << "[HikvisionCamera] received ES stream header, size=" << buffer_size;
+        }
+        return;
+    }
+
+    const bool is_video_packet =
+        packet_type == kHikvisionPacketVideoIFrame ||
+        packet_type == kHikvisionPacketVideoBFrame ||
+        packet_type == kHikvisionPacketVideoPFrame;
+    if (!is_video_packet) {
+        const int log_count = g_ignored_data_type_log_count.fetch_add(1);
+        if (log_count < 10) {
+            CMVR_LOG(INFO) << "[HikvisionCamera] ignore ES packet_type="
+                           << packet_type << ", size=" << buffer_size;
+        }
+        return;
+    }
+
+    const int video_log_count = g_es_video_log_count.fetch_add(1);
+    if (video_log_count < 10) {
+        CMVR_LOG(INFO) << "[HikvisionCamera] ES video packet_type="
+                       << packet_type << ", size=" << buffer_size
+                       << ", source_timestamp=" << source_timestamp
+                       << ", source_frame_number=" << source_frame_number
+                       << ", source_frame_rate=" << source_frame_rate
+                       << ", source_packet_mode=" << source_packet_mode;
+    }
+
+    if (!callback_publishing_enabled_) {
+        return;
+    }
+
+    const bool is_key_frame = packet_type == kHikvisionPacketVideoIFrame;
+    if (awaiting_key_frame_) {
+        if (!is_key_frame) {
+            return;
+        }
+        awaiting_key_frame_ = false;
+        CMVR_LOG(INFO) << "[HikvisionCamera] received first key frame after stream start";
+    }
+
+    pushEncodedFrame_(buffer,
+                      buffer_size,
+                      is_key_frame,
+                      packet_width,
+                      packet_height,
+                      source_timestamp,
+                      source_frame_number,
+                      source_frame_rate,
+                      source_packet_mode);
+}
+
+void HikvisionCamera::pushEncodedFrame_(
+    const unsigned char* buffer,
+    unsigned int buffer_size,
+    bool is_key_frame,
+    unsigned int packet_width,
+    unsigned int packet_height,
+    uint64_t source_timestamp,
+    uint64_t source_frame_number,
+    unsigned int source_frame_rate,
+    unsigned int source_packet_mode)
+{
+    if (!buffer || buffer_size == 0 || !stream_frame_buffer_) {
+        return;
+    }
+
+    StreamFrameData frame_data;
+    const auto capture_monotonic = std::chrono::steady_clock::now();
+    const auto capture_utc = std::chrono::system_clock::now();
+    frame_data.rgbFrame.assign(buffer, buffer + buffer_size);
+    frame_data.codec = codec_;
+    frame_data.fps =
+        source_frame_rate >= 1U && source_frame_rate <= 1000U
+            ? static_cast<int>(source_frame_rate)
+            : fps_;
+    frame_data.width = packet_width > 0 ? static_cast<int>(packet_width) : width_;
+    frame_data.height = packet_height > 0 ? static_cast<int>(packet_height) : height_;
+    frame_data.bKey = is_key_frame;
+    frame_data.stream_epoch = stream_epoch_;
+    frame_data.sequence = stream_sequence_++;
+    frame_data.source_timestamp = source_timestamp;
+    frame_data.source_frame_number = source_frame_number;
+    frame_data.capture_monotonic_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+        capture_monotonic.time_since_epoch()).count();
+    frame_data.capture_utc_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+        capture_utc.time_since_epoch()).count();
+    frame_data.pts = static_cast<int64_t>(frame_data.sequence);
+    frame_data.dts = frame_data.pts;
+    frame_data.time_base_num = 1;
+    frame_data.time_base_den = std::max(1, frame_data.fps);
+    frame_data.duration = 1;
+    frame_data.codec_config_generation = codec_config_generation_;
+    // Hikvision labels packet type 0 as a "file header", but the bundled SDK
+    // does not guarantee that it is a decoder-ready VPS/SPS/PPS blob. Keep it
+    // only for change detection until its format is verified on real hardware.
+    fillIntrinsics_(frame_data.intrinsics);
+    if (source_packet_mode > 1U) {
+        const int log_count = g_unexpected_packet_mode_log_count.fetch_add(1);
+        if (log_count < 5) {
+            CMVR_LOG(WARNING) << "[HikvisionCamera] unexpected ES source_packet_mode="
+                              << source_packet_mode
+                              << ", source_frame_number=" << source_frame_number;
+        }
+    }
+    stream_frame_buffer_->push(frame_data);
+}
+
+void HikvisionCamera::resetStreamState_()
+{
+    std::lock_guard callback_lock(callback_mtx_);
+    callback_publishing_enabled_ = false;
+    callback_preview_handle_ = -1;
+    awaiting_key_frame_ = false;
+    es_stream_header_.clear();
+    has_es_stream_header_ = false;
+}
+
+bool HikvisionCamera::initSdk_()
+{
+    if (sdk_acquired_) {
+        return true;
+    }
+
+    std::lock_guard<std::mutex> lock(g_sdk_mutex);
+    if (g_sdk_ref_count == 0) {
+        if (!NET_DVR_Init()) {
+            setError_(sdkError_("NET_DVR_Init"));
+            return false;
+        }
+        NET_DVR_SetConnectTime(3000, 3);
+        NET_DVR_SetReconnect(10000, TRUE);
+        NET_DVR_SetLogToFile(3, nullptr, TRUE);
+        g_sdk_initialized = true;
+    }
+
+    ++g_sdk_ref_count;
+    sdk_acquired_ = true;
+    return g_sdk_initialized;
+}
+
+void HikvisionCamera::releaseSdk_()
+{
+    if (!sdk_acquired_) {
+        return;
+    }
+
+    std::lock_guard<std::mutex> lock(g_sdk_mutex);
+    sdk_acquired_ = false;
+    if (g_sdk_ref_count > 0) {
+        --g_sdk_ref_count;
+    }
+    if (g_sdk_ref_count == 0 && g_sdk_initialized) {
+        NET_DVR_Cleanup();
+        g_sdk_initialized = false;
+    }
+}
+
+bool HikvisionCamera::login_()
+{
+    NET_DVR_USER_LOGIN_INFO login_info{};
+    NET_DVR_DEVICEINFO_V40 device_info{};
+
+    copyCString(login_info.sDeviceAddress, sizeof(login_info.sDeviceAddress), ip_);
+    copyCString(login_info.sUserName, sizeof(login_info.sUserName), username_);
+    copyCString(login_info.sPassword, sizeof(login_info.sPassword), password_);
+    login_info.wPort = static_cast<WORD>(port_);
+    login_info.bUseAsynLogin = FALSE;
+
+    user_id_ = NET_DVR_Login_V40(&login_info, &device_info);
+    if (user_id_ < 0) {
+        setError_(sdkError_("NET_DVR_Login_V40"));
+        return false;
+    }
+    return true;
+}
+
+bool HikvisionCamera::startPreview_()
+{
+    NET_DVR_PREVIEWINFO preview_info{};
+    preview_info.lChannel = channel_;
+    preview_info.dwStreamType = static_cast<DWORD>(stream_type_);
+    preview_info.dwLinkMode = static_cast<DWORD>(link_mode_);
+    preview_info.hPlayWnd = 0;
+    preview_info.bBlocked = 1;
+    preview_info.dwDisplayBufNum = 1;
+    preview_info.byReconnect = 1;
+
+    real_handle_ = NET_DVR_RealPlay_V40(user_id_, &preview_info, nullptr, nullptr);
+    if (real_handle_ < 0) {
+        setError_(sdkError_("NET_DVR_RealPlay_V40"));
+        return false;
+    }
+    {
+        std::lock_guard callback_lock(callback_mtx_);
+        callback_publishing_enabled_ = false;
+        callback_preview_handle_ = static_cast<long>(real_handle_);
+        awaiting_key_frame_ = false;
+    }
+    if (!NET_DVR_SetESRealPlayCallBack(real_handle_, hikvisionEsRealPlayCallback, this)) {
+        setError_(sdkError_("NET_DVR_SetESRealPlayCallBack"));
+        {
+            std::lock_guard callback_lock(callback_mtx_);
+            callback_publishing_enabled_ = false;
+            callback_preview_handle_ = -1;
+            awaiting_key_frame_ = false;
+        }
+        NET_DVR_StopRealPlay(real_handle_);
+        real_handle_ = -1;
+        return false;
+    }
+    return true;
+}
+
+void HikvisionCamera::stopPreview_()
+{
+    const int preview_handle = real_handle_;
+    {
+        // Invalidate before asking the SDK to stop. We intentionally release
+        // callback_mtx_ before NET_DVR_StopRealPlay because that function may
+        // wait for an SDK callback to return.
+        std::lock_guard callback_lock(callback_mtx_);
+        callback_publishing_enabled_ = false;
+        callback_preview_handle_ = -1;
+        awaiting_key_frame_ = false;
+    }
+    if (preview_handle >= 0) {
+        NET_DVR_StopRealPlay(preview_handle);
+        real_handle_ = -1;
+    }
+}
+
+void HikvisionCamera::fillIntrinsics_(Rs2Intrinsics& intrinsics) const
+{
+    intrinsics.fx = camera_.fx();
+    intrinsics.fy = camera_.fy();
+    intrinsics.cx = camera_.cx() > 0.0f ? camera_.cx() : static_cast<float>(width_) * 0.5f;
+    intrinsics.cy = camera_.cy() > 0.0f ? camera_.cy() : static_cast<float>(height_) * 0.5f;
+    for (int i = 0; i < 5; ++i) {
+        intrinsics.coeffs[i] = i < camera_.coeffs_size() ? camera_.coeffs(i) : 0.0f;
+    }
+}
+
+void HikvisionCamera::setError_(const std::string& message)
+{
+    state_.is_error = true;
+    state_.error_message = message;
+    CMVR_LOG(ERROR) << "[HikvisionCamera] " << message;
+}
+
+std::string HikvisionCamera::sdkError_(const std::string& action) const
+{
+    return action + " failed, error_code=" + std::to_string(NET_DVR_GetLastError());
+}
+
+} // namespace cmvr::device

cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp

@@ -0,0 +1,425 @@
+#include "../include/hikvision_camera.h"
+
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "HCNetSDK.h"
+
+namespace {
+
+using EsDataCallback =
+    void(CALLBACK*)(LONG, NET_DVR_PACKET_INFO_EX*, void*);
+
+constexpr DWORD kFileHeader = 0;
+constexpr DWORD kVideoIFrame = 1;
+constexpr DWORD kVideoPFrame = 3;
+
+std::mutex g_fake_sdk_mutex;
+EsDataCallback g_es_data_callback = nullptr;
+void* g_es_data_user = nullptr;
+LONG g_real_handle = 42;
+std::atomic<int> g_stop_callback_count{0};
+std::atomic<int> g_key_frame_request_count{0};
+std::atomic<int> g_ptz_call_count{0};
+std::atomic<DWORD> g_last_ptz_command{0};
+std::atomic<DWORD> g_last_ptz_stop{0};
+std::atomic<DWORD> g_last_ptz_speed{0};
+
+void resetFakeSdk()
+{
+    std::lock_guard lock(g_fake_sdk_mutex);
+    g_es_data_callback = nullptr;
+    g_es_data_user = nullptr;
+    g_stop_callback_count = 0;
+    g_key_frame_request_count = 0;
+    g_ptz_call_count = 0;
+    g_last_ptz_command = 0;
+    g_last_ptz_stop = 0;
+    g_last_ptz_speed = 0;
+}
+
+void emitEsPacket(
+    const DWORD packet_type,
+    const LONG real_handle,
+    std::vector<BYTE> payload,
+    const WORD width = 640,
+    const WORD height = 360,
+    const DWORD timestamp_low = 0,
+    const DWORD timestamp_high = 0,
+    const DWORD frame_number = 0,
+    const DWORD frame_rate = 0,
+    const DWORD packet_mode = 0)
+{
+    EsDataCallback callback = nullptr;
+    void* user = nullptr;
+    {
+        std::lock_guard lock(g_fake_sdk_mutex);
+        callback = g_es_data_callback;
+        user = g_es_data_user;
+    }
+
+    if (!callback) {
+        return;
+    }
+
+    NET_DVR_PACKET_INFO_EX packet{};
+    packet.wWidth = width;
+    packet.wHeight = height;
+    packet.dwTimeStamp = timestamp_low;
+    packet.dwTimeStampHigh = timestamp_high;
+    packet.dwFrameNum = frame_number;
+    packet.dwFrameRate = frame_rate;
+    packet.dwPacketType = packet_type;
+    packet.dwPacketSize = static_cast<DWORD>(payload.size());
+    packet.pPacketBuffer = payload.data();
+    packet.dwPacketMode = packet_mode;
+    callback(real_handle, &packet, user);
+}
+
+void emitIFrame(
+    const LONG real_handle = g_real_handle,
+    const DWORD timestamp_low = 0,
+    const DWORD timestamp_high = 0,
+    const DWORD frame_number = 0,
+    const DWORD frame_rate = 0,
+    const DWORD packet_mode = 0)
+{
+    emitEsPacket(
+        kVideoIFrame,
+        real_handle,
+        {0x00, 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x21},
+        640,
+        360,
+        timestamp_low,
+        timestamp_high,
+        frame_number,
+        frame_rate,
+        packet_mode);
+}
+
+void emitPFrame(const LONG real_handle = g_real_handle)
+{
+    emitEsPacket(
+        kVideoPFrame,
+        real_handle,
+        {0x00, 0x00, 0x00, 0x01, 0x41, 0x9A, 0x20});
+}
+
+bool check(const bool condition, const char* expression, const int line)
+{
+    if (condition) {
+        return true;
+    }
+    std::cerr << "CHECK failed at line " << line << ": " << expression << '\n';
+    return false;
+}
+
+#define CHECK_TRUE(expression) \
+    do { \
+        if (!check(static_cast<bool>(expression), #expression, __LINE__)) { \
+            return false; \
+        } \
+    } while (false)
+
+cmvr::config::HikvisionCameraConfig makeConfig()
+{
+    cmvr::config::HikvisionCameraConfig config;
+    config.set_id("hikvision_callback_test");
+    config.set_ip("127.0.0.1");
+    config.set_username("admin");
+    config.set_password("test-only");
+    config.set_port(8000);
+    config.set_channel(1);
+    config.set_stream_type(0);
+    config.set_link_mode(0);
+    config.set_width(1920);
+    config.set_height(1080);
+    config.set_fps(25);
+    config.set_codec("H264");
+    config.set_buffer_size(32);
+    return config;
+}
+
+bool testCallbackPublicationLifecycle()
+{
+    resetFakeSdk();
+    cmvr::device::HikvisionCamera camera(makeConfig());
+    CHECK_TRUE(camera.init());
+    CHECK_TRUE(camera.start());
+    CHECK_TRUE(camera.startStreaming());
+    CHECK_TRUE(g_key_frame_request_count.load() == 1);
+
+    size_t cursor = 0;
+    cmvr::device::StreamFrameData frame;
+
+    // A new subscriber must not receive an undecodable inter frame, and a
+    // delayed callback from an older preview handle must also be rejected.
+    emitPFrame();
+    emitIFrame(g_real_handle - 1);
+    CHECK_TRUE(!camera.waitEncodedFrame(
+        frame, cursor, std::chrono::milliseconds(10)));
+
+    emitIFrame(
+        g_real_handle,
+        0x89ABCDEFU,
+        0x01234567U,
+        42U,
+        30U,
+        1U);
+    CHECK_TRUE(camera.waitEncodedFrame(
+        frame, cursor, std::chrono::milliseconds(50)));
+    CHECK_TRUE(frame.stream_epoch == 1);
+    CHECK_TRUE(frame.sequence == 0);
+    CHECK_TRUE(frame.codec_config_generation == 1);
+    CHECK_TRUE(frame.bKey);
+    CHECK_TRUE(frame.width == 640);
+    CHECK_TRUE(frame.height == 360);
+    CHECK_TRUE(frame.source_timestamp == 0x0123456789ABCDEFULL);
+    CHECK_TRUE(frame.source_frame_number == 42U);
+    CHECK_TRUE(frame.fps == 30);
+    CHECK_TRUE(frame.time_base_den == 30);
+
+    CHECK_TRUE(camera.requestKeyFrame());
+    CHECK_TRUE(g_key_frame_request_count.load() == 2);
+
+    CHECK_TRUE(camera.controlPtz(
+        cmvr::device::PtzCommand::PanLeft, false, 99));
+    CHECK_TRUE(g_ptz_call_count.load() == 1);
+    CHECK_TRUE(g_last_ptz_command.load() == PAN_LEFT);
+    CHECK_TRUE(g_last_ptz_stop.load() == 0);
+    CHECK_TRUE(g_last_ptz_speed.load() == 7);
+
+    std::string json_response;
+    CHECK_TRUE(camera.executeJsonCommand(
+        R"({"command":"ptz","direction":"zoom_in","speed":3})",
+        json_response));
+    CHECK_TRUE(json_response.find(R"("success":true)") != std::string::npos);
+    CHECK_TRUE(g_ptz_call_count.load() == 2);
+    CHECK_TRUE(g_last_ptz_command.load() == ZOOM_IN);
+    CHECK_TRUE(g_last_ptz_speed.load() == 3);
+
+    // A changed SDK file header invalidates the descriptor generation, but is
+    // not exposed as decoder config until its vendor-specific format is known.
+    emitEsPacket(kFileHeader, g_real_handle, {0x01, 0x02, 0x03});
+    emitPFrame();
+    CHECK_TRUE(!camera.waitEncodedFrame(
+        frame, cursor, std::chrono::milliseconds(10)));
+    emitIFrame(
+        g_real_handle,
+        0x76543210U,
+        0xFEDCBA98U,
+        99U,
+        1001U,
+        0U);
+    CHECK_TRUE(camera.waitEncodedFrame(
+        frame, cursor, std::chrono::milliseconds(50)));
+    CHECK_TRUE(frame.sequence == 1);
+    CHECK_TRUE(frame.codec_config_generation == 2);
+    CHECK_TRUE(frame.codec_config.empty());
+    CHECK_TRUE(frame.source_timestamp == 0xFEDCBA9876543210ULL);
+    CHECK_TRUE(frame.source_frame_number == 99U);
+    CHECK_TRUE(frame.fps == 25);
+    CHECK_TRUE(frame.time_base_den == 25);
+
+    constexpr int kConcurrentCallbacks = 8;
+    std::vector<std::thread> producers;
+    producers.reserve(kConcurrentCallbacks);
+    for (int i = 0; i < kConcurrentCallbacks; ++i) {
+        producers.emplace_back(emitPFrame, g_real_handle);
+    }
+    for (auto& producer : producers) {
+        producer.join();
+    }
+
+    size_t latest_cursor = 0;
+    cmvr::device::StreamFrameData latest;
+    CHECK_TRUE(camera.getLatestEncodedFrame(latest, latest_cursor));
+    CHECK_TRUE(latest.stream_epoch == 1);
+    CHECK_TRUE(latest.sequence == static_cast<uint64_t>(kConcurrentCallbacks + 1));
+    CHECK_TRUE(latest_cursor == static_cast<size_t>(kConcurrentCallbacks + 2));
+
+    for (int sequence = 2; sequence <= kConcurrentCallbacks + 1; ++sequence) {
+        CHECK_TRUE(camera.waitEncodedFrame(
+            frame, cursor, std::chrono::milliseconds(50)));
+        CHECK_TRUE(frame.stream_epoch == 1);
+        CHECK_TRUE(frame.sequence == static_cast<uint64_t>(sequence));
+    }
+
+    camera.stopStreaming();
+    emitIFrame();
+    CHECK_TRUE(!camera.waitEncodedFrame(
+        frame, cursor, std::chrono::milliseconds(10)));
+
+    CHECK_TRUE(camera.startStreaming());
+    CHECK_TRUE(g_key_frame_request_count.load() == 3);
+    emitPFrame();
+    emitIFrame(g_real_handle - 1);
+    CHECK_TRUE(!camera.waitEncodedFrame(
+        frame, cursor, std::chrono::milliseconds(10)));
+    emitIFrame();
+    CHECK_TRUE(camera.waitEncodedFrame(
+        frame, cursor, std::chrono::milliseconds(50)));
+    CHECK_TRUE(frame.stream_epoch == 2);
+    CHECK_TRUE(frame.sequence == 0);
+    CHECK_TRUE(frame.codec_config_generation == 3);
+
+    // The fake StopRealPlay invokes the SDK callback synchronously. stop()
+    // owns ctrl_mtx_ here, proving the callback neither takes that mutex nor
+    // publishes after the preview handle has been invalidated.
+    CHECK_TRUE(camera.stop());
+    CHECK_TRUE(g_stop_callback_count.load() == 1);
+    CHECK_TRUE(!camera.waitEncodedFrame(
+        frame, cursor, std::chrono::milliseconds(10)));
+    return true;
+}
+
+} // namespace
+
+extern "C" {
+
+BOOL NET_DVR_Init()
+{
+    return TRUE;
+}
+
+BOOL NET_DVR_Cleanup()
+{
+    return TRUE;
+}
+
+BOOL NET_DVR_SetConnectTime(DWORD, DWORD)
+{
+    return TRUE;
+}
+
+BOOL NET_DVR_SetReconnect(DWORD, BOOL)
+{
+    return TRUE;
+}
+
+BOOL NET_DVR_SetLogToFile(DWORD, char*, BOOL)
+{
+    return TRUE;
+}
+
+LONG NET_DVR_Login_V40(
+    LPNET_DVR_USER_LOGIN_INFO,
+    LPNET_DVR_DEVICEINFO_V40)
+{
+    return 7;
+}
+
+BOOL NET_DVR_Logout(LONG)
+{
+    return TRUE;
+}
+
+DWORD NET_DVR_GetLastError()
+{
+    return 0;
+}
+
+LONG NET_DVR_RealPlay_V40(
+    LONG,
+    LPNET_DVR_PREVIEWINFO,
+    REALDATACALLBACK,
+    void*)
+{
+    return g_real_handle;
+}
+
+BOOL NET_DVR_SetESRealPlayCallBack(
+    LONG,
+    EsDataCallback callback,
+    void* user)
+{
+    std::lock_guard lock(g_fake_sdk_mutex);
+    g_es_data_callback = callback;
+    g_es_data_user = user;
+    return TRUE;
+}
+
+BOOL NET_DVR_StopRealPlay(const LONG real_handle)
+{
+    EsDataCallback callback = nullptr;
+    void* user = nullptr;
+    {
+        std::lock_guard lock(g_fake_sdk_mutex);
+        callback = g_es_data_callback;
+        user = g_es_data_user;
+    }
+
+    if (callback) {
+        std::vector<BYTE> idr{
+            0x00, 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x21
+        };
+        NET_DVR_PACKET_INFO_EX packet{};
+        packet.wWidth = 640;
+        packet.wHeight = 360;
+        packet.dwPacketType = kVideoIFrame;
+        packet.dwPacketSize = static_cast<DWORD>(idr.size());
+        packet.pPacketBuffer = idr.data();
+        callback(real_handle, &packet, user);
+        ++g_stop_callback_count;
+    }
+
+    {
+        std::lock_guard lock(g_fake_sdk_mutex);
+        g_es_data_callback = nullptr;
+        g_es_data_user = nullptr;
+    }
+    return TRUE;
+}
+
+BOOL NET_DVR_SaveRealData(LONG, char*)
+{
+    return TRUE;
+}
+
+BOOL NET_DVR_StopSaveRealData(LONG)
+{
+    return TRUE;
+}
+
+BOOL NET_DVR_MakeKeyFrame(LONG, LONG)
+{
+    ++g_key_frame_request_count;
+    return TRUE;
+}
+
+BOOL NET_DVR_MakeKeyFrameSub(LONG, LONG)
+{
+    ++g_key_frame_request_count;
+    return TRUE;
+}
+
+BOOL NET_DVR_PTZControlWithSpeed_Other(
+    LONG,
+    LONG,
+    const DWORD command,
+    const DWORD stop,
+    const DWORD speed)
+{
+    ++g_ptz_call_count;
+    g_last_ptz_command = command;
+    g_last_ptz_stop = stop;
+    g_last_ptz_speed = speed;
+    return TRUE;
+}
+
+} // extern "C"
+
+int main()
+{
+    if (!testCallbackPublicationLifecycle()) {
+        return 1;
+    }
+    std::cout << "hikvision_camera_callback_test: PASS\n";
+    return 0;
+}

cmvr-es/devices/camera/mechmind/include/mechmind_camera.h

@@ -30,6 +30,7 @@ namespace cmvr::device
         bool init() override;
         bool start() override;
         bool stop() override;
+        void getState(CameraState& state) override;
         void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
         void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
         void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;

cmvr-es/devices/camera/mechmind/src/mechmind_camera.cpp

@@ -50,6 +50,11 @@ bool MechmindCamera::init() {
     return true;
 }
 
+void MechmindCamera::getState(CameraState& state) {
+    std::lock_guard lock(dev_mtx_);
+    state = state_;
+}
+
 bool MechmindCamera::start() {
     try {
         std::lock_guard lock(dev_mtx_);
@@ -68,6 +73,7 @@ bool MechmindCamera::start() {
         return true;
     }
     catch (const std::exception& e) {
+        std::lock_guard lock(dev_mtx_);
         const string error_msg = "[MechmindCamera] (start): " + string(e.what());
         CMVR_LOG(ERROR) << error_msg;
         state_.is_error = true;
@@ -85,6 +91,7 @@ bool MechmindCamera::stop() {
         return true;
     }
     catch (const exception &e) {
+        std::lock_guard lock(dev_mtx_);
         const string error_msg = "[MechmindCamera] (stop): " + string(e.what());
         CMVR_LOG(ERROR) << error_msg;
         state_.is_error = true;
@@ -141,6 +148,7 @@ void MechmindCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
         }
     }
     catch (const exception &e) {
+        std::lock_guard lock(dev_mtx_);
         const string error_msg = "[MechmindCamera] (getRGBImage): " + string(e.what());
         CMVR_LOG(ERROR) << error_msg;
         state_.is_error = true;
@@ -175,6 +183,7 @@ void MechmindCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
         depth = cv::Mat(depthMap.height(), depthMap.width(), CV_32FC1, depthMap.data());
     }
     catch (const exception &e) {
+        std::lock_guard lock(dev_mtx_);
         const string error_msg = "[MechmindCamera] (getDepthImage): " + string(e.what());
         CMVR_LOG(ERROR) << error_msg;
         state_.is_error = true;
@@ -258,6 +267,7 @@ void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics
         textured_plc_to_rgbd_(textured_pcl, color, depth);
     }
     catch (const exception &e) {
+        std::lock_guard lock(dev_mtx_);
         const string error_msg = "[MechmindCamera] (getRGBDImages): " + string(e.what());
         CMVR_LOG(ERROR) << error_msg;
         state_.is_error = true;
@@ -269,12 +279,14 @@ void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics
 }
 
 void MechmindCamera::startRecording(const std::string& video_path) {
+    std::lock_guard lock(dev_mtx_);
     state_.is_error = true;
     state_.error_message = "startRecording is not implemented";
     CMVR_LOG(ERROR) << "[MechmindCamera] (startRecording): " << state_.error_message;
 }
 
 void MechmindCamera::stopRecording() {
+    std::lock_guard lock(dev_mtx_);
     state_.is_error = true;
     state_.error_message = "stopRecording is not implemented";
     CMVR_LOG(ERROR) << "[MechmindCamera] (stopRecording): " << state_.error_message;

cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h

@@ -5,13 +5,10 @@
 #pragma once
 
 #include <cstdint>
-#include <condition_variable>
-#include <chrono>
 #include <functional>
 #include <memory>
 #include <mutex>
 #include <string>
-#include <thread>
 #include <vector>
 
 #include <mujoco/mujoco.h>
@@ -42,6 +39,7 @@ public:
     bool init() override;
     bool start() override;
     bool stop() override;
+    void getState(CameraState& state) override;
 
     void setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn);
     void setFovyDeg(double fovy_deg);
@@ -60,11 +58,6 @@ private:
     bool initOffscreen_();
     void destroyOffscreen_();
     bool renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics);
-    void renderLoop_();
-    bool fetchCached_(cv::Mat& color,
-                      cv::Mat& depth,
-                      Rs2Intrinsics& intrinsics,
-                      bool consume_new_frame_only);
     bool ensureEncoder_(int width, int height, int fps);
     void setError_(const std::string& error);
     static void flipRgbAndDepth_(std::vector<unsigned char>& rgb,
@@ -73,14 +66,6 @@ private:
                                  int height);
     static void linearizeDepth_(const mjModel* model, std::vector<float>& depth);
 
-    struct CachedFrame {
-        cv::Mat color;
-        cv::Mat depth;
-        Rs2Intrinsics intrinsics{};
-        uint64_t frame_id{0};
-        bool valid{false};
-    };
-
 private:
     FetchRgbdFn fetch_rgbd_fn_;
     mutable std::mutex mtx_;
@@ -94,7 +79,6 @@ private:
     mjrContext context_{};
     bool scene_initialized_{false};
     bool context_initialized_{false};
-    mjData* render_data_{nullptr};
     int camera_id_{-1};
     int width_{640};
     int height_{480};
@@ -109,13 +93,6 @@ private:
     size_t stream_frame_index_{0};
     bool streaming_{false};
     std::shared_ptr<FfmpegEncoderInfo> rgb_encoder_;
-
-    mutable std::mutex cache_mtx_;
-    std::condition_variable cache_cv_;
-    CachedFrame latest_frame_;
-    std::thread render_thread_;
-    bool render_thread_running_{false};
-    bool render_stop_requested_{false};
 };
 
 } // namespace cmvr::device

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp

@@ -19,13 +19,6 @@ constexpr int kDefaultWidth = 640;
 constexpr int kDefaultHeight = 480;
 constexpr int kMaxGeom = 100000;
 
-// GLFW keeps process-global initialization state. MuJoCo cameras render on
-// independent threads, so serialize the one-time init/window creation path.
-std::mutex& glfwInitMutex() {
-    static std::mutex mutex;
-    return mutex;
-}
-
 int positiveOrDefault(const int value, const int fallback)
 {
     return value > 0 ? value : fallback;
@@ -59,6 +52,12 @@ MujocoCamera::~MujocoCamera()
     stop();
 }
 
+void MujocoCamera::getState(CameraState& state)
+{
+    std::lock_guard<std::mutex> lock(mtx_);
+    state = state_;
+}
+
 bool MujocoCamera::init()
 {
     std::lock_guard<std::mutex> lock(mtx_);
@@ -113,6 +112,10 @@ bool MujocoCamera::init()
         fovy_deg_ = model->cam_fovy[camera_id_];
     }
 
+    if (!initOffscreen_()) {
+        return false;
+    }
+
     state_.is_initialized = true;
     state_.is_opened = true;
     state_.fps = positiveOrDefault(config_.render().fps(), 30);
@@ -128,132 +131,37 @@ bool MujocoCamera::init()
 
 bool MujocoCamera::start()
 {
-    bool initialized = false;
+    if (!state_.is_initialized) {
-    {
-        std::lock_guard<std::mutex> lock(mtx_);
-        initialized = state_.is_initialized;
-    }
-    if (!initialized) {
         if (!init()) {
             return false;
         }
     }
-
+    std::lock_guard<std::mutex> lock(mtx_);
     auto world = world_.lock();
     if (world && !world->isRunning() && !world->start()) {
         setError_("[MujocoCamera] failed to start MuJoCo world: " + world->lastError());
         return false;
     }
-
-    bool use_external_frames = false;
-    {
-        std::lock_guard<std::mutex> lock(mtx_);
     state_.is_streaming = true;
     state_.is_opened = true;
-        use_external_frames = static_cast<bool>(fetch_rgbd_fn_);
-    }
-    std::thread stale_thread;
-    {
-        std::lock_guard<std::mutex> lock(cache_mtx_);
-        if (render_thread_running_) {
-            return true;
-        }
-        latest_frame_ = CachedFrame{};
-        last_frame_id_ = 0;
-        has_last_frame_id_ = false;
-        render_stop_requested_ = false;
-        render_thread_running_ = true;
-    }
-
-    {
-        std::lock_guard<std::mutex> lock(mtx_);
-        stale_thread = std::move(render_thread_);
-    }
-    if (stale_thread.joinable()) {
-        stale_thread.join();
-    }
-
-    try {
-        std::lock_guard<std::mutex> lock(mtx_);
-        render_thread_ = std::thread(&MujocoCamera::renderLoop_, this);
-    } catch (const std::exception& e) {
-        {
-            std::lock_guard<std::mutex> lock(cache_mtx_);
-            render_thread_running_ = false;
-            render_stop_requested_ = true;
-        }
-        cache_cv_.notify_all();
-        setError_("[MujocoCamera] failed to start render thread: " + std::string(e.what()));
-        return false;
-    }
-
-    // External PiP callbacks are not ready until the viewer enters its render
-    // loop, so let their polling thread warm up asynchronously.
-    if (use_external_frames) {
-        return true;
-    }
-
-    std::unique_lock<std::mutex> cache_lock(cache_mtx_);
-    const bool ready = cache_cv_.wait_for(
-        cache_lock,
-        std::chrono::seconds(5),
-        [this] { return latest_frame_.valid || !render_thread_running_ || render_stop_requested_; });
-    const bool has_frame = latest_frame_.valid;
-    cache_lock.unlock();
-    if (!ready || !has_frame) {
-        stop();
-        if (ready) {
-            setError_("[MujocoCamera] render thread stopped before producing a frame");
-        } else {
-            setError_("[MujocoCamera] timed out waiting for the first rendered frame");
-        }
-        return false;
-    }
     return true;
 }
 
 bool MujocoCamera::stop()
-{
-    {
-        std::lock_guard<std::mutex> lock(cache_mtx_);
-        render_stop_requested_ = true;
-    }
-    cache_cv_.notify_all();
-
-    std::thread thread_to_join;
 {
     std::lock_guard<std::mutex> lock(mtx_);
     state_.is_streaming = false;
     state_.is_opened = false;
-        thread_to_join = std::move(render_thread_);
+    destroyOffscreen_();
-    }
-    if (thread_to_join.joinable()) {
-        thread_to_join.join();
-    }
-
-    {
-        std::lock_guard<std::mutex> lock(cache_mtx_);
-        render_thread_running_ = false;
-        latest_frame_ = CachedFrame{};
-    }
-    cache_cv_.notify_all();
     return true;
 }
 
 void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn)
 {
-    bool render_thread_active = false;
-    {
-        std::lock_guard<std::mutex> lock(cache_mtx_);
-        render_thread_active = render_thread_running_;
-    }
-    if (render_thread_active) {
-        stop();
-    }
-
     std::lock_guard<std::mutex> lock(mtx_);
     fetch_rgbd_fn_ = std::move(fetch_rgbd_fn);
     if (fetch_rgbd_fn_) {
+        destroyOffscreen_();
         state_.is_initialized = true;
         state_.is_opened = true;
         state_.fps = positiveOrDefault(config_.render().fps(), 30);
@@ -301,10 +209,9 @@ void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics&
 
 bool MujocoCamera::startStreaming()
 {
-    if (!start()) {
+    if (!state_.is_initialized && !init()) {
         return false;
     }
-
     std::lock_guard<std::mutex> lock(mtx_);
     streaming_ = true;
     state_.is_streaming = true;
@@ -345,17 +252,12 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
     }
 
     frame_data.rgbImage = color.clone();
-    frame_data.intrinsics = intrinsics;
     CameraStreamEncodeOptions encode_options;
     encode_options.draw_timestamp = enable_stream_timestamp_;
-    encode_options.overlay = streamOverlay();
-    const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
-        intrinsics, color.cols, color.rows, color_to_encode.cols, color_to_encode.rows);
     if (!CameraStreamEncoder::encode(rgb_encoder_,
                                      color_to_encode,
                                      frame_data.rgbFrame,
                                      frame_data.bKey,
-                                     encode_intrinsics,
                                      encode_options)) {
         return false;
     }
@@ -365,6 +267,7 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
         const auto* depth_end = depth_begin + depth.total() * depth.elemSize();
         frame_data.depthFrame.assign(depth_begin, depth_end);
     }
+    frame_data.intrinsics = intrinsics;
     frame_data.width = color_to_encode.cols;
     frame_data.height = color_to_encode.rows;
     frame_data.fps = fps;
@@ -375,30 +278,15 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
 }
 
 bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
-{
-    FetchRgbdFn fetch_rgbd_fn;
-    bool consume_new_frame_only = false;
-    bool render_thread_active = false;
 {
     std::lock_guard<std::mutex> lock(mtx_);
-        fetch_rgbd_fn = fetch_rgbd_fn_;
+    if (fetch_rgbd_fn_) {
-        consume_new_frame_only = consume_new_frame_only_;
-    }
-
-    {
-        std::lock_guard<std::mutex> lock(cache_mtx_);
-        render_thread_active = render_thread_running_;
-    }
-
-    // Keep compatibility with callback-only cameras that have not been
-    // started. Once start() owns a polling thread, reads are cache-only.
-    if (fetch_rgbd_fn && !render_thread_active) {
         std::vector<unsigned char> rgb_raw;
         std::vector<float> depth_raw;
         int width = 0;
         int height = 0;
         std::uint64_t frame_id = 0;
-        if (!fetch_rgbd_fn(rgb_raw, depth_raw, width, height, frame_id)) {
+        if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) {
             return false;
         }
         if (width <= 0 || height <= 0) {
@@ -410,14 +298,11 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
         if (!depth_raw.empty() && static_cast<int>(depth_raw.size()) != width * height) {
             return false;
         }
-        {
+        if (consume_new_frame_only_ && has_last_frame_id_ && frame_id == last_frame_id_) {
-            std::lock_guard<std::mutex> lock(cache_mtx_);
-            if (consume_new_frame_only && has_last_frame_id_ && frame_id == last_frame_id_) {
             return false;
         }
         last_frame_id_ = frame_id;
         has_last_frame_id_ = true;
-        }
 
         cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
         cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
@@ -431,31 +316,7 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
         return true;
     }
 
-    return fetchCached_(color, depth, intrinsics, consume_new_frame_only);
+    return renderOffscreen_(color, depth, intrinsics);
-}
-
-bool MujocoCamera::fetchCached_(cv::Mat& color,
-                                cv::Mat& depth,
-                                Rs2Intrinsics& intrinsics,
-                                const bool consume_new_frame_only)
-{
-    std::lock_guard<std::mutex> lock(cache_mtx_);
-    if (!latest_frame_.valid || latest_frame_.color.empty()) {
-        return false;
-    }
-    if (consume_new_frame_only && has_last_frame_id_ &&
-        latest_frame_.frame_id == last_frame_id_) {
-        return false;
-    }
-
-    // cv::Mat copies are reference-counted; keep the cache immutable while the
-    // consumer reads the published frame and avoid a full image copy per poll.
-    color = latest_frame_.color;
-    depth = latest_frame_.depth;
-    intrinsics = latest_frame_.intrinsics;
-    last_frame_id_ = latest_frame_.frame_id;
-    has_last_frame_id_ = true;
-    return !color.empty();
 }
 
 bool MujocoCamera::initOffscreen_()
@@ -464,7 +325,6 @@ bool MujocoCamera::initOffscreen_()
         return true;
     }
 
-    std::lock_guard<std::mutex> glfw_lock(glfwInitMutex());
     if (!glfwInit()) {
         setError_("[MujocoCamera] glfwInit failed");
         return false;
@@ -491,27 +351,12 @@ bool MujocoCamera::initOffscreen_()
         return false;
     }
 
-    mjModel* model = nullptr;
-    {
     std::lock_guard<std::mutex> world_lock(world->mutex());
-        model = world->model();
+    const mjModel* model = world->model();
     if (model == nullptr) {
         setError_("[MujocoCamera] world model is null");
         return false;
     }
-
-        // MuJoCo clips rendering to the model's offscreen buffer. Make sure
-        // the buffer is large enough before creating this camera's context;
-        // otherwise a larger requested frame is only partially populated.
-        model->vis.global.offwidth = std::max(model->vis.global.offwidth, width_);
-        model->vis.global.offheight = std::max(model->vis.global.offheight, height_);
-    }
-
-    render_data_ = mj_makeData(model);
-    if (render_data_ == nullptr) {
-        setError_("[MujocoCamera] failed to allocate render data");
-        return false;
-    }
     mjv_makeScene(model, &scene_, kMaxGeom);
     scene_initialized_ = true;
     mjr_makeContext(model, &context_, mjFONTSCALE_150);
@@ -521,116 +366,9 @@ bool MujocoCamera::initOffscreen_()
         setError_("[MujocoCamera] MuJoCo offscreen buffer is not available");
         return false;
     }
-    if (context_.offWidth < width_ || context_.offHeight < height_) {
-        setError_("[MujocoCamera] offscreen buffer is smaller than requested frame: " +
-                  std::to_string(context_.offWidth) + "x" +
-                  std::to_string(context_.offHeight) + " < " +
-                  std::to_string(width_) + "x" + std::to_string(height_));
-        return false;
-    }
     return true;
 }
 
-void MujocoCamera::renderLoop_()
-{
-    FetchRgbdFn external_fetch;
-    {
-        std::lock_guard<std::mutex> lock(mtx_);
-        external_fetch = fetch_rgbd_fn_;
-    }
-
-    const bool use_external_frames = static_cast<bool>(external_fetch);
-    if (!use_external_frames && !initOffscreen_()) {
-        destroyOffscreen_();
-        {
-            std::lock_guard<std::mutex> lock(cache_mtx_);
-            render_thread_running_ = false;
-        }
-        cache_cv_.notify_all();
-        return;
-    }
-
-    const int fps = positiveOrDefault(config_.render().fps(), 30);
-    const auto period = std::chrono::duration<double>(1.0 / static_cast<double>(fps));
-    const auto period_ticks = std::chrono::duration_cast<std::chrono::steady_clock::duration>(period);
-    auto next_tick = std::chrono::steady_clock::now();
-
-    while (true) {
-        {
-            std::lock_guard<std::mutex> lock(cache_mtx_);
-            if (render_stop_requested_) {
-                break;
-            }
-        }
-
-        cv::Mat color;
-        cv::Mat depth;
-        Rs2Intrinsics intrinsics{};
-        uint64_t external_frame_id = 0;
-        bool got_frame = false;
-
-        if (use_external_frames) {
-            std::vector<unsigned char> rgb_raw;
-            std::vector<float> depth_raw;
-            int width = 0;
-            int height = 0;
-            if (external_fetch(rgb_raw, depth_raw, width, height, external_frame_id) &&
-                width > 0 && height > 0 &&
-                static_cast<int>(rgb_raw.size()) == width * height * 3 &&
-                (depth_raw.empty() || static_cast<int>(depth_raw.size()) == width * height)) {
-                cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
-                cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
-                if (!depth_raw.empty()) {
-                    cv::Mat dep(height, width, CV_32FC1, depth_raw.data());
-                    depth = dep.clone();
-                }
-                fillIntrinsics(width, height, intrinsics);
-                got_frame = !color.empty();
-            }
-        } else {
-            got_frame = renderOffscreen_(color, depth, intrinsics);
-        }
-
-        if (got_frame) {
-            {
-                std::lock_guard<std::mutex> lock(cache_mtx_);
-                latest_frame_.color = std::move(color);
-                latest_frame_.depth = std::move(depth);
-                latest_frame_.intrinsics = intrinsics;
-                latest_frame_.frame_id = use_external_frames && external_frame_id != 0
-                    ? external_frame_id
-                    : latest_frame_.frame_id + 1;
-                latest_frame_.valid = true;
-            }
-            {
-                std::lock_guard<std::mutex> lock(mtx_);
-                clear_error_();
-            }
-            cache_cv_.notify_all();
-        }
-
-        next_tick += period_ticks;
-        std::unique_lock<std::mutex> lock(cache_mtx_);
-        if (cache_cv_.wait_until(lock, next_tick, [this] { return render_stop_requested_; })) {
-            break;
-        }
-
-        const auto now = std::chrono::steady_clock::now();
-        if (next_tick < now) {
-            next_tick = now + period_ticks;
-        }
-    }
-
-    if (!use_external_frames) {
-        destroyOffscreen_();
-    }
-    {
-        std::lock_guard<std::mutex> lock(cache_mtx_);
-        render_thread_running_ = false;
-    }
-    cache_cv_.notify_all();
-}
-
 void MujocoCamera::destroyOffscreen_()
 {
     if (window_ != nullptr) {
@@ -644,10 +382,6 @@ void MujocoCamera::destroyOffscreen_()
         mjv_freeScene(&scene_);
         scene_initialized_ = false;
     }
-    if (render_data_ != nullptr) {
-        mj_deleteData(render_data_);
-        render_data_ = nullptr;
-    }
     if (window_ != nullptr) {
         glfwDestroyWindow(window_);
         window_ = nullptr;
@@ -660,8 +394,7 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
         setError_("[MujocoCamera] camera is not initialized: " + id_);
         return false;
     }
-    if (window_ == nullptr || !context_initialized_ || !scene_initialized_) {
+    if (!initOffscreen_()) {
-        setError_("[MujocoCamera] offscreen renderer is not initialized: " + id_);
         return false;
     }
 
@@ -675,27 +408,15 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
     std::vector<unsigned char> rgb(static_cast<std::size_t>(width_) * height_ * 3);
     std::vector<float> depth_raw(static_cast<std::size_t>(width_) * height_);
 
-    mjModel* model = nullptr;
     {
-        std::unique_lock<std::mutex> world_lock(world->mutex(), std::try_to_lock);
+        std::lock_guard<std::mutex> world_lock(world->mutex());
-        if (!world_lock.owns_lock()) {
+        mjModel* model = world->model();
-            // Never make the simulation wait for a camera frame. The next
+        mjData* data = world->data();
-            // scheduled capture will use a newer state if this one is busy.
+        if (model == nullptr || data == nullptr) {
-            return false;
-        }
-        model = world->model();
-        const mjData* data = world->data();
-        if (model == nullptr || data == nullptr || render_data_ == nullptr) {
             setError_("[MujocoCamera] world model/data is null");
             return false;
         }
 
-        // Keep the world lock limited to the state copy. GPU rendering runs on
-        // the camera thread using its private data snapshot.
-        mjv_copyData(render_data_, model, data);
-    }
-
-    {
         camera_.type = mjCAMERA_FIXED;
         camera_.fixedcamid = camera_id_;
         camera_.trackbodyid = -1;
@@ -706,7 +427,7 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
         viewport.width = width_;
         viewport.height = height_;
 
-        mjv_updateScene(model, render_data_, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
+        mjv_updateScene(model, data, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
         mjr_render(viewport, &scene_, &context_);
         mjr_readPixels(rgb.data(), depth_raw.data(), viewport, &context_);
         flipRgbAndDepth_(rgb, depth_raw, width_, height_);
@@ -714,12 +435,13 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
     }
 
     cv::Mat rgb_mat(height_, width_, CV_8UC3, rgb.data());
-    // mjr_readPixels returns RGB, while the rest of the camera API exposes
+    color = rgb_mat.clone();
-    // OpenCV-compatible BGR frames (as UVC and RealSense do).
-    cv::cvtColor(rgb_mat, color, cv::COLOR_RGB2BGR);
     cv::Mat depth_mat(height_, width_, CV_32FC1, depth_raw.data());
     depth = depth_mat.clone();
     fillIntrinsics(width_, height_, intrinsics);
+    ++last_frame_id_;
+    has_last_frame_id_ = true;
+    clear_error_();
     return true;
 }
 

cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h

@@ -24,6 +24,7 @@ namespace cmvr::device{
         bool init() override;
         bool start() override;
         bool stop() override;
+        void getState(CameraState& state) override;
         void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
         void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
         void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;

cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp

@@ -145,6 +145,12 @@ RealsenseCamera::~RealsenseCamera() {
     }
 }
 
+void RealsenseCamera::getState(CameraState& state)
+{
+    std::lock_guard lock(ctrl_mtx_);
+    state = state_;
+}
+
 bool RealsenseCamera::init() {
     try {
         std::lock_guard lock(ctrl_mtx_);
@@ -315,8 +321,20 @@ bool RealsenseCamera::stop() {
             CMVR_LOG(WARNING) << "[RealsenseCamera] (stop): stopRecording failed: " << e.what();
         }
     }
+    std::shared_ptr<std::thread> stream_thread_to_join;
+    {
         std::lock_guard lock(ctrl_mtx_);
         clear_error_();
+        stream_count_ = 0;
+        state_.is_streaming = false;
+        state_.is_recording = false;
+        stream_thread_to_join = stream_thread_;
+        stream_thread_.reset();
+    }
+    if (stream_thread_to_join && stream_thread_to_join->joinable()) {
+        stream_thread_to_join->join();
+    }
+    std::lock_guard lock(ctrl_mtx_);
     if (!state_.is_opened || !state_.is_initialized) {
         state_.is_opened = false;
         return true;
@@ -525,9 +543,12 @@ void RealsenseCamera::startRecording(const std::string &video_path) {
 
         // 1. 确定编码格式对应的AVCodecID（H.264/H.265）
         AVCodecID codec_id;
-        if (codec_ == "H264" || codec_ == "h264") {
+        if (codec_ == "H264" || codec_ == "h264" ||
+            codec_ == "H264_QSV" || codec_ == "h264_qsv") {
             codec_id = AV_CODEC_ID_H264;
-        } else if (codec_ == "H265" || codec_ == "hevc") {
+        } else if (codec_ == "H265" || codec_ == "h265" || codec_ == "HEVC" || codec_ == "hevc" ||
+                   codec_ == "H265_QSV" || codec_ == "h265_qsv" ||
+                   codec_ == "HEVC_QSV" || codec_ == "hevc_qsv") {
             codec_id = AV_CODEC_ID_HEVC;
         } else {
             state_.is_error = true;
@@ -535,7 +556,8 @@ void RealsenseCamera::startRecording(const std::string &video_path) {
             CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message;
             return;
         }
-        const char* format_name = (codec_ == "H265" || codec_ == "h265" || codec_ == "HEVC" || codec_ == "hevc") ? "mp4" : "avi";
+        const bool is_hevc = codec_id == AV_CODEC_ID_HEVC;
+        const char* format_name = is_hevc ? "mp4" : "avi";
         auto ret = avformat_alloc_output_context2(&format_context_, nullptr, format_name, output_path_.c_str());
         // 2. 创建输出格式上下文（封装器核心）
         if (ret < 0) {
@@ -720,11 +742,15 @@ void RealsenseCamera::streaming_worker_() {
 
         bool success = false;
         is_streaming_running = true;
+        uint64_t frame_sequence = 0;
+        const uint64_t stream_epoch = static_cast<uint64_t>(
+            std::chrono::duration_cast<std::chrono::nanoseconds>(
+                std::chrono::steady_clock::now().time_since_epoch()).count());
 
         // 处于流传输或者录像状态时就不退出线程
         while (state_.is_streaming || state_.is_recording) {
             // 记录当前帧处理开始时间
-            auto frame_start_time = std::chrono::high_resolution_clock::now();
+            const auto frame_start_time = std::chrono::steady_clock::now();
 
             rs2::frameset frames;
             frames = get_frameset(true);
@@ -800,23 +826,6 @@ void RealsenseCamera::streaming_worker_() {
                                                       frame_data.bKey,
                                                       encode_options);
             }
-            CameraStreamEncodeOptions encode_options;
-            encode_options.draw_timestamp = enable_stream_timestamp_;
-            encode_options.overlay = streamOverlay();
-            const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
-                frame_data.intrinsics,
-                frame_data.rgbImage.cols,
-                frame_data.rgbImage.rows,
-                rgb_to_encode.cols,
-                rgb_to_encode.rows);
-            success = CameraStreamEncoder::encode(rgbEncoder_,
-                                                  rgb_to_encode,
-                                                  frame_data.rgbFrame,
-                                                  frame_data.bKey,
-                                                  encode_intrinsics,
-                                                  encode_options);
-            // 深度图编码
-            // success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey);
             if (needs_depth && frame_data.depthFrame.empty())
                 success = false;
             if (success) {
@@ -844,7 +853,7 @@ void RealsenseCamera::streaming_worker_() {
             }
             // 计算从帧开始到现在的总耗时
             auto total_duration = std::chrono::duration_cast<std::chrono::milliseconds>(
-                std::chrono::high_resolution_clock::now() - frame_start_time
+                std::chrono::steady_clock::now() - frame_start_time
             ).count();
 
             // 计算需要休眠的时间(确保总耗时达到frame_interval)
@@ -1010,13 +1019,25 @@ bool RealsenseCamera::startStreaming()
 }
 
 void RealsenseCamera::stopStreaming()
+{
+    std::shared_ptr<std::thread> stream_thread_to_join;
     {
         std::lock_guard lock(ctrl_mtx_);
+        if (stream_count_ > 0) {
             stream_count_--;
+        }
         if (stream_count_ == 0)
         {
         // 当前已经没有正在使用的流了，编码采集线程状态修改
             state_.is_streaming = false;
+            if (!state_.is_recording) {
+                stream_thread_to_join = stream_thread_;
+                stream_thread_.reset();
+            }
+        }
+    }
+    if (stream_thread_to_join && stream_thread_to_join->joinable()) {
+        stream_thread_to_join->join();
     }
 }
 

cmvr-es/devices/camera/uvc_camera/CMakeLists.txt

@@ -2,7 +2,18 @@ add_library(uvc_camera SHARED src/uvc_camera.cpp)
 
 target_include_directories(uvc_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-target_link_libraries(uvc_camera PUBLIC glog opencv_core opencv_imgproc cmvr_es::proto cmvr_es::device::camera_stream_encoder)
+target_link_libraries(uvc_camera PUBLIC
+        glog
+        opencv_core
+        opencv_imgproc
+        avcodec
+        avdevice
+        avformat
+        avutil
+        swscale
+        cmvr_es::proto
+        cmvr_es::device::camera_stream_encoder
+)
 
 add_library(cmvr_es::device::uvc_camera ALIAS uvc_camera)
 install(TARGETS uvc_camera LIBRARY DESTINATION lib)

cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h

@@ -5,6 +5,9 @@
 #ifndef CMVR_ES_UVC_CAMERA_H
 #define CMVR_ES_UVC_CAMERA_H
 
+#include <atomic>
+#include <condition_variable>
+
 #include "common/base/ring_buffer.h"
 #include "camera/abstract_camera.h"
 #include "devices/camera/common/include/camera_stream_encoder.h"
@@ -29,6 +32,7 @@ namespace cmvr::device {
         bool init() override;
         bool start() override;
         bool stop() override;
+        void getState(CameraState& state) override;
         void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
         void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
         void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
@@ -44,6 +48,16 @@ namespace cmvr::device {
     private:
         void streaming_worker_();
         void recording_worker_();
+        void cleanup_recording_resources_();
+        bool open_capture_();
+        void close_capture_();
+        void capture_worker_();
+        bool wait_for_capture_frame_(AVFrame* frame,
+                                     uint64_t& last_sequence,
+                                     int64_t& capture_monotonic_ns,
+                                     int64_t& capture_utc_ns);
+        bool convert_capture_frame_to_bgr_(const AVFrame* frame, cv::Mat& bgr_frame);
+        static int interrupt_capture_(void* opaque);
 
         int fps_;
         int width_;
@@ -51,7 +65,6 @@ namespace cmvr::device {
         int encode_width_;
         int encode_height_;
         std::string serial_;
-        cv::VideoCapture cap_;
         size_t buffer_size_;
         std::string codec_;
         bool enable_stream_timestamp_{false};
@@ -62,13 +75,24 @@ namespace cmvr::device {
         std::string current_video_path_;
 
         std::mutex ctrl_mtx_{};
-        std::unique_ptr<cv::VideoWriter> video_writer_;
         std::shared_ptr<std::thread> stream_thread_;
         std::shared_ptr<std::thread> recording_thread_;
+        std::shared_ptr<std::thread> capture_thread_;
 
-
+        AVFormatContext* capture_format_context_ = nullptr;
-        cv::Mat latest_frame_;  // 存储最新帧
+        AVCodecContext* capture_decoder_context_ = nullptr;
-        std::mutex frame_mutex_;  // 保护最新帧的访问
+        AVBufferRef* capture_hw_device_context_ = nullptr;
+        SwsContext* capture_sws_context_ = nullptr;
+        int capture_video_stream_index_ = -1;
+        std::atomic<bool> capture_running_{false};
+        std::mutex capture_frame_mutex_;
+        std::mutex capture_conversion_mutex_;
+        std::condition_variable capture_frame_cv_;
+        AVFrame* latest_capture_frame_ = nullptr;
+        uint64_t latest_capture_sequence_ = 0;
+        int64_t latest_capture_monotonic_ns_ = 0;
+        int64_t latest_capture_utc_ns_ = 0;
+        std::string capture_error_;
 
         std::string output_path_;
         bool is_recording_ = false;

cmvr-es/service/grpc/include/grpc_camera_service.h

@@ -9,12 +9,14 @@
 #include "common/base/grpc_utils.h"
 #include "manager/device_manager/include/device_manager.h"
 #include "devices/camera/abstract_camera.h"
+#include "service/grpc/include/grpc_camera_stream_policy.h"
 
 namespace cmvr::service {
 
     class gRPCCameraServiceImpl final: public api::CameraService::Service {
     public:
-        gRPCCameraServiceImpl();
+        explicit gRPCCameraServiceImpl(
+            CameraStreamLowLatencyConfig stream_config = {});
         ~gRPCCameraServiceImpl() override = default;
         grpc::Status GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) override;
         grpc::Status StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) override;
@@ -24,11 +26,13 @@ namespace cmvr::service {
         grpc::Status GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) override;
         grpc::Status StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) override;
         grpc::Status StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) override;
+        grpc::Status ControlPtz(grpc::ServerContext* context, const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response) override;
         grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream) override;
         grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream) override;
         grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream) override;
     private:
         device::DeviceManager& dmgr_;
+        CameraStreamLowLatencyConfig stream_config_;
 
         //双向流读写线程
         std::shared_ptr<std::thread> read_thread_ = nullptr;

cmvr-es/service/grpc/src/grpc_camera_service.cpp

@@ -1,10 +1,15 @@
 #include "common/base/logging/logger.h"
+#include "manager/media_source_hub/include/device_media_source_adapter.h"
 //
 // Created by xtkuang on 2025/6/1.
 //
 
 #include "../include/grpc_camera_service.h"
 
+#include <algorithm>
+#include <atomic>
+#include <chrono>
+#include <cstdint>
 #include <limits>
 #include <thread>
 
@@ -21,9 +26,88 @@ grpc::Status failResponse(ResponseT* response, const std::string& message) {
     setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
     return grpc::Status::OK;
 }
+
+bool toPtzCommand(cmvr::api::ControlPtzCommand_Command command, PtzCommand& out)
+{
+    switch (command) {
+        case cmvr::api::ControlPtzCommand_Command_TILT_UP:
+            out = PtzCommand::TiltUp;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_TILT_DOWN:
+            out = PtzCommand::TiltDown;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_PAN_LEFT:
+            out = PtzCommand::PanLeft;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_PAN_RIGHT:
+            out = PtzCommand::PanRight;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_UP_LEFT:
+            out = PtzCommand::UpLeft;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_UP_RIGHT:
+            out = PtzCommand::UpRight;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_DOWN_LEFT:
+            out = PtzCommand::DownLeft;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_DOWN_RIGHT:
+            out = PtzCommand::DownRight;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_ZOOM_IN:
+            out = PtzCommand::ZoomIn;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_ZOOM_OUT:
+            out = PtzCommand::ZoomOut;
+            return true;
+        case cmvr::api::ControlPtzCommand_Command_PAN_AUTO:
+            out = PtzCommand::PanAuto;
+            return true;
+        default:
+            return false;
+    }
 }
 
-gRPCCameraServiceImpl::gRPCCameraServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
+// The legacy depth/RGBD RPCs acquire the camera's shared producer directly
+// instead of going through MediaSourceHub.  Keep that lease exception-safe:
+// cancellation, a failed Write(), or any conversion error must release exactly
+// the one startStreaming() reference acquired by this call.
+class CameraStreamingLease final {
+public:
+    explicit CameraStreamingLease(std::shared_ptr<AbstractCamera> camera)
+        : camera_(std::move(camera)) {
+        active_ = camera_ && camera_->startStreaming();
+    }
+
+    ~CameraStreamingLease() {
+        if (!active_ || !camera_) {
+            return;
+        }
+        try {
+            camera_->stopStreaming();
+        } catch (const std::exception& e) {
+            CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease: "
+                            << e.what();
+        } catch (...) {
+            CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease";
+        }
+    }
+
+    CameraStreamingLease(const CameraStreamingLease&) = delete;
+    CameraStreamingLease& operator=(const CameraStreamingLease&) = delete;
+
+    explicit operator bool() const noexcept { return active_; }
+
+private:
+    std::shared_ptr<AbstractCamera> camera_;
+    bool active_{false};
+};
+}
+
+gRPCCameraServiceImpl::gRPCCameraServiceImpl(
+    CameraStreamLowLatencyConfig stream_config)
+    : dmgr_(DeviceManager::getInstance()),
+      stream_config_(stream_config) {}
 
 grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
     const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response)
@@ -48,14 +132,6 @@ grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
         response->mutable_state()->set_fps(state.fps);
         response->mutable_state()->set_width(state.width);
         response->mutable_state()->set_height(state.height);
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetStatus): success, id=" << dev_id
-                        << ", initialized=" << state.is_initialized
-                        << ", opened=" << state.is_opened
-                        << ", streaming=" << state.is_streaming
-                        << ", recording=" << state.is_recording
-                        << ", error=" << state.is_error
-                        << ", size=" << state.width << "x" << state.height
-                        << ", fps=" << state.fps;
         return grpc::Status::OK;
     }
     catch(const exception &e) {
@@ -81,7 +157,6 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context,
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartCamera): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const exception &e) {
@@ -107,7 +182,6 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context,
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopCamera): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -131,7 +205,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
         }
         Rs2Intrinsics intrinsics = {0};
         dev->getRGBImage(image,intrinsics);
-        response->mutable_header()->set_success(true);
+        if (image.empty()) {
+            return failResponse(response, "Camera returned an empty RGB image: " + dev_id);
+        }
 
         response->mutable_intrinsics()->set_fx(intrinsics.fx);
         response->mutable_intrinsics()->set_fy(intrinsics.fy);
@@ -161,10 +237,6 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
         response->mutable_color_frame()->set_height(image.rows);
         response->mutable_color_frame()->set_width(image.cols);
         response->mutable_color_frame()->set_codec("none");
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImage): success, id=" << dev_id
-                        << ", size=" << image.cols << "x" << image.rows
-                        << ", cv_type=" << image.type()
-                        << ", bytes=" << image.total() * image.elemSize();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -188,6 +260,9 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
         }
         Rs2Intrinsics intrinsics = {0};
         dev->getDepthImage(image,intrinsics);
+        if (image.empty()) {
+            return failResponse(response, "Camera returned an empty depth image: " + dev_id);
+        }
         response->mutable_header()->set_success(true);
 
         response->mutable_intrinsics()->set_fx(intrinsics.fx);
@@ -221,10 +296,6 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
         response->mutable_depth_frame()->set_height(image.rows);
         response->mutable_depth_frame()->set_width(image.cols);
         response->mutable_depth_frame()->set_codec("none");
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImage): success, id=" << dev_id
-                        << ", size=" << image.cols << "x" << image.rows
-                        << ", cv_type=" << image.type()
-                        << ", bytes=" << image.total() * image.elemSize();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -248,6 +319,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
         }
         Rs2Intrinsics intrinsics = {0};
         dev->getRGBDImages(color_image,depth_image, intrinsics);
+        if (color_image.empty()) {
+            return failResponse(response, "Camera returned an empty RGB image: " + dev_id);
+        }
+        if (depth_image.empty()) {
+            return failResponse(response, "Camera returned an empty depth image: " + dev_id);
+        }
         response->mutable_header()->set_success(true);
 
         response->mutable_intrinsics()->set_fx(intrinsics.fx);
@@ -290,16 +367,14 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
         else if (depth_image.type() == CV_32FC1) {
             response->mutable_depth_frame()->set_type(api::FrameData::F32C1);
         }
+        else {
+            return failResponse(response, "unsupported depth image type");
+        }
         response->mutable_depth_frame()->set_data(
             reinterpret_cast<const char*>(depth_image.data), depth_image.total() * depth_image.elemSize());
         response->mutable_depth_frame()->set_height(depth_image.rows);
         response->mutable_depth_frame()->set_width(depth_image.cols);
         response->mutable_depth_frame()->set_codec("none");
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImages): success, id=" << dev_id
-                        << ", color_size=" << color_image.cols << "x" << color_image.rows
-                        << ", color_type=" << color_image.type()
-                        << ", depth_size=" << depth_image.cols << "x" << depth_image.rows
-                        << ", depth_type=" << depth_image.type();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -323,8 +398,6 @@ grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context,
         dev->startRecording(request->video_path());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartRecording): success, id=" << dev_id
-                        << ", path=" << request->video_path();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -348,7 +421,50 @@ grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context,
         dev->stopRecording();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopRecording): success, id=" << dev_id;
+        return grpc::Status::OK;
+    }
+    catch (exception &e) {
+        response->mutable_header()->set_success(false);
+        response->mutable_header()->set_error_message(e.what());
+        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        return grpc::Status::OK;
+    }
+}
+
+grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context,
+    const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response)
+{
+    try {
+        string dev_id = request->header().device_id();
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (ControlPtz): id=" << dev_id
+                       << ", command=" << request->command()
+                       << ", action=" << request->action()
+                       << ", speed=" << request->speed();
+        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
+        if (!dev) {
+            return failResponse(response, "Camera device not found: " + dev_id);
+        }
+
+        PtzCommand command{};
+        if (!toPtzCommand(request->command(), command)) {
+            return failResponse(response, "Invalid PTZ command");
+        }
+
+        if (request->action() != api::ControlPtzCommand_Action_START &&
+            request->action() != api::ControlPtzCommand_Action_STOP) {
+            return failResponse(response, "Invalid PTZ action");
+        }
+        const bool stop = request->action() == api::ControlPtzCommand_Action_STOP;
+        if (!dev->controlPtz(command, stop, static_cast<int>(request->speed()))) {
+            CameraState state{};
+            dev->getState(state);
+            const std::string error_message =
+                state.error_message.empty() ? "Failed to control PTZ: " + dev_id : state.error_message;
+            return failResponse(response, error_message);
+        }
+
+        response->mutable_header()->set_success(true);
+        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -365,9 +481,11 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
     try {
         //读取首次传递的数据，获取设备id
         api::GetDepthImageStreamCommand_Request request;
-        stream->Read(&request);
+        if (!stream->Read(&request)) {
+            return grpc::Status::OK;
+        }
         string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id;
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start,id=" << dev_id;
         const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
         if (!dev) {
             api::GetDepthImageStreamCommand_Feedback response;
@@ -377,9 +495,17 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
             stream->Write(response);
             return grpc::Status::OK;
         }
+        CameraStreamingLease stream_lease(dev);
+        if (!stream_lease) {
+            api::GetDepthImageStreamCommand_Feedback response;
+            response.mutable_header()->set_success(false);
+            response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id);
+            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+            stream->Write(response);
+            return grpc::Status::OK;
+        }
+
         int nFrameCount = 0;
-        dev->startStreaming();
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start streaming success, id=" << dev_id;
         size_t index = 0;
         while (true)
         {
@@ -391,8 +517,8 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
 
             api::GetDepthImageStreamCommand_Feedback response;
             cmvr::device::StreamFrameData frame_data;
-            dev->getEncodedFrame(frame_data,index);
+            if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
-            if (!frame_data.rgbFrame.empty()) {
+                !frame_data.depthFrame.empty()) {
                 response.mutable_header()->set_success(true);
                 setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
                 response.mutable_depth_frame()->set_type(api::FrameData::U16C1);
@@ -402,158 +528,6 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
                 response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width);
                 response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height);
 
-                response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx);
-                for (int i = 0; i < 5 ; i++) {
-                    response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
-                }
-
-                response.set_seq_no(nFrameCount++);
-
-                grpc::WriteOptions options;
-                options.set_last_message();
-                if (!stream->Write(response)) {
-                    CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
-                    break;
-                }
-            }
-        }
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id;
-        dev->stopStreaming();
-        return grpc::Status::OK;
-    }
-    catch (exception &e) {
-        api::GetDepthImageStreamCommand_Feedback response;
-        response.mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-        stream->Write(response);
-        return grpc::Status::OK;
-    }
-}
-grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* context
-    , grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream){
-    try {
-        //读取首次传递的数据，获取设备id
-        api::GetRGBDImagesStreamCommand_Request request;
-        stream->Read(&request);
-        string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            api::GetRGBDImagesStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Camera device not found: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-        int nFrameCount = 0;
-        dev->startStreaming();
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start streaming success, id=" << dev_id;
-        size_t index = 0;
-        while (true)
-        {
-            if (context->IsCancelled())
-            {
-                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBDImagesStream) context is cancelled,id=" << dev_id;
-                break;
-            }
-
-            api::GetRGBDImagesStreamCommand_Feedback response;
-            cmvr::device::StreamFrameData frame_data;
-            dev->getEncodedFrame(frame_data,index);
-            if (!frame_data.rgbFrame.empty()) {
-                response.mutable_header()->set_success(true);
-                response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size());
-                response.mutable_color_frame()->set_is_key_frame(frame_data.bKey);
-                response.mutable_color_frame()->set_codec(frame_data.codec);
-                response.mutable_color_frame()->set_width(frame_data.width);
-                response.mutable_color_frame()->set_height(frame_data.height);
-
-                response.mutable_depth_frame()->set_type(api::FrameData::U16C1);
-                response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
-                response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
-                response.mutable_depth_frame()->set_codec("none");
-                response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width);
-                response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height);
-
-                response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx);
-                for (int i = 0; i < 5 ; i++) {
-                    response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
-                }
-
-                response.set_seq_no(nFrameCount++);
-
-                grpc::WriteOptions options;
-                options.set_last_message();
-                if (!stream->Write(response)) {
-                    CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
-                    break;
-                }
-            }
-        }
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id;
-        dev->stopStreaming();
-        return grpc::Status::OK;
-    }
-    catch (exception &e) {
-        api::GetRGBDImagesStreamCommand_Feedback response;
-        response.mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-        stream->Write(response);
-        return grpc::Status::OK;
-    }
-}
-grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream){
-    try {
-        //读取首次传递的数据，获取设备id
-        api::GetRGBImageStreamCommand_Request request;
-        stream->Read(&request);
-        string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            api::GetRGBImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Camera device not found: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-        int nFrameCount = 0;
-        dev->startStreaming();
-        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start streaming success, id=" << dev_id;
-        size_t last_sent_index = std::numeric_limits<size_t>::max();
-        while (true)
-        {
-            if (context->IsCancelled())
-            {
-                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id;
-                break;
-            }
-
-            api::GetRGBImageStreamCommand_Feedback response;
-            cmvr::device::StreamFrameData frame_data;
-            size_t next_index = last_sent_index;
-            if (!dev->getLatestEncodedFrame(frame_data, next_index) ||
-                frame_data.rgbFrame.empty() ||
-                next_index == last_sent_index) {
-                std::this_thread::sleep_for(std::chrono::milliseconds(1));
-                continue;
-            }
-
-            response.mutable_header()->set_success(true);
-            response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size());
-            response.mutable_color_frame()->set_is_key_frame(frame_data.bKey);
-            response.mutable_color_frame()->set_codec(frame_data.codec);
-            response.mutable_color_frame()->set_width(frame_data.width);
-            response.mutable_color_frame()->set_height(frame_data.height);
-
                 response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
                 response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
                 response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
@@ -568,14 +542,380 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
                     CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
                     break;
                 }
-            last_sent_index = next_index;
             }
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id;
+        }
-        dev->stopStreaming();
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): end,id=" << dev_id;
         return grpc::Status::OK;
     }
-    catch (exception &e) {
+    catch (const exception &e) {
-        api::GetRGBImageStreamCommand_Feedback response;
+        api::GetDepthImageStreamCommand_Feedback response;
+        response.mutable_header()->set_success(false);
+        response.mutable_header()->set_error_message(e.what());
+        setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+        stream->Write(response);
+        return grpc::Status::OK;
+    }
+}
+grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* context
+    , grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream){
+    try {
+        //读取首次传递的数据，获取设备id
+        api::GetRGBDImagesStreamCommand_Request request;
+        if (!stream->Read(&request)) {
+            return grpc::Status::OK;
+        }
+        string dev_id = request.header().device_id();
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start,id=" << dev_id;
+        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
+        if (!dev) {
+            api::GetRGBDImagesStreamCommand_Feedback response;
+            response.mutable_header()->set_success(false);
+            response.mutable_header()->set_error_message("Camera device not found: " + dev_id);
+            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+            stream->Write(response);
+            return grpc::Status::OK;
+        }
+        CameraStreamingLease stream_lease(dev);
+        if (!stream_lease) {
+            api::GetRGBDImagesStreamCommand_Feedback response;
+            response.mutable_header()->set_success(false);
+            response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id);
+            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+            stream->Write(response);
+            return grpc::Status::OK;
+        }
+
+        int nFrameCount = 0;
+        size_t index = 0;
+        while (true)
+        {
+            if (context->IsCancelled())
+            {
+                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBDImagesStream) context is cancelled,id=" << dev_id;
+                break;
+            }
+
+            api::GetRGBDImagesStreamCommand_Feedback response;
+            cmvr::device::StreamFrameData frame_data;
+            if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
+                !frame_data.rgbFrame.empty() &&
+                !frame_data.depthFrame.empty()) {
+                response.mutable_header()->set_success(true);
+                setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+                response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size());
+                response.mutable_color_frame()->set_is_key_frame(frame_data.bKey);
+                response.mutable_color_frame()->set_codec(frame_data.codec);
+                response.mutable_color_frame()->set_width(frame_data.width);
+                response.mutable_color_frame()->set_height(frame_data.height);
+
+                response.mutable_depth_frame()->set_type(api::FrameData::U16C1);
+                response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
+                response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
+                response.mutable_depth_frame()->set_codec("none");
+                response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width);
+                response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height);
+
+                response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
+                response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
+                response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
+                response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy);
+                for (int i = 0; i < 5 ; i++) {
+                    response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
+                }
+
+                response.set_seq_no(nFrameCount++);
+
+                if (!stream->Write(response)) {
+                    CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
+                    break;
+                }
+            }
+        }
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id;
+        return grpc::Status::OK;
+    }
+    catch (const exception &e) {
+        api::GetRGBDImagesStreamCommand_Feedback response;
+        response.mutable_header()->set_success(false);
+        response.mutable_header()->set_error_message(e.what());
+        setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+        stream->Write(response);
+        return grpc::Status::OK;
+    }
+}
+grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream){
+    try {
+        //读取首次传递的数据，获取设备id
+        api::GetRGBImageStreamCommand_Request request;
+        if (!stream->Read(&request)) {
+            return grpc::Status::OK;
+        }
+        string dev_id = request.header().device_id();
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id
+                       << ", max_pending_frames=" << stream_config_.max_pending_frames
+                       << ", max_frame_age_ms=" << stream_config_.max_frame_age.count();
+        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
+        if (!dev) {
+            api::GetRGBImageStreamCommand_Feedback response;
+            response.mutable_header()->set_success(false);
+            response.mutable_header()->set_error_message("Camera device not found: " + dev_id);
+            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+            stream->Write(response);
+            return grpc::Status::OK;
+        }
+        auto& media_hub = cmvr::media::globalMediaSourceHub();
+        const std::string track_id = cmvr::media::cameraColorTrackId(dev_id);
+        if (!cmvr::media::ensureCameraMediaSource(media_hub, dev)) {
+            api::GetRGBImageStreamCommand_Feedback response;
+            response.mutable_header()->set_success(false);
+            response.mutable_header()->set_error_message("Failed to register camera media source: " + dev_id);
+            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+            stream->Write(response);
+            return grpc::Status::OK;
+        }
+        auto subscription = media_hub.subscribe(
+            track_id,
+            cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED,
+            [context] { return context->IsCancelled(); });
+        if (!subscription) {
+            api::GetRGBImageStreamCommand_Feedback response;
+            response.mutable_header()->set_success(false);
+            response.mutable_header()->set_error_message("Failed to subscribe camera media source: " + dev_id);
+            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+            stream->Write(response);
+            return grpc::Status::OK;
+        }
+
+        std::atomic<bool> client_eof_requested{false};
+        std::atomic<bool> request_stream_closed{false};
+        std::atomic<uint64_t> control_requests_read{1};
+        std::thread request_reader([&] {
+            api::GetRGBImageStreamCommand_Request control_request;
+            while (stream->Read(&control_request)) {
+                ++control_requests_read;
+                if (control_request.eof()) {
+                    client_eof_requested.store(true, std::memory_order_release);
+                    CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] client requested RGB stream EOF"
+                                   << ", id=" << dev_id
+                                   << ", peer=" << context->peer()
+                                   << ", control_requests=" << control_requests_read.load();
+                    break;
+                }
+            }
+            request_stream_closed.store(true, std::memory_order_release);
+        });
+        struct RequestReaderJoiner {
+            std::thread& thread;
+            ~RequestReaderJoiner() {
+                if (thread.joinable()) {
+                    thread.join();
+                }
+            }
+        } request_reader_joiner{request_reader};
+        const auto join_request_reader = [&] {
+            if (request_reader.joinable()) {
+                request_reader.join();
+            }
+        };
+
+        bool waiting_for_key_frame = true;
+        const char* exit_reason = "unknown";
+        auto last_key_frame_request = std::chrono::steady_clock::now();
+        auto last_latency_log = std::chrono::steady_clock::time_point{};
+        uint64_t discarded_since_log = 0;
+        std::chrono::microseconds last_write_duration{0};
+        media_hub.requestKeyFrame(track_id);
+        const auto request_key_frame_if_due = [&] {
+            const auto now = std::chrono::steady_clock::now();
+            if (now - last_key_frame_request >= std::chrono::milliseconds(500)) {
+                media_hub.requestKeyFrame(track_id);
+                last_key_frame_request = now;
+            }
+        };
+        const auto request_key_frame_now = [&] {
+            waiting_for_key_frame = true;
+            media_hub.requestKeyFrame(track_id);
+            last_key_frame_request = std::chrono::steady_clock::now();
+        };
+        const auto log_latency_event = [&](
+            const char* reason,
+            const uint64_t discarded,
+            const uint64_t frame_age_ns,
+            const std::chrono::microseconds write_duration) {
+            discarded_since_log += discarded;
+            const auto now = std::chrono::steady_clock::now();
+            if (last_latency_log != std::chrono::steady_clock::time_point{} &&
+                now - last_latency_log < std::chrono::seconds(1)) {
+                return;
+            }
+            const double frame_age_ms = static_cast<double>(frame_age_ns) / 1'000'000.0;
+            const double write_ms = static_cast<double>(write_duration.count()) / 1'000.0;
+            CMVR_LOG(WARNING) << "[gRPCCameraServiceImpl] low-latency camera stream event"
+                              << ", id=" << dev_id
+                              << ", reason=" << reason
+                              << ", discarded=" << discarded_since_log
+                              << ", age_ms=" << frame_age_ms
+                              << ", write_ms=" << write_ms
+                              << ", max_pending_frames="
+                              << stream_config_.max_pending_frames
+                              << ", max_frame_age_ms="
+                              << stream_config_.max_frame_age.count();
+            discarded_since_log = 0;
+            last_latency_log = now;
+        };
+        while (true)
+        {
+            if (client_eof_requested.load(std::memory_order_acquire)) {
+                exit_reason = "client_eof";
+                break;
+            }
+            if (context->IsCancelled())
+            {
+                exit_reason = "context_cancelled";
+                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled"
+                               << ", id=" << dev_id
+                               << ", peer=" << context->peer()
+                               << ", client_eof=" << client_eof_requested.load()
+                               << ", request_stream_closed=" << request_stream_closed.load();
+                break;
+            }
+
+            const auto read = subscription.waitRead(std::chrono::milliseconds(100));
+            if (!read || !read->value || read->value->empty()) {
+                if (!subscription.valid()) {
+                    exit_reason = "subscription_invalid";
+                    break;
+                }
+                if (waiting_for_key_frame) {
+                    request_key_frame_if_due();
+                }
+                continue;
+            }
+            const auto& frame = *read->value;
+            const auto descriptor = frame.descriptor;
+            if (!descriptor) {
+                continue;
+            }
+            const uint64_t now_ns = static_cast<uint64_t>(
+                std::chrono::duration_cast<std::chrono::nanoseconds>(
+                    std::chrono::steady_clock::now().time_since_epoch()).count());
+            const auto frame_age = cameraFrameAgeNs(frame.capture_time_ns, now_ns);
+            if (frame_age &&
+                cameraFrameExceedsAgeLimit(
+                    frame.capture_time_ns,
+                    now_ns,
+                    stream_config_.max_frame_age)) {
+                // This frame is already outside the latency budget. Flush all
+                // currently queued frames and wait for a fresh IDR; sending any
+                // P/B frame after an intentional gap would break decoder continuity.
+                const uint64_t discarded =
+                    1 + subscription.discardPendingIfExceeds(0);
+                request_key_frame_now();
+                log_latency_event(
+                    "stale_frame",
+                    discarded,
+                    *frame_age,
+                    last_write_duration);
+                continue;
+            }
+            const bool inter_frame_codec = descriptor->codec == cmvr::media::Codec::H264 ||
+                                           descriptor->codec == cmvr::media::Codec::H265;
+            if (!inter_frame_codec || descriptor->payload_format != cmvr::media::PayloadFormat::ANNEX_B) {
+                api::GetRGBImageStreamCommand_Feedback response;
+                response.mutable_header()->set_success(false);
+                response.mutable_header()->set_error_message(
+                    "Unsupported camera stream codec or payload format: " + dev_id);
+                setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+                stream->Write(response);
+                exit_reason = "unsupported_stream";
+                break;
+            }
+            if (read->dropped_since_last_read > 0 || read->generation_changed || frame.discontinuity) {
+                request_key_frame_now();
+            }
+            if (waiting_for_key_frame && !frame.key_frame) {
+                request_key_frame_if_due();
+                continue;
+            }
+            waiting_for_key_frame = false;
+
+            api::GetRGBImageStreamCommand_Feedback response;
+            response.mutable_header()->set_success(true);
+            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+            response.mutable_color_frame()->set_data(frame.data(), frame.size());
+            response.mutable_color_frame()->set_is_key_frame(frame.key_frame);
+            response.mutable_color_frame()->set_codec(
+                descriptor->codec == cmvr::media::Codec::H264 ? "h264" :
+                descriptor->codec == cmvr::media::Codec::H265 ? "h265" : "unknown");
+            response.mutable_color_frame()->set_width(static_cast<int32_t>(descriptor->width));
+            response.mutable_color_frame()->set_height(static_cast<int32_t>(descriptor->height));
+            response.mutable_color_frame()->set_capture_utc_ns(frame.capture_utc_ns);
+            response.mutable_color_frame()->set_source_sequence(frame.sequence);
+            response.mutable_color_frame()->set_pts(frame.pts);
+            response.mutable_color_frame()->set_dts(frame.dts);
+            response.mutable_color_frame()->set_source_fps(descriptor->nominal_rate);
+            response.mutable_color_frame()->set_source_timestamp(frame.source_timestamp);
+            response.mutable_color_frame()->set_source_frame_number(frame.source_frame_number);
+
+            response.mutable_intrinsics()->set_fx(descriptor->fx);
+            response.mutable_intrinsics()->set_fy(descriptor->fy);
+            response.mutable_intrinsics()->set_cx(descriptor->cx);
+            response.mutable_intrinsics()->set_cy(descriptor->cy);
+            for (const float coefficient : descriptor->distortion) {
+                response.mutable_intrinsics()->add_coeffs(coefficient);
+            }
+
+            response.set_seq_no(static_cast<int32_t>(std::min<uint64_t>(
+                frame.sequence,
+                static_cast<uint64_t>(std::numeric_limits<int32_t>::max()))));
+
+            const auto write_started = std::chrono::steady_clock::now();
+            if (!stream->Write(response)) {
+                exit_reason = "write_failed";
+                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed"
+                               << ", id=" << dev_id
+                               << ", peer=" << context->peer()
+                               << ", context_cancelled=" << context->IsCancelled()
+                               << ", client_eof=" << client_eof_requested.load()
+                               << ", request_stream_closed=" << request_stream_closed.load()
+                               << ", control_requests=" << control_requests_read.load()
+                               << ", write_ms="
+                               << std::chrono::duration_cast<std::chrono::microseconds>(
+                                      std::chrono::steady_clock::now() - write_started).count() / 1000.0;
+                break;
+            }
+            last_write_duration = std::chrono::duration_cast<std::chrono::microseconds>(
+                std::chrono::steady_clock::now() - write_started);
+
+            // A successful synchronous Write may have been flow-controlled long
+            // enough for the source to outpace this consumer. Once the pending
+            // count crosses the configured trigger, discard the whole pending
+            // batch and require a fresh key frame before resuming.
+            const uint64_t discarded = subscription.discardPendingIfExceeds(
+                stream_config_.max_pending_frames);
+            if (discarded > 0) {
+                request_key_frame_now();
+                log_latency_event(
+                    "write_backpressure",
+                    discarded,
+                    frame_age.value_or(0),
+                    last_write_duration);
+            }
+        }
+        join_request_reader();
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end"
+                       << ", id=" << dev_id
+                       << ", reason=" << exit_reason
+                       << ", peer=" << context->peer()
+                       << ", context_cancelled=" << context->IsCancelled()
+                       << ", client_eof=" << client_eof_requested.load()
+                       << ", request_stream_closed=" << request_stream_closed.load()
+                       << ", control_requests=" << control_requests_read.load();
+        return grpc::Status::OK;
+    }
+    catch (const exception &e) {
+        api::GetRGBImageStreamCommand_Feedback response;
+        response.mutable_header()->set_success(false);
         response.mutable_header()->set_error_message(e.what());
         setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
         stream->Write(response);

protos/cmvr/api/camera_command.proto

@@ -19,6 +19,15 @@ message FrameData {
     FrameType type = 4;
     string codec = 5;
     bool is_key_frame = 6;
+    // Optional capture and source metadata. Fields 1-6 remain wire-compatible
+    // with existing clients; older clients safely ignore these additions.
+    int64 capture_utc_ns = 7;
+    uint64 source_sequence = 8;
+    int64 pts = 9;
+    int64 dts = 10;
+    uint32 source_fps = 11;
+    uint64 source_timestamp = 12;
+    uint64 source_frame_number = 13;
 }
 
 message CameraIntrinsics {
@@ -128,6 +137,39 @@ message StopCameraRecordingCommand {
     }
 }
 
+message ControlPtzCommand {
+    enum Command {
+        COMMAND_UNSPECIFIED = 0;
+        TILT_UP = 1;
+        TILT_DOWN = 2;
+        PAN_LEFT = 3;
+        PAN_RIGHT = 4;
+        UP_LEFT = 5;
+        UP_RIGHT = 6;
+        DOWN_LEFT = 7;
+        DOWN_RIGHT = 8;
+        ZOOM_IN = 9;
+        ZOOM_OUT = 10;
+        PAN_AUTO = 11;
+    }
+
+    enum Action {
+        START = 0;
+        STOP = 1;
+    }
+
+    message Request {
+        CommandHeader.Request header = 1;
+        Command command = 2;
+        Action action = 3;
+        uint32 speed = 4;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
 message GetRGBImageStreamCommand {
     message Request {
         CommandHeader.Request header = 1;
@@ -172,4 +214,3 @@ message GetRGBDImagesStreamCommand {
 }
 
 
-

protos/cmvr/api/camera_service.proto

@@ -14,6 +14,7 @@ service CameraService {
     rpc GetRGBDImages(GetRGBDImagesCommand.Request) returns (GetRGBDImagesCommand.Feedback) {}
     rpc StartRecording(StartCameraRecordingCommand.Request) returns (StartCameraRecordingCommand.Feedback) {}
     rpc StopRecording(StopCameraRecordingCommand.Request) returns (StopCameraRecordingCommand.Feedback) {}
+    rpc ControlPtz(ControlPtzCommand.Request) returns (ControlPtzCommand.Feedback) {}
 
     rpc GetRGBImageStream(stream GetRGBImageStreamCommand.Request) returns (stream GetRGBImageStreamCommand.Feedback) {}
     rpc GetDepthImageStream(stream GetDepthImageStreamCommand.Request) returns (stream GetDepthImageStreamCommand.Feedback) {}

protos/cmvr/config/camera_config/camera_config.proto

@@ -90,6 +90,32 @@ message MujocoCameraConfig {
     MujocoViewerPipConfig viewer_pip = 7;
 }
 
+message HikvisionCameraConfig {
+    string id = 1;
+    string ip = 2;
+    int32 port = 3;
+    string username = 4;
+    string password = 5;
+    int32 channel = 6;
+    int32 stream_type = 7;
+    int32 link_mode = 8;
+    int32 width = 9;
+    int32 height = 10;
+    int32 fps = 11;
+    string codec = 12;
+    CameraMode camera_mode = 13;
+    StreamMode stream_mode = 14;
+    int32 buffer_size = 15;
+    int32 encode_width = 16;
+    int32 encode_height = 17;
+    string sdk_path = 18;
+    float fx = 19;
+    float fy = 20;
+    float cx = 21;
+    float cy = 22;
+    repeated float coeffs = 23;
+}
+
 message CameraDeviceConfig {
     string id = 1;
     reserved 2;
@@ -99,6 +125,7 @@ message CameraDeviceConfig {
         RealSenseCameraConfig realsense = 11;
         MechMindCameraConfig mechmind = 12;
         MujocoCameraConfig mujoco = 13;
+        HikvisionCameraConfig hikvision = 14;
     }
 }
 

protos/cmvr/config/device_manager_config/device_manager_config.proto

@@ -31,12 +31,11 @@ message DeviceConfigEntry {
 }
 
 message DeviceManagerConfig {
-  reserved 20;
-
   string name = 1;
   string version = 2;
   string description = 3;
   repeated DeviceConfigEntry devices = 4;
+  bool init_all_motors_when_no_active_joints = 20;
 }
 message DeviceManagerRootConfig {
   DeviceManagerConfig device_manager = 1;

protos/cmvr/config/joint_limits_config.proto

@@ -33,6 +33,7 @@ message JointLimitAvoidanceConfig {
   double gain = 2;
   double margin_ratio = 3;
   double max_push = 4;
+  double weight = 5;
 }
 
 message JointLimitPolicyConfig {