From 0f6417c940d9fa91e198cc7d05a2e5f0161f52bf Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Wed, 16 Sep 2026 16:38:34 +0800 Subject: [PATCH] merge lgv device control with linbo camera updates --- cmvr-es/algorithms/CMakeLists.txt | 1 + .../collision_detection/CMakeLists.txt | 48 + .../benchmark/self_collision_benchmark.cpp | 53 + .../include/distance_sampling_policy.h | 46 + .../include/self_collision_checker.h | 82 ++ .../src/distance_sampling_policy.cpp | 100 ++ .../src/self_collision_checker.cpp | 403 +++++++ .../test/self_collision_checker_test.cpp | 236 +++++ cmvr-es/algorithms/controllers/CMakeLists.txt | 12 +- .../include/cartesian_velocity_controller.h | 4 + .../src/cartesian_velocity_controller.cpp | 91 +- .../pbvs/include/pbvs_controller.h | 453 ++++++++ .../controllers/pbvs/src/pbvs_controller.cpp | 774 ++++++++++++++ .../pbvs/src/pbvs_controller_test.cpp | 61 ++ .../kinematics/ik_solver/CMakeLists.txt | 13 +- .../include/pinocchio_qp_ik_solver.h | 5 + .../pinocchio/src/pinocchio_ik_base.cpp | 20 +- .../pinocchio/src/pinocchio_qp_ik_solver.cpp | 135 ++- .../src/pinocchio_qp_ik_solver_test.cpp | 104 ++ .../motion_planner/arm_motion/CMakeLists.txt | 11 + .../pinocchio_cartesian_motion_planner.h | 5 + .../pinocchio_cartesian_motion_planner.cpp | 49 +- .../joint_motion/joint_motion_planner.h | 139 ++- .../include/toppra_joint_motion_planner.h | 12 +- .../src/toppra_joint_motion_planner.cpp | 206 +++- .../test/toppra_joint_motion_planner_test.cpp | 139 +++ .../motion_planner/base_motion/CMakeLists.txt | 24 +- .../src/cartesian_twist_limiter.cpp | 5 + .../include/toppra_joint_trajectory_planner.h | 37 +- .../src/toppra_joint_trajectory_planner.cpp | 206 +++- .../test/toppra_multi_waypoint_test.cpp | 228 ++++ .../s_curve/src/s_curve_velocity_planner.cpp | 26 +- .../s_curve_velocity_planner_stop_test.cpp | 77 ++ cmvr-es/algorithms/perception/CMakeLists.txt | 1 + .../apriltag/include/apriltag_perception.h | 4 + .../apriltag/include/tag_relative_tcp_pose.h | 289 +++++ .../apriltag/src/tag_relative_tcp_pose.cpp | 315 ++++++ .../common/media/ffmpeg/camera_capture.cpp | 6 +- cmvr-es/common/media/media_frame.h | 244 +++++ cmvr-es/config/devices/camera/camera.pb.txt | 141 ++- cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp | 2 +- .../motor_robot_arm/include/motor_robot_arm.h | 1 + .../motor_robot_arm/src/motor_robot_arm.cpp | 10 +- cmvr-es/devices/camera/CMakeLists.txt | 3 +- cmvr-es/devices/camera/abstract_camera.h | 70 +- cmvr-es/devices/camera/camera_factory.h | 5 + .../common/include/camera_stream_encoder.h | 23 +- .../common/include/camera_stream_overlay.h | 2 - .../common/src/camera_stream_encoder.cpp | 413 ++++---- .../camera/hikvision_camera/CMakeLists.txt | 122 +++ .../include/hikvision_camera.h | 117 +++ .../hikvision_camera/src/hikvision_camera.cpp | 961 +++++++++++++++++ .../tests/hikvision_camera_callback_test.cpp | 425 ++++++++ .../camera/mechmind/include/mechmind_camera.h | 1 + .../camera/mechmind/src/mechmind_camera.cpp | 12 + .../mujoco_camera/include/mujoco_camera.h | 25 +- .../mujoco_camera/src/mujoco_camera.cpp | 364 +------ .../include/realsense_camera.h | 1 + .../realsense_camera/src/realsense_camera.cpp | 75 +- .../devices/camera/uvc_camera/CMakeLists.txt | 13 +- .../camera/uvc_camera/include/uvc_camera.h | 34 +- .../camera/uvc_camera/src/uvc_camera.cpp | 992 ++++++++++++++---- .../grpc/include/grpc_camera_service.h | 6 +- .../service/grpc/src/grpc_camera_service.cpp | 524 +++++++-- protos/cmvr/api/camera_command.proto | 43 +- protos/cmvr/api/camera_service.proto | 3 +- .../config/camera_config/camera_config.proto | 27 + .../device_manager_config.proto | 3 +- protos/cmvr/config/joint_limits_config.proto | 1 + 69 files changed, 7987 insertions(+), 1096 deletions(-) create mode 100644 cmvr-es/algorithms/collision_detection/CMakeLists.txt create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp create mode 100644 cmvr-es/algorithms/controllers/pbvs/include/pbvs_controller.h create mode 100644 cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp create mode 100644 cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp create mode 100644 cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp create mode 100644 cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp create mode 100644 cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp create mode 100644 cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp create mode 100644 cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h create mode 100644 cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp create mode 100644 cmvr-es/common/media/media_frame.h create mode 100644 cmvr-es/devices/camera/hikvision_camera/CMakeLists.txt create mode 100644 cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h create mode 100644 cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp create mode 100644 cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp diff --git a/cmvr-es/algorithms/CMakeLists.txt b/cmvr-es/algorithms/CMakeLists.txt index 53f2415c..471f0dfe 100644 --- a/cmvr-es/algorithms/CMakeLists.txt +++ b/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) diff --git a/cmvr-es/algorithms/collision_detection/CMakeLists.txt b/cmvr-es/algorithms/collision_detection/CMakeLists.txt new file mode 100644 index 00000000..f6401175 --- /dev/null +++ b/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) diff --git a/cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp b/cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp new file mode 100644 index 00000000..dae09d71 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp @@ -0,0 +1,53 @@ +#include +#include +#include +#include +#include + +#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 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 samples_us; + samples_us.reserve(kIterations); + std::vector q(joint_names.size(), 0.0); + for (std::size_t iteration = 0; iteration < kIterations; ++iteration) { + q[0] = 0.2 * static_cast(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(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(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; +} diff --git a/cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h b/cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h new file mode 100644 index 00000000..f2b022af --- /dev/null +++ b/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 +#include + +#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 diff --git a/cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h b/cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h new file mode 100644 index 00000000..60fae78b --- /dev/null +++ b/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 +#include +#include +#include + +#include + +namespace cmvr { + +struct CollisionPair { + std::string first; + std::string second; +}; + +struct SelfCollisionOptions { + std::vector 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> 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& active_joint_names, + const SelfCollisionOptions& options, + std::string* error = nullptr); + + bool makeSnapshot(const std::vector& joint_positions, + CollisionGeometrySnapshot* snapshot, + std::string* error = nullptr); + + SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot); + SelfCollisionResult check(const std::vector& joint_positions); + + bool initialized() const; + std::size_t dof() const; + std::size_t activePairCount() const; + const std::vector& jointNames() const; + +private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace cmvr + +#endif // CMVR_ES_SELF_COLLISION_CHECKER_H diff --git a/cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp b/cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp new file mode 100644 index 00000000..e45df763 --- /dev/null +++ b/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 +#include +#include + +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(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::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::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 diff --git a/cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp b/cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp new file mode 100644 index 00000000..8606bd22 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr { +namespace { + +using LinkPairKey = std::pair; + +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& 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 active_joint_ids; + std::unordered_set 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 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 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(model_); + geometry_data_ = std::make_unique(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& 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::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& 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 joint_names_; + std::vector joint_q_indices_; + std::vector selected_geometry_indices_; + std::vector geometry_link_names_; + pinocchio::Model model_; + pinocchio::GeometryModel geometry_model_; + std::unique_ptr data_; + std::unique_ptr geometry_data_; + Eigen::VectorXd neutral_q_; +}; + +SelfCollisionChecker::SelfCollisionChecker() + : impl_(std::make_unique()) +{ +} + +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& 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& 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& 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& SelfCollisionChecker::jointNames() const +{ + return impl_->joint_names_; +} + +} // namespace cmvr diff --git a/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp b/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp new file mode 100644 index 00000000..5c5d75b4 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp @@ -0,0 +1,236 @@ +#include +#include +#include + +#include + +#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 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 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 kGen2SetupPose{ + 0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0, +}; + +const std::vector kGen2WarningPose{ + 2.45028525340088, + 0.413065394330014, + -1.78610031118294, + 2.3232081721811, + -2.96828882895788, + -1.59350098130002, + 0.582912411114367, +}; + +const std::vector kGen2StopPose{ + 2.13758633436379, + 1.61835160165575, + -2.3836142221041, + 0.964538527544213, + -0.00382525077004825, + 1.74586899135531, + -0.336868659266887, +}; + +const std::vector kGen2CollisionPose{ + -0.42656969579233, + 1.41426471041774, + -2.67949400419915, + 2.45814854129954, + -2.35907388079205, + 1.14125209449898, + 1.53232912981414, +}; + +const std::vector 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(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(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 diff --git a/cmvr-es/algorithms/controllers/CMakeLists.txt b/cmvr-es/algorithms/controllers/CMakeLists.txt index 1921842a..07fe6948 100644 --- a/cmvr-es/algorithms/controllers/CMakeLists.txt +++ b/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 +) diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h index 9dd0fc29..d8a728ed 100644 --- a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h +++ b/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& q, std::vector& 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 acceleration = std::nullopt); + void abortCommand_(); void sendZero_(); static double velocityNorm_(const std::vector& velocity); diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp index efad63b6..91ed55b3 100644 --- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp +++ b/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,8 +64,13 @@ 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)) { - return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); + 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 acceleratio if (!worker_ || !worker_->joinable()) { return Result::success(); } - { - std::lock_guard lock(mutex_); - target_twist_ = {}; - target_frame_ = FrameType::Base; - target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration; - command_active_ = true; - ++command_version_; + // A completed speedL command leaves the worker thread joinable but idle. + // Do not turn that idle worker into a new command just because a caller + // requests a stop during a task transition. + if (!busy_.load()) { + return Result::success(); } - 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) { - std::lock_guard lock(mutex_); - command_active_ = false; - sendZero_(); - busy_.store(false); + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); break; } CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration=" << acceleration; - sendZero_(); - busy_.store(false); - return; + requestStop_(); + continue; } std::vector q_now; std::vector qd_now; if (!read_state_(q_now, qd_now)) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed"; - sendZero_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + break; + } + requestStop_(); + continue; } std::vector 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_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + 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_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + 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 acceleration) +{ + { + std::lock_guard 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 lock(mutex_); + command_active_ = false; + target_twist_ = {}; + target_frame_ = FrameType::Base; + } + sendZero_(); + busy_.store(false); +} + void CartesianVelocityController::sendZero_() { if (!send_velocity_) { diff --git a/cmvr-es/algorithms/controllers/pbvs/include/pbvs_controller.h b/cmvr-es/algorithms/controllers/pbvs/include/pbvs_controller.h new file mode 100644 index 00000000..645ddd83 --- /dev/null +++ b/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 + +#include + +#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& vmax6); + + /** + * @brief 设置六维加速度限制。 + * + * 前三维 m/s^2； + * 后三维 rad/s^2。 + * + * <= 0 表示对应维度不限制。 + */ + void setAccelerationLimit6( + const std::array& amax6); + + /** + * @brief 设置六维误差阈值。 + * + * [x y z rx ry rz] + * + * 前三维 m； + * 后三维 rad。 + */ + void setTolerance6( + const std::array& 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& 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& velocityLimit6() const { + return vmax6_; + } + + const std::array& accelerationLimit6() const { + return amax6_; + } + + const std::array& tolerance6() const { + return tolerance6_; + } + + double twistFilterAlpha() const { + return twist_lpf_alpha_; + } + + const Eigen::Matrix& + 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& 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 vmax6_{{ + 0.10, + 0.10, + 0.05, + 0.50, + 0.50, + 0.50 + }}; + + // ---------------- acceleration limits ---------------- + + std::array amax6_{{ + 0.50, + 0.50, + 0.30, + 2.0, + 2.0, + 2.0 + }}; + + // ---------------- tolerance ---------------- + + std::array 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 axis_enabled_{{ + true, + true, + true, + true, + true, + true + }}; + + // ---------------- LPF ---------------- + + double twist_lpf_alpha_{1.0}; + + // ---------------- command history ---------------- + + Eigen::Matrix + previous_twist_cmd_G_{ + Eigen::Matrix::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 + last_twist_cmd_G_{ + Eigen::Matrix::Zero() + }; + + bool last_reached_{false}; + + ComputeStatus last_compute_status_{ + ComputeStatus::TARGET_NOT_SET + }; +}; + +} // namespace cmvr + +#endif // CMVR_PBVS_CONTROLLER_H diff --git a/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp b/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp new file mode 100644 index 00000000..fedd1f87 --- /dev/null +++ b/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp @@ -0,0 +1,774 @@ +#include "algorithms/controllers/pbvs/include/pbvs_controller.h" + +#include +#include + +#include +#include + +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 + twist_raw_G = + Eigen::Matrix::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 + 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 + 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 + 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& 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& amax6) +{ + for (int i = 0; i < 6; ++i) { + + if (std::isfinite(amax6[i])) { + amax6_[i] = + amax6[i]; + } + } +} + +void PbvsController::setTolerance6( + const std::array& 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& 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 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& 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 \ No newline at end of file diff --git a/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp b/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp new file mode 100644 index 00000000..cde596f8 --- /dev/null +++ b/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp @@ -0,0 +1,61 @@ +#include "gtest/gtest.h" + +#include + +#include + +#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 vmax{{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}}; + const std::array amax{{1.0, 2.0, 3.0, 4.0, 5.0, 6.0}}; + const std::array 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 diff --git a/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt index 310be1b7..1ade9e29 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt +++ b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt @@ -25,4 +25,15 @@ target_link_libraries(ik_solver PUBLIC add_library(cmvr_es::ik_solver ALIAS ik_solver) -install(TARGETS ik_solver LIBRARY DESTINATION lib) \ No newline at end of file +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 +) diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h index 1fa37f8a..ffce089f 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h @@ -45,6 +45,11 @@ public: std::vector& qdot_out, double qdot_abs_max = std::numeric_limits::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); diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp index 823073a0..3ceba79e 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp +++ b/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; - if (q_chain[i] <= lower) { - limited[i] = std::max(0.0, limited[i]); - } - } else if (limited[i] > 0.0 && q_chain[i] > upper - margin) { + scale = std::min(scale, ratio); + } else if (qdot[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; - if (q_chain[i] >= upper) { - limited[i] = std::min(0.0, limited[i]); - } + scale = std::min(scale, ratio); } } - return limited; + return scale * qdot; } void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) { diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp index ce7af494..4825d0a5 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp @@ -13,11 +13,66 @@ #include #include // std::clamp, std::max, std::min +#include #include // std::sqrt +#include #include +#include #include +#include 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 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::epsilon() * + static_cast(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 &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 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); - target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid; + nullspace; + target.segment(6 + dof, dof) = qdot_avoid_null; + + static std::atomic 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::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; } diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp new file mode 100644 index 00000000..9edbb089 --- /dev/null +++ b/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 +#include + +#include + +#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 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 q_chain = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.56}; + std::vector qdot_disabled; + std::vector 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 qdot_disabled_eigen( + qdot_disabled.data(), static_cast(qdot_disabled.size())); + const Eigen::Map qdot_enabled_eigen( + qdot_enabled.data(), static_cast(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 diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt index eb26409f..33026787 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt +++ b/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 +) diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h index 9d6f070e..d0eddc48 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h +++ b/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}; }; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp index 329c7b96..08de05d0 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp +++ b/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 target_twist = common::math::velocityToVector(target_velocity); - const bool is_stop_command = target_twist.squaredNorm() <= 1e-12; + const Eigen::Matrix target_twist = + 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); - } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { - twist_limiter_.initialize(Eigen::Matrix::Zero()); + if (!speedl_stop_active_) { + // 只在 stop 边沿执行一次。 + // + // 非常重要： + // 不再调用 + // twist_limiter_.synchronize(measured_twist_base, dt, true); + // + // 停止应当从“上一拍已经发送出去的 command twist” + // 连续规划到 0，而不是每 1 ms 被 measured twist 重新点燃。 + speedl_stop_active_ = true; + + twist_limiter_.stop(); + } + } else { + // 收到新的非零 speedL，退出停止状态。 + speedl_stop_active_ = false; + + // 从静止开始一个新的 speedL command。 + if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { + twist_limiter_.initialize( + Eigen::Matrix::Zero()); + } + + twist_limiter_.setTargetTwist( + target_twist, + common::math::toPlannerFrame(frame)); } - twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame)); - speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool); + + 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 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)) { diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h index ea500ea4..a2f77c51 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h +++ b/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 +#include #include +#include "common/base/logging/logger.h" #include "common/types/arm/arm_types.h" namespace cmvr::device { -struct JointTrajectorySample { - double t{0.0}; - std::vector position; - std::vector velocity; -}; - class JointMotionPlanner { public: virtual ~JointMotionPlanner() = default; @@ -23,9 +20,137 @@ public: const JointPositionCommand& target, const MotionOptions& options, double speed_scaling, - std::vector& samples) = 0; + JointTrajectory& trajectory) = 0; + + virtual bool planReplay(const std::vector& 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 previous_position_velocity(expected_dof, 0.0); + std::vector 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 diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h index 8c6db577..7aadd085 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h +++ b/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& samples) override; + JointTrajectory& trajectory) override; + + bool planReplay(const std::vector& current_position, + const JointTrajectory& recorded_trajectory, + const MotionOptions& options, + JointTrajectory& replay_trajectory) override; private: + bool sampleTrajectory_( + const std::shared_ptr& planner, + const cmvr::TrajPtr& raw_trajectory, + JointTrajectory& trajectory) const; + std::shared_ptr planner_; cmvr::PathType path_type_{cmvr::PathType::Quintic}; double sample_period_s_{0.001}; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp index 8ee1e8e2..1fe882fa 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp +++ b/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 + #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& 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& start, const JointPositionCommand& target, const MotionOptions& options, const double speed_scaling, - std::vector& 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(start.size(), options.velocity * speed_scaling), std::vector(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_); - samples.reserve(raw_samples.size()); - for (const auto& sample : raw_samples) { - JointTrajectorySample dst; - dst.t = sample.t; - dst.position = toStdVector(sample.q); - dst.velocity = toStdVector(sample.qd); - samples.push_back(std::move(dst)); + return sampleTrajectory_(planner_, raw_trajectory, trajectory); +} + +bool ToppraJointMotionPlanner::planReplay( + const std::vector& current_position, + const JointTrajectory& recorded_trajectory, + const MotionOptions& options, + 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 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(dof, 0.0)}); + + double replay_time_s = ramp_duration_s; + replay_trajectory.push_back(JointTrajectoryPoint{ + replay_time_s, + recorded_trajectory.back().position, + std::vector(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(dof, 0.0)}); + } + replay_time_s += ramp_duration_s; + replay_trajectory.push_back(JointTrajectoryPoint{ + replay_time_s, + recorded_trajectory.front().position, + std::vector(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 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; } diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp new file mode 100644 index 00000000..40ad8414 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp @@ -0,0 +1,139 @@ +#include +#include +#include +#include +#include + +#include + +#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(i) / + static_cast(point_count - 1); + JointTrajectoryPoint point; + point.time_s = static_cast(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& lhs, + const std::vector& rhs) +{ + if (lhs.size() != rhs.size()) { + return std::numeric_limits::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 diff --git a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt index bd84d4b9..36881029 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt @@ -20,4 +20,26 @@ target_link_libraries(base_motion PUBLIC ) add_library(cmvr_es::base_motion ALIAS base_motion) -install(TARGETS base_motion LIBRARY DESTINATION lib) \ No newline at end of file +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 +) diff --git a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp index a4ecf10b..bcf884b0 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp +++ b/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, diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h index 39865020..f344fc1e 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h +++ b/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 - makeS_centripetal(const std::vector &q) { + makeSChordLength(const std::vector &q) { const size_t M = q.size(); std::vector 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]); - if (S[i] <= S[i - 1]) S[i] = S[i - 1] + 1e-12; + S[i] = S[i - 1] + (q[i] - q[i - 1]).norm(); } return S; } - // 等距参数（简单稳妥） - static inline std::vector makeS_equal(size_t M) { - std::vector S(M); - for (size_t i = 0; i < M; ++i) S[i] = static_cast(i); - return S; - } - - // 或：先用centripetal，再整体归一化到跨度≈(M-1)，并设置每段最小ds - static inline void normalize_and_floor_S(std::vector &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 estimateVelsCatmull(const std::vector &q, @@ -159,14 +140,16 @@ namespace cmvr { // 对内点几何速度限幅，抑制过冲（k∈[0.5,1.0]） static void clampNodeVels(std::vector &v, const std::vector &q, + const std::vector &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(); - double d1 = (q[i + 1] - q[i]).norm(); - double d = std::max(std::min(d0, d1), 1e-12); - double vmax = k * d; + const double ds0 = std::max(S[i] - S[i - 1], 1e-12); + const double ds1 = std::max(S[i + 1] - S[i], 1e-12); + const double slope0 = (q[i] - q[i - 1]).norm() / ds0; + 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); } diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp index e3637b8b..040b59e1 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp @@ -5,10 +5,117 @@ #include #include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" +#include +#include #include #include 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& velocity_limits, + const std::vector& 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::ceil(duration / 0.001)) + 1; + const std::size_t path_samples = waypoint_count * 20; + const std::size_t sample_count = std::clamp( + 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(sample) / + static_cast(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(velocity_limits.size()) || + acceleration.size() != + static_cast(acceleration_limits.size())) { + return false; + } + for (Eigen::Index joint = 0; joint < velocity.size(); ++joint) { + const std::size_t index = static_cast(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( + source, required_scale * kNumericalMargin); + return true; + } + + } // namespace + // ===== ConstAccelTraj ===== ConstAccelTraj::ConstAccelTraj(std::shared_ptr p) : impl_(std::move(p)) { @@ -54,22 +161,39 @@ namespace cmvr { bool ToppraJointTrajectoryPlanner::plan(const std::vector>& waypoints, TrajPtr& traj_out) { traj_out.reset(); - const size_t M = waypoints.size(); - if (M < 2) return false; + if (waypoints.size() < 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 q; q.reserve(M); - for (const auto& w : waypoints) - q.emplace_back(Eigen::Map(w.data(), DoF)); + std::vector q; + q.reserve(waypoints.size()); + constexpr double kDuplicateDistance = 1e-10; + for (const auto& waypoint : waypoints) { + Eigen::VectorXd value = Eigen::Map( + waypoint.data(), static_cast(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 -// std::vector S = (M==2) ? std::vector{0.0,1.0} -// : makeS_centripetal(q); - std::vector S = (M==2) ? std::vector{0.0,1.0} - : makeS_equal(M); + const std::vector S = M == 2 + ? std::vector{0.0, 1.0} + : makeSChordLength(q); // 几何路径 auto path = buildPathUnified(q, S); @@ -86,8 +210,23 @@ namespace cmvr { // TOPPRA toppra::algorithm::TOPPRA algo{constraints, path}; - auto solve_once = [&](int N)->bool{ - algo.setN(N); + auto solve_once = [&](const int requested_intervals)->bool{ + const int segment_count = static_cast(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(segment)]; + const double length = S[static_cast(segment + 1)] - start; + for (int subdivision = 0; subdivision < subdivisions; ++subdivision) { + grid[index++] = start + length * + static_cast(subdivision) / + static_cast(subdivisions); + } + } + grid[index] = S.back(); + algo.setGridpoints(grid); algo.solver(std::make_shared()); return algo.computePathParametrization(0.0, 0.0) == toppra::ReturnCode::OK; }; @@ -99,19 +238,20 @@ namespace cmvr { toppra::Vector grid = data.gridpoints; toppra::Vector vsq = data.parametrization; + TrajPtr candidate; auto ca = std::make_shared(path, grid, vsq); if (ca->validate()) { - traj_out = std::make_shared(std::move(ca)); - return true; - } - sanitizeVsq(vsq); - try { - traj_out = std::make_shared(path, grid, vsq); - (void) traj_out->timeInterval(); - return true; - } catch (...) { - return false; + candidate = std::make_shared(std::move(ca)); + } else { + sanitizeVsq(vsq); + try { + candidate = std::make_shared(path, grid, vsq); + (void) candidate->timeInterval(); + } catch (...) { + return false; + } } + return enforceSampledLimits(candidate, v_max_, a_max_, M, traj_out); } @@ -225,7 +365,7 @@ namespace cmvr { double ds = std::max(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& q, const std::vector& 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& 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(1)=C4.transpose(); - seg.row(2)=C3.transpose(); - seg.row(3)=A2.transpose(); - seg.row(4)=A1.transpose(); + seg.row(0)=(C5 / std::pow(ds, 5)).transpose(); + seg.row(1)=(C4 / std::pow(ds, 4)).transpose(); + seg.row(2)=(C3 / std::pow(ds, 3)).transpose(); + seg.row(3)=(a0 / 2.0).transpose(); + seg.row(4)=v0.transpose(); seg.row(5)=A0.transpose(); segs.emplace_back(std::move(seg)); } @@ -384,4 +524,4 @@ namespace cmvr { } return true; } -} // namespace cmvr \ No newline at end of file +} // namespace cmvr diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp new file mode 100644 index 00000000..87c6f3cb --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp @@ -0,0 +1,228 @@ +#include +#include +#include +#include +#include +#include +#include + +#include + +#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> makeSmoothWaypoints(const std::size_t count) +{ + constexpr double kPi = 3.14159265358979323846; + std::vector> waypoints; + waypoints.reserve(count); + for (std::size_t i = 0; i < count; ++i) { + const double s = static_cast(i) / + static_cast(count - 1); + std::vector 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& expected) +{ + if (actual.size() != static_cast(expected.size())) { + return std::numeric_limits::infinity(); + } + double squared_error = 0.0; + for (Eigen::Index i = 0; i < actual.size(); ++i) { + const double error = actual[i] - expected[static_cast(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>& waypoints, + const PathType path_type = PathType::Linear) +{ + PlanMetrics metrics; + ToppraJointTrajectoryPlanner planner(path_type); + planner.setSymmetricLimits( + std::vector(kDof, kVelocityLimit), + std::vector(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( + 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> 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 diff --git a/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp index b6748195..c058e673 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp @@ -114,13 +114,29 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity, updateIsMovingFlag(); } - void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity, - double acceleration) +void SCurveVelocityPlanner1D::synchronizeAndReplan(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; @@ -128,7 +144,7 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity, state_.jerk = 0.0; updateIsMovingFlag(); return; - } + } // 如果测量值已经基本落在当前采样状态上，就继续沿现有 profile 走。 // 否则每拍都从同一目标重规划，会把已经进入的 jerk phase 反复打断。 @@ -139,7 +155,7 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity, state_.jerk = getJerkAtTime(active_profile_, state_.elapsed_time); updateIsMovingFlag(); return; - } + } state_.velocity = measured_velocity; state_.acceleration = measured_acceleration; diff --git a/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp new file mode 100644 index 00000000..12ba5e38 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp @@ -0,0 +1,77 @@ +#include + +#include + +#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 diff --git a/cmvr-es/algorithms/perception/CMakeLists.txt b/cmvr-es/algorithms/perception/CMakeLists.txt index 98eddf21..d47a2906 100644 --- a/cmvr-es/algorithms/perception/CMakeLists.txt +++ b/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}) diff --git a/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h b/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h index a4486e56..6f4e57cb 100644 --- a/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h +++ b/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 { diff --git a/cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h b/cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h new file mode 100644 index 00000000..7e41574d --- /dev/null +++ b/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 + +#include + +#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& perception = nullptr); + + /** + * @brief 设置 AprilTag 感知前端。 + * + * 本类只读取感知缓存，不主动 update。 + */ + void setPerception( + const std::shared_ptr& perception); + + const std::shared_ptr& 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 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 \ No newline at end of file diff --git a/cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp b/cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp new file mode 100644 index 00000000..25501d8d --- /dev/null +++ b/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 + +namespace cmvr::perception { + +TagRelativeTcpPose::TagRelativeTcpPose( + const std::shared_ptr& perception) + : perception_(perception) +{ +} + +void TagRelativeTcpPose::setPerception( + const std::shared_ptr& 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 \ No newline at end of file diff --git a/cmvr-es/common/media/ffmpeg/camera_capture.cpp b/cmvr-es/common/media/ffmpeg/camera_capture.cpp index d8f2c61a..06ea1fb6 100644 --- a/cmvr-es/common/media/ffmpeg/camera_capture.cpp +++ b/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 diff --git a/cmvr-es/common/media/media_frame.h b/cmvr-es/common/media/media_frame.h new file mode 100644 index 00000000..b81e8345 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +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 distortion; + uint64_t generation{1}; + std::vector 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 distortion; + const uint64_t generation; + const std::vector codec_config; +}; + +using TrackDescriptorPtr = std::shared_ptr; + +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; + using PayloadPtr = std::shared_ptr; + + 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(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; + +inline TrackDescriptorPtr makeTrackDescriptor(TrackDescriptor::Config config) { + return std::make_shared(std::move(config)); +} + +inline MediaFramePtr makeMediaFrame(MediaFrame::Config config) { + return std::make_shared(std::move(config)); +} + +} // namespace cmvr::media + +#endif // CMVR_ES_COMMON_MEDIA_MEDIA_FRAME_H diff --git a/cmvr-es/config/devices/camera/camera.pb.txt b/cmvr-es/config/devices/camera/camera.pb.txt index 038da4b6..1eb69fdd 100644 --- a/cmvr-es/config/devices/camera/camera.pb.txt +++ b/cmvr-es/config/devices/camera/camera.pb.txt @@ -83,8 +83,8 @@ camera { stream_mode: STREAM_MODE_RGBD } encoder { - width: 480 - height: 320 + width: 1280 + 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 + } + } + } } diff --git a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp index 11c7312c..1413c8d4 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/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(); diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index d8e37615..6405922d 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -131,6 +131,7 @@ private: std::shared_ptr joint_planner_{nullptr}; std::shared_ptr cartesian_planner_{nullptr}; std::unique_ptr 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. diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 3cebe17f..5ff10796 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/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( - cartesian_velocity_controller_->stopTimeoutS()); + std::chrono::duration(cartesian_stop_timeout_s_); 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; } diff --git a/cmvr-es/devices/camera/CMakeLists.txt b/cmvr-es/devices/camera/CMakeLists.txt index e659f9ab..973287f9 100644 --- a/cmvr-es/devices/camera/CMakeLists.txt +++ b/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 ) diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h index 86ce2879..e44ecce1 100644 --- a/cmvr-es/devices/camera/abstract_camera.h +++ b/cmvr-es/devices/camera/abstract_camera.h @@ -2,8 +2,13 @@ #define CMVR_ES_ABSTRACT_CAMERA_H #pragma once -#include +#include +#include #include +#include +#include + +#include #include "../abstract_device.h" #include #include "cmvr/config/camera_config/camera_config.pb.h" @@ -14,11 +19,11 @@ namespace cmvr::device { struct Rs2Intrinsics { - float cx{0.0F}; - float cy{0.0F}; - float fx{0.0F}; - float fy{0.0F}; - float coeffs[5]{}; + float cx; + float cy; + float fx; + float fy; + float coeffs[5]; }; struct StreamFrameData @@ -62,6 +67,21 @@ namespace cmvr::device { uint32_t codec_config_generation = 0; std::vector 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 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_{}; diff --git a/cmvr-es/devices/camera/camera_factory.h b/cmvr-es/devices/camera/camera_factory.h index 60a8f549..09c478b0 100644 --- a/cmvr-es/devices/camera/camera_factory.h +++ b/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( + backendWithId_(cfg.id(), cfg.hikvision())); + case config::CameraDeviceConfig::kMujoco: return std::make_shared( backendWithId_(cfg.id(), cfg.mujoco())); diff --git a/cmvr-es/devices/camera/common/include/camera_stream_encoder.h b/cmvr-es/devices/camera/common/include/camera_stream_encoder.h index 53222545..32390ddb 100644 --- a/cmvr-es/devices/camera/common/include/camera_stream_encoder.h +++ b/cmvr-es/devices/camera/common/include/camera_stream_encoder.h @@ -7,12 +7,9 @@ #include #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& encoder, @@ -59,12 +42,10 @@ public: const cv::Mat& frame, std::vector& 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& encoder, - const cv::Mat& frame, + const AVFrame* frame, std::vector& encoded_frame, bool& is_key, const CameraStreamEncodeOptions& options = {}); diff --git a/cmvr-es/devices/camera/common/include/camera_stream_overlay.h b/cmvr-es/devices/camera/common/include/camera_stream_overlay.h index 7ecd6cfe..850b50a0 100644 --- a/cmvr-es/devices/camera/common/include/camera_stream_overlay.h +++ b/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; diff --git a/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp b/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp index 8efd1fb6..f9ef2f76 100644 --- a/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp +++ b/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp @@ -1,16 +1,16 @@ #include "devices/camera/common/include/camera_stream_encoder.h" #include -#include #include #include #include #include +#include +#include #include #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(intrinsics.fx) * point.x() / point.z() + - static_cast(intrinsics.cx); - const double v = static_cast(intrinsics.fy) * point.y() / point.z() + - static_cast(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& 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(target_width) / static_cast(source_width); - const float sy = static_cast(target_height) / static_cast(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& 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,11 +216,13 @@ bool CameraStreamEncoder::init(std::shared_ptr& encoder, av_opt_set(ctx->priv_data, "no-open-gop", "1", 0); } - const AVPixelFormat* pix_fmts = codec->pix_fmts; - if (!pix_fmts) { - ctx->pix_fmt = AV_PIX_FMT_YUV420P; - } else { - ctx->pix_fmt = pix_fmts[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); @@ -287,9 +253,11 @@ bool CameraStreamEncoder::init(std::shared_ptr& 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& encoder, const cv::Mat& frame, std::vector& encoded_frame, bool& is_key, - const Rs2Intrinsics& intrinsics, const CameraStreamEncodeOptions& options) { encoded_frame.clear(); @@ -314,27 +281,9 @@ bool CameraStreamEncoder::encode(std::shared_ptr& encoder, } cv::Mat frame_to_encode = frame; - if (options.draw_timestamp || options.overlay.draw_coordinate_frames) { + if (options.draw_timestamp) { frame_to_encode = frame.clone(); - if (options.draw_timestamp) { - 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); - } - } + drawTimeStamp(frame_to_encode); } const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode); @@ -343,24 +292,30 @@ bool CameraStreamEncoder::encode(std::shared_ptr& encoder, return false; } - if (encoder->sws_context) { - sws_freeContext(encoder->sws_context); - } - encoder->sws_context = sws_getContext(frame_to_encode.cols, - frame_to_encode.rows, - src_pix_fmt, - encoder->codec_context->width, - encoder->codec_context->height, - encoder->codec_context->pix_fmt, - SWS_BILINEAR, - nullptr, - nullptr, - nullptr); + encoder->sws_context = sws_getCachedContext(encoder->sws_context, + frame_to_encode.cols, + frame_to_encode.rows, + src_pix_fmt, + 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 SwsContext"; 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(frame_to_encode.step)}; @@ -376,59 +331,125 @@ bool CameraStreamEncoder::encode(std::shared_ptr& encoder, return false; } - 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; - 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; + return encodePreparedFrame(*encoder, encoded_frame, is_key); } bool CameraStreamEncoder::encode(std::shared_ptr& encoder, - const cv::Mat& frame, + const AVFrame* frame, std::vector& encoded_frame, bool& is_key, const CameraStreamEncodeOptions& options) { - Rs2Intrinsics intrinsics{}; - return encode(encoder, frame, encoded_frame, is_key, intrinsics, options); + encoded_frame.clear(); + 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(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(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(encoder->frame->linesize[0])); + drawTimeStamp(luma_plane); + } + + return encodePreparedFrame(*encoder, encoded_frame, is_key); } } // namespace cmvr::device diff --git a/cmvr-es/devices/camera/hikvision_camera/CMakeLists.txt b/cmvr-es/devices/camera/hikvision_camera/CMakeLists.txt new file mode 100644 index 00000000..432a1cb8 --- /dev/null +++ b/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() diff --git a/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h b/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h new file mode 100644 index 00000000..0fd6e27f --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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> 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 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 diff --git a/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp b/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp new file mode 100644 index 00000000..e2d92821 --- /dev/null +++ b/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp @@ -0,0 +1,961 @@ +#include "../include/hikvision_camera.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined(__linux__) +#include +#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 g_ignored_data_type_log_count{0}; +std::atomic g_es_video_log_count{0}; +std::atomic 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(speed); +} + +std::string lowerString(std::string value) +{ + std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) { + return static_cast(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 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(user); + if (!camera || !packet_info) { + return; + } + const uint64_t source_timestamp = + (static_cast(packet_info->dwTimeStampHigh) << 32U) | + static_cast(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(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>(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(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(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(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(source_frame_rate) + : fps_; + frame_data.width = packet_width > 0 ? static_cast(packet_width) : width_; + frame_data.height = packet_height > 0 ? static_cast(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( + capture_monotonic.time_since_epoch()).count(); + frame_data.capture_utc_ns = std::chrono::duration_cast( + capture_utc.time_since_epoch()).count(); + frame_data.pts = static_cast(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 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 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(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(stream_type_); + preview_info.dwLinkMode = static_cast(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(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(width_) * 0.5f; + intrinsics.cy = camera_.cy() > 0.0f ? camera_.cy() : static_cast(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 diff --git a/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp b/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp new file mode 100644 index 00000000..1aa72441 --- /dev/null +++ b/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp @@ -0,0 +1,425 @@ +#include "../include/hikvision_camera.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 g_stop_callback_count{0}; +std::atomic g_key_frame_request_count{0}; +std::atomic g_ptz_call_count{0}; +std::atomic g_last_ptz_command{0}; +std::atomic g_last_ptz_stop{0}; +std::atomic 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 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(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(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 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(kConcurrentCallbacks + 1)); + CHECK_TRUE(latest_cursor == static_cast(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(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 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(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; +} diff --git a/cmvr-es/devices/camera/mechmind/include/mechmind_camera.h b/cmvr-es/devices/camera/mechmind/include/mechmind_camera.h index 6a8c225b..7a8628b6 100644 --- a/cmvr-es/devices/camera/mechmind/include/mechmind_camera.h +++ b/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; diff --git a/cmvr-es/devices/camera/mechmind/src/mechmind_camera.cpp b/cmvr-es/devices/camera/mechmind/src/mechmind_camera.cpp index 39e30539..27f53738 100644 --- a/cmvr-es/devices/camera/mechmind/src/mechmind_camera.cpp +++ b/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; diff --git a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h index 11cbaff3..501b0da8 100644 --- a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h +++ b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h @@ -5,13 +5,10 @@ #pragma once #include -#include -#include #include #include #include #include -#include #include #include @@ -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& rgb, @@ -73,14 +66,6 @@ private: int height); static void linearizeDepth_(const mjModel* model, std::vector& 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 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 diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp index abf9f499..416dc7bb 100644 --- a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp +++ b/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 lock(mtx_); + state = state_; +} + bool MujocoCamera::init() { std::lock_guard 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; - { - std::lock_guard lock(mtx_); - initialized = state_.is_initialized; - } - if (!initialized) { + if (!state_.is_initialized) { if (!init()) { return false; } } - + std::lock_guard 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 lock(mtx_); - state_.is_streaming = true; - state_.is_opened = true; - use_external_frames = static_cast(fetch_rgbd_fn_); - } - std::thread stale_thread; - { - std::lock_guard 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 lock(mtx_); - stale_thread = std::move(render_thread_); - } - if (stale_thread.joinable()) { - stale_thread.join(); - } - - try { - std::lock_guard lock(mtx_); - render_thread_ = std::thread(&MujocoCamera::renderLoop_, this); - } catch (const std::exception& e) { - { - std::lock_guard 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 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; - } + state_.is_streaming = true; + state_.is_opened = true; return true; } bool MujocoCamera::stop() { - { - std::lock_guard lock(cache_mtx_); - render_stop_requested_ = true; - } - cache_cv_.notify_all(); - - std::thread thread_to_join; - { - std::lock_guard lock(mtx_); - state_.is_streaming = false; - state_.is_opened = false; - thread_to_join = std::move(render_thread_); - } - if (thread_to_join.joinable()) { - thread_to_join.join(); - } - - { - std::lock_guard lock(cache_mtx_); - render_thread_running_ = false; - latest_frame_ = CachedFrame{}; - } - cache_cv_.notify_all(); + std::lock_guard lock(mtx_); + state_.is_streaming = false; + state_.is_opened = false; + destroyOffscreen_(); return true; } void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn) { - bool render_thread_active = false; - { - std::lock_guard lock(cache_mtx_); - render_thread_active = render_thread_running_; - } - if (render_thread_active) { - stop(); - } - std::lock_guard 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 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; @@ -376,29 +279,14 @@ 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 lock(mtx_); - fetch_rgbd_fn = fetch_rgbd_fn_; - consume_new_frame_only = consume_new_frame_only_; - } - - { - std::lock_guard 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::lock_guard lock(mtx_); + if (fetch_rgbd_fn_) { std::vector rgb_raw; std::vector 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(depth_raw.size()) != width * height) { return false; } - { - std::lock_guard 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; + 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); -} - -bool MujocoCamera::fetchCached_(cv::Mat& color, - cv::Mat& depth, - Rs2Intrinsics& intrinsics, - const bool consume_new_frame_only) -{ - std::lock_guard 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(); + return renderOffscreen_(color, depth, intrinsics); } bool MujocoCamera::initOffscreen_() @@ -464,7 +325,6 @@ bool MujocoCamera::initOffscreen_() return true; } - std::lock_guard glfw_lock(glfwInitMutex()); if (!glfwInit()) { setError_("[MujocoCamera] glfwInit failed"); return false; @@ -491,25 +351,10 @@ bool MujocoCamera::initOffscreen_() return false; } - mjModel* model = nullptr; - { - std::lock_guard world_lock(world->mutex()); - 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"); + std::lock_guard world_lock(world->mutex()); + const mjModel* model = world->model(); + if (model == nullptr) { + setError_("[MujocoCamera] world model is null"); return false; } mjv_makeScene(model, &scene_, kMaxGeom); @@ -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 lock(mtx_); - external_fetch = fetch_rgbd_fn_; - } - - const bool use_external_frames = static_cast(external_fetch); - if (!use_external_frames && !initOffscreen_()) { - destroyOffscreen_(); - { - std::lock_guard 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(1.0 / static_cast(fps)); - const auto period_ticks = std::chrono::duration_cast(period); - auto next_tick = std::chrono::steady_clock::now(); - - while (true) { - { - std::lock_guard 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 rgb_raw; - std::vector 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(rgb_raw.size()) == width * height * 3 && - (depth_raw.empty() || static_cast(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 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 lock(mtx_); - clear_error_(); - } - cache_cv_.notify_all(); - } - - next_tick += period_ticks; - std::unique_lock 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 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_) { - setError_("[MujocoCamera] offscreen renderer is not initialized: " + id_); + if (!initOffscreen_()) { return false; } @@ -675,27 +408,15 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic std::vector rgb(static_cast(width_) * height_ * 3); std::vector depth_raw(static_cast(width_) * height_); - mjModel* model = nullptr; { - std::unique_lock world_lock(world->mutex(), std::try_to_lock); - if (!world_lock.owns_lock()) { - // Never make the simulation wait for a camera frame. The next - // scheduled capture will use a newer state if this one is busy. - return false; - } - model = world->model(); - const mjData* data = world->data(); - if (model == nullptr || data == nullptr || render_data_ == nullptr) { + std::lock_guard world_lock(world->mutex()); + mjModel* model = world->model(); + mjData* data = world->data(); + if (model == nullptr || 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 - // OpenCV-compatible BGR frames (as UVC and RealSense do). - cv::cvtColor(rgb_mat, color, cv::COLOR_RGB2BGR); + color = rgb_mat.clone(); 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; } diff --git a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h b/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h index fcc50dfe..ec700d96 100644 --- a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h +++ b/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; diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index da7396cc..a22918c1 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/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 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_); - clear_error_(); 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( + std::chrono::duration_cast( + 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::high_resolution_clock::now() - frame_start_time + std::chrono::steady_clock::now() - frame_start_time ).count(); // 计算需要休眠的时间(确保总耗时达到frame_interval) @@ -1011,12 +1020,24 @@ bool RealsenseCamera::startStreaming() void RealsenseCamera::stopStreaming() { - std::lock_guard lock(ctrl_mtx_); - stream_count_--; - if (stream_count_ == 0) + std::shared_ptr stream_thread_to_join; { + std::lock_guard lock(ctrl_mtx_); + if (stream_count_ > 0) { + stream_count_--; + } + if (stream_count_ == 0) + { // 当前已经没有正在使用的流了，编码采集线程状态修改 - state_.is_streaming = false; + 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(); } } diff --git a/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt b/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt index 0b36983e..6ba925bb 100644 --- a/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt +++ b/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) diff --git a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h index 651e0dbd..14194a28 100644 --- a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h +++ b/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 +#include + #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 video_writer_; std::shared_ptr stream_thread_; std::shared_ptr recording_thread_; + std::shared_ptr capture_thread_; - - cv::Mat latest_frame_; // 存储最新帧 - std::mutex frame_mutex_; // 保护最新帧的访问 + AVFormatContext* capture_format_context_ = nullptr; + AVCodecContext* capture_decoder_context_ = nullptr; + AVBufferRef* capture_hw_device_context_ = nullptr; + SwsContext* capture_sws_context_ = nullptr; + int capture_video_stream_index_ = -1; + std::atomic 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; diff --git a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp index 8e8be98d..2105d234 100644 --- a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp +++ b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp @@ -3,13 +3,118 @@ // Created by xtkuang on 2025/5/30. // +#include +#include + #include "../include/uvc_camera.h" +extern "C" { +#include +#include +#include +} + using namespace std; using namespace cmvr::device; #define USE_LIST_IMAGE 1 +namespace { + +std::string ffmpeg_error_string(const int error_code) +{ + char error_buffer[AV_ERROR_MAX_STRING_SIZE] = {}; + av_strerror(error_code, error_buffer, sizeof(error_buffer)); + return error_buffer; +} + +AVPixelFormat select_qsv_pixel_format(AVCodecContext*, const AVPixelFormat* formats) +{ + for (const AVPixelFormat* format = formats; *format != AV_PIX_FMT_NONE; ++format) { + if (*format == AV_PIX_FMT_QSV) { + return *format; + } + } + CMVR_LOG(ERROR) << "[UVCCamera] mjpeg_qsv decoder did not offer QSV frames"; + return AV_PIX_FMT_NONE; +} + +AVPixelFormat normalize_deprecated_yuv_format(const AVPixelFormat format) +{ + switch (format) { + case AV_PIX_FMT_YUVJ420P: + return AV_PIX_FMT_YUV420P; + case AV_PIX_FMT_YUVJ422P: + return AV_PIX_FMT_YUV422P; + case AV_PIX_FMT_YUVJ444P: + return AV_PIX_FMT_YUV444P; + case AV_PIX_FMT_YUVJ440P: + return AV_PIX_FMT_YUV440P; + default: + return format; + } +} + +int swscale_colorspace(const AVColorSpace colorspace) +{ + switch (colorspace) { + case AVCOL_SPC_BT709: + return SWS_CS_ITU709; + case AVCOL_SPC_BT2020_NCL: + case AVCOL_SPC_BT2020_CL: + return SWS_CS_BT2020; + case AVCOL_SPC_SMPTE170M: + case AVCOL_SPC_BT470BG: + return SWS_CS_ITU601; + default: + return SWS_CS_DEFAULT; + } +} + +bool is_sof_marker(const uint8_t marker) +{ + return marker >= 0xc0 && marker <= 0xcf + && marker != 0xc4 && marker != 0xc8 && marker != 0xcc; +} + +bool is_complete_mjpeg_packet(const AVPacket* packet) +{ + if (!packet || !packet->data || packet->size < 4 + || packet->data[0] != 0xff || packet->data[1] != 0xd8) { + return false; + } + + bool found_sof = false; + for (int index = 2; index + 1 < packet->size; ++index) { + if (packet->data[index] != 0xff) { + continue; + } + + int marker_index = index + 1; + while (marker_index < packet->size && packet->data[marker_index] == 0xff) { + ++marker_index; + } + if (marker_index >= packet->size) { + break; + } + + const uint8_t marker = packet->data[marker_index]; + if (marker == 0x00) { + index = marker_index; + continue; + } + if (is_sof_marker(marker)) { + found_sof = true; + } else if (marker == 0xd9) { + return found_sof; + } + index = marker_index; + } + return false; +} + +} // namespace + UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera) { id_ = camera_.id(); @@ -51,12 +156,19 @@ UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera) } UVCCamera::~UVCCamera() { stop(); + close_capture_(); if (stream_thread_ && stream_thread_->joinable()) stream_thread_->join(); if (recording_thread_ && recording_thread_->joinable()) recording_thread_->join(); } +void UVCCamera::getState(CameraState& state) +{ + std::lock_guard lock(ctrl_mtx_); + state = state_; +} + bool UVCCamera::init() { std::lock_guard lock(ctrl_mtx_); clear_error_(); @@ -64,55 +176,29 @@ bool UVCCamera::init() { stream_frame_buffer_ = std::make_shared>(buffer_size_); stream_frame_buffer_->clear(); - if (cap_.isOpened()) { - cap_.release(); - } - cap_.open(serial_, cv::CAP_V4L2); - this_thread::sleep_for(chrono::milliseconds(100)); - - if (!cap_.isOpened()) { + // Validate the requested V4L2 mode during initialization. start() reopens + // the device and owns it for the lifetime of the camera session. + if (!open_capture_()) { state_.is_error = true; - state_.error_message = "Failed to open USB camera at index " + serial_; - CMVR_LOG(ERROR) << "[UVCCamera] (init)" << state_.error_message; + state_.error_message = capture_error_; + CMVR_LOG(ERROR) << "[UVCCamera] (init): " << state_.error_message; return false; } + close_capture_(); - // 设置格式为MJPG - cap_.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); - - if (!cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_)) { - state_.is_error = true; - state_.error_message = "set width failed"; - CMVR_LOG(ERROR) << "[UVCCamera] (init): set width failed"; - return false; - } - state_.width = width_; - if (!cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_)) { - state_.is_error = true; - state_.error_message = "set height failed"; - CMVR_LOG(ERROR) << "[UVCCamera] (init): set height failed"; - return false; - } - state_.height = height_; - if (!cap_.set(cv::CAP_PROP_FPS, fps_)) { - state_.is_error = true; - state_.error_message = "set fps failed"; - CMVR_LOG(ERROR) << "[UVCCamera] (init): set fps failed"; - return false; - } - //初始化编码器 - // 初始化RGB编码器（示例参数：640x480，30fps，H.264） if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) { - CMVR_LOG(ERROR) << "[UVCCamera] (start): Failed to init RGB encoder!"; + CMVR_LOG(ERROR) << "[UVCCamera] (init): Failed to init RGB encoder!"; state_.is_error = true; state_.error_message = "Failed to init RGB encoder!"; return false; } state_.fps = fps_; + state_.width = width_; + state_.height = height_; state_.is_initialized = true; state_.is_error = false; - CMVR_LOG(INFO) << "[UVCCamera] (init): UVC camera initialized at index " << serial_; // 记录初始化成功日志 + CMVR_LOG(INFO) << "[UVCCamera] (init): UVC camera initialized at " << serial_; return true; } @@ -129,46 +215,55 @@ bool UVCCamera::start() { CMVR_LOG(WARNING) << "[RealsenseCamera] (start): camera already started"; return true; } - cap_.open(serial_, cv::CAP_V4L2); - this_thread::sleep_for(chrono::milliseconds(100)); - if (!cap_.isOpened()) { + if (!open_capture_()) { state_.is_error = true; - state_.error_message = "Failed to open USB camera at index " + serial_; - CMVR_LOG(ERROR) << "[UVCCamera] (init)" << state_.error_message; + state_.error_message = capture_error_; + CMVR_LOG(ERROR) << "[UVCCamera] (start): " << state_.error_message; return false; } - // 设置格式为MJPG - cap_.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); + try { + capture_thread_ = std::make_shared(&UVCCamera::capture_worker_, this); + } catch (const std::exception& e) { + capture_error_ = std::string("failed to start capture thread: ") + e.what(); + close_capture_(); + state_.is_error = true; + state_.error_message = capture_error_; + CMVR_LOG(ERROR) << "[UVCCamera] (start): " << state_.error_message; + return false; + } - if (!cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_)) { + AVFrame* first_frame = av_frame_alloc(); + uint64_t first_sequence = 0; + int64_t first_capture_monotonic_ns = 0; + int64_t first_capture_utc_ns = 0; + if (!first_frame || !wait_for_capture_frame_(first_frame, + first_sequence, + first_capture_monotonic_ns, + first_capture_utc_ns)) { + std::string capture_error; + { + std::lock_guard capture_lock(capture_frame_mutex_); + capture_error = capture_error_; + } + close_capture_(); state_.is_error = true; - state_.error_message = "set width failed"; - CMVR_LOG(ERROR) << "[UVCCamera] (init): set width failed"; - return false; - } - state_.width = width_; - if (!cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_)) { - state_.is_error = true; - state_.error_message = "set height failed"; - CMVR_LOG(ERROR) << "[UVCCamera] (init): set height failed"; - return false; - } - state_.height = height_; - if (!cap_.set(cv::CAP_PROP_FPS, fps_)) { - state_.is_error = true; - state_.error_message = "set fps failed"; - CMVR_LOG(ERROR) << "[UVCCamera] (init): set fps failed"; + state_.error_message = capture_error.empty() + ? "timed out waiting for the first camera frame" + : capture_error; + CMVR_LOG(ERROR) << "[UVCCamera] (start): " << state_.error_message; + av_frame_free(&first_frame); return false; } + av_frame_free(&first_frame); state_.is_opened = true; return true; } bool UVCCamera::stop() { //先停止录制再关闭摄像头 - if (state_.is_recording) { + if (state_.is_recording || recording_thread_ || packet_ || format_context_ || stream_) { stopRecording(); } std::lock_guard lock(ctrl_mtx_); @@ -180,16 +275,16 @@ bool UVCCamera::stop() { } if (mode_ == VIDEO_MODE){ state_.is_streaming = false; - if (stream_thread_->joinable()) { - stream_thread_->join(); + if (stream_thread_) { + if (stream_thread_->joinable()) { + stream_thread_->join(); + } stream_thread_.reset(); - stream_thread_ = nullptr; } + is_streaming_running = false; } - if (cap_.isOpened()) { - cap_.release(); - } + close_capture_(); state_.is_opened = false; return true; } @@ -205,32 +300,471 @@ void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { std::lock_guard lock(ctrl_mtx_); clear_error_(); - if (mode_ == PHOTO_MODE) { - if (!state_.is_opened) { - state_.is_error = true; - state_.error_message = "camera not opened"; - CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; - color.release(); - return; - } - if (!cap_.read(color)) { - state_.is_error = true; - state_.error_message = "read color image failed"; - CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; - color.release(); - return; - } + color.release(); + if (!state_.is_opened) { + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; + return; } - else if (mode_ == VIDEO_MODE) + + uint64_t requested_sequence = 0; { - if (!state_.is_opened) { - state_.is_error = true; - state_.error_message = "camera not opened"; - CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; - color.release(); - return; + std::lock_guard capture_lock(capture_frame_mutex_); + requested_sequence = latest_capture_sequence_; + } + + int64_t capture_monotonic_ns = 0; + int64_t capture_utc_ns = 0; + AVFrame* capture_frame = av_frame_alloc(); + const bool received_frame = capture_frame + && wait_for_capture_frame_(capture_frame, + requested_sequence, + capture_monotonic_ns, + capture_utc_ns); + const bool converted_frame = received_frame + && convert_capture_frame_to_bgr_(capture_frame, color); + av_frame_free(&capture_frame); + if (!converted_frame || color.empty()) { + std::string capture_error; + { + std::lock_guard capture_lock(capture_frame_mutex_); + capture_error = capture_error_; + } + state_.is_error = true; + state_.error_message = capture_error.empty() + ? "timed out waiting for a fresh camera frame" + : capture_error; + CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; + color.release(); + } +} + +int UVCCamera::interrupt_capture_(void* opaque) +{ + const auto* camera = static_cast(opaque); + return camera && !camera->capture_running_.load(std::memory_order_relaxed); +} + +bool UVCCamera::open_capture_() +{ + close_capture_(); + + { + std::lock_guard frame_lock(capture_frame_mutex_); + av_frame_free(&latest_capture_frame_); + latest_capture_sequence_ = 0; + latest_capture_monotonic_ns_ = 0; + latest_capture_utc_ns_ = 0; + capture_error_.clear(); + } + + avdevice_register_all(); + auto* input_format = av_find_input_format("v4l2"); + if (!input_format) { + capture_error_ = "FFmpeg was built without the v4l2 input device"; + return false; + } + + capture_format_context_ = avformat_alloc_context(); + if (!capture_format_context_) { + capture_error_ = "failed to allocate FFmpeg capture context"; + return false; + } + + capture_running_.store(true, std::memory_order_relaxed); + capture_format_context_->interrupt_callback.callback = &UVCCamera::interrupt_capture_; + capture_format_context_->interrupt_callback.opaque = this; + capture_format_context_->flags |= AVFMT_FLAG_NOBUFFER; + + AVDictionary* options = nullptr; + const std::string video_size = std::to_string(width_) + "x" + std::to_string(height_); + av_dict_set(&options, "input_format", "mjpeg", 0); + av_dict_set(&options, "video_size", video_size.c_str(), 0); + av_dict_set(&options, "framerate", std::to_string(fps_).c_str(), 0); + + int result = avformat_open_input( + &capture_format_context_, serial_.c_str(), input_format, &options); + av_dict_free(&options); + if (result < 0) { + const std::string error = + "failed to open " + serial_ + ": " + ffmpeg_error_string(result); + close_capture_(); + capture_error_ = error; + return false; + } + + result = avformat_find_stream_info(capture_format_context_, nullptr); + if (result < 0) { + const std::string error = + "failed to read camera stream info: " + ffmpeg_error_string(result); + close_capture_(); + capture_error_ = error; + return false; + } + + capture_video_stream_index_ = av_find_best_stream( + capture_format_context_, AVMEDIA_TYPE_VIDEO, -1, -1, nullptr, 0); + if (capture_video_stream_index_ < 0) { + const std::string error = "camera has no video stream: " + + ffmpeg_error_string(capture_video_stream_index_); + close_capture_(); + capture_error_ = error; + return false; + } + + AVStream* video_stream = capture_format_context_->streams[capture_video_stream_index_]; + if (video_stream->codecpar->codec_id != AV_CODEC_ID_MJPEG) { + const std::string error = "camera input is not MJPEG; QSV MJPEG decoding is unavailable"; + close_capture_(); + capture_error_ = error; + return false; + } + + const AVCodec* decoder = avcodec_find_decoder_by_name("mjpeg_qsv"); + if (!decoder) { + const std::string error = "FFmpeg was built without the mjpeg_qsv decoder"; + close_capture_(); + capture_error_ = error; + return false; + } + + capture_decoder_context_ = avcodec_alloc_context3(decoder); + if (!capture_decoder_context_) { + const std::string error = "failed to allocate camera decoder context"; + close_capture_(); + capture_error_ = error; + return false; + } + + result = avcodec_parameters_to_context(capture_decoder_context_, video_stream->codecpar); + if (result >= 0) { + const AVRational stream_time_base = video_stream->time_base; + capture_decoder_context_->pkt_timebase = + stream_time_base.num > 0 && stream_time_base.den > 0 + ? stream_time_base + : AVRational{1, AV_TIME_BASE}; + capture_decoder_context_->framerate = av_guess_frame_rate( + capture_format_context_, video_stream, nullptr); + } + if (result >= 0) { + AVBufferRef* vaapi_device_context = nullptr; + result = av_hwdevice_ctx_create( + &vaapi_device_context, + AV_HWDEVICE_TYPE_VAAPI, + "/dev/dri/renderD128", + nullptr, + 0); + if (result >= 0) { + result = av_hwdevice_ctx_create_derived( + &capture_hw_device_context_, + AV_HWDEVICE_TYPE_QSV, + vaapi_device_context, + 0); + } + av_buffer_unref(&vaapi_device_context); + } + if (result >= 0) { + capture_decoder_context_->hw_device_ctx = av_buffer_ref(capture_hw_device_context_); + if (!capture_decoder_context_->hw_device_ctx) { + result = AVERROR(ENOMEM); + } else { + capture_decoder_context_->get_format = select_qsv_pixel_format; } } + if (result >= 0) { + result = avcodec_open2(capture_decoder_context_, decoder, nullptr); + } + if (result < 0) { + const std::string error = + "failed to initialize mjpeg_qsv decoder: " + ffmpeg_error_string(result); + close_capture_(); + capture_error_ = error; + return false; + } + + CMVR_LOG(INFO) << "[UVCCamera] FFmpeg capture opened" + << ", device=" << serial_ + << ", input_codec=" << avcodec_get_name(video_stream->codecpar->codec_id) + << ", decoder=" << decoder->name + << ", hardware_device=qsv" + << ", width=" << capture_decoder_context_->width + << ", height=" << capture_decoder_context_->height + << ", packet_time_base=" << capture_decoder_context_->pkt_timebase.num + << "/" << capture_decoder_context_->pkt_timebase.den + << ", requested_fps=" << fps_; + return true; +} + +void UVCCamera::close_capture_() +{ + capture_running_.store(false, std::memory_order_relaxed); + capture_frame_cv_.notify_all(); + + if (capture_thread_) { + if (capture_thread_->joinable()) { + capture_thread_->join(); + } + capture_thread_.reset(); + } + + if (capture_sws_context_) { + sws_freeContext(capture_sws_context_); + capture_sws_context_ = nullptr; + } + if (capture_decoder_context_) { + avcodec_free_context(&capture_decoder_context_); + } + av_buffer_unref(&capture_hw_device_context_); + if (capture_format_context_) { + avformat_close_input(&capture_format_context_); + } + { + std::lock_guard frame_lock(capture_frame_mutex_); + av_frame_free(&latest_capture_frame_); + } + capture_video_stream_index_ = -1; +} + +void UVCCamera::capture_worker_() +{ + AVPacket* input_packet = av_packet_alloc(); + AVFrame* decoded_frame = av_frame_alloc(); + uint64_t discarded_frame_count = 0; + if (!input_packet || !decoded_frame) { + { + std::lock_guard frame_lock(capture_frame_mutex_); + capture_error_ = "failed to allocate FFmpeg capture frame resources"; + } + capture_running_.store(false, std::memory_order_relaxed); + capture_frame_cv_.notify_all(); + av_packet_free(&input_packet); + av_frame_free(&decoded_frame); + return; + } + + while (capture_running_.load(std::memory_order_relaxed)) { + int result = av_read_frame(capture_format_context_, input_packet); + if (result == AVERROR(EAGAIN)) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + if (result < 0) { + if (capture_running_.load(std::memory_order_relaxed)) { + std::lock_guard frame_lock(capture_frame_mutex_); + capture_error_ = "failed to read camera packet: " + ffmpeg_error_string(result); + } + break; + } + + if (input_packet->stream_index != capture_video_stream_index_) { + av_packet_unref(input_packet); + continue; + } + + if (!is_complete_mjpeg_packet(input_packet)) { + av_packet_unref(input_packet); + ++discarded_frame_count; + if (discarded_frame_count == 1 || discarded_frame_count % 30 == 0) { + CMVR_LOG(WARNING) << "[UVCCamera] discarded incomplete MJPEG frame" + << ", device=" << serial_ + << ", discarded=" << discarded_frame_count; + } + continue; + } + + result = avcodec_send_packet(capture_decoder_context_, input_packet); + av_packet_unref(input_packet); + if (result == AVERROR_INVALIDDATA) { + ++discarded_frame_count; + if (discarded_frame_count == 1 || discarded_frame_count % 30 == 0) { + CMVR_LOG(WARNING) << "[UVCCamera] discarded invalid MJPEG frame" + << ", device=" << serial_ + << ", discarded=" << discarded_frame_count; + } + avcodec_flush_buffers(capture_decoder_context_); + av_frame_unref(decoded_frame); + continue; + } + if (result < 0 && result != AVERROR(EAGAIN)) { + std::lock_guard frame_lock(capture_frame_mutex_); + capture_error_ = "failed to submit camera packet: " + ffmpeg_error_string(result); + break; + } + + while (capture_running_.load(std::memory_order_relaxed)) { + result = avcodec_receive_frame(capture_decoder_context_, decoded_frame); + if (result == AVERROR(EAGAIN) || result == AVERROR_EOF) { + break; + } + if (result == AVERROR_INVALIDDATA) { + ++discarded_frame_count; + if (discarded_frame_count == 1 || discarded_frame_count % 30 == 0) { + CMVR_LOG(WARNING) << "[UVCCamera] discarded undecodable MJPEG frame" + << ", device=" << serial_ + << ", discarded=" << discarded_frame_count; + } + avcodec_flush_buffers(capture_decoder_context_); + av_frame_unref(decoded_frame); + break; + } + if (result < 0) { + std::lock_guard frame_lock(capture_frame_mutex_); + capture_error_ = "failed to decode camera frame: " + ffmpeg_error_string(result); + capture_running_.store(false, std::memory_order_relaxed); + break; + } + + const auto capture_monotonic = std::chrono::steady_clock::now(); + const auto capture_utc = std::chrono::system_clock::now(); + AVFrame* cached_frame = av_frame_clone(decoded_frame); + if (!cached_frame) { + std::lock_guard frame_lock(capture_frame_mutex_); + capture_error_ = "failed to retain QSV camera frame"; + capture_running_.store(false, std::memory_order_relaxed); + break; + } + { + std::lock_guard frame_lock(capture_frame_mutex_); + av_frame_free(&latest_capture_frame_); + latest_capture_frame_ = cached_frame; + ++latest_capture_sequence_; + latest_capture_monotonic_ns_ = std::chrono::duration_cast( + capture_monotonic.time_since_epoch()).count(); + latest_capture_utc_ns_ = std::chrono::duration_cast( + capture_utc.time_since_epoch()).count(); + capture_error_.clear(); + } + capture_frame_cv_.notify_all(); + av_frame_unref(decoded_frame); + } + } + + capture_running_.store(false, std::memory_order_relaxed); + capture_frame_cv_.notify_all(); + av_packet_free(&input_packet); + av_frame_free(&decoded_frame); +} + +bool UVCCamera::wait_for_capture_frame_(AVFrame* frame, + uint64_t& last_sequence, + int64_t& capture_monotonic_ns, + int64_t& capture_utc_ns) +{ + if (!frame) { + return false; + } + std::unique_lock frame_lock(capture_frame_mutex_); + const auto timeout = std::chrono::milliseconds( + std::max(500, fps_ > 0 ? 3000 / fps_ : 500)); + const bool ready = capture_frame_cv_.wait_for(frame_lock, timeout, [&] { + return latest_capture_sequence_ > last_sequence + || !capture_running_.load(std::memory_order_relaxed); + }); + if (!ready || latest_capture_sequence_ <= last_sequence || !latest_capture_frame_) { + return false; + } + + av_frame_unref(frame); + if (av_frame_ref(frame, latest_capture_frame_) < 0) { + return false; + } + last_sequence = latest_capture_sequence_; + capture_monotonic_ns = latest_capture_monotonic_ns_; + capture_utc_ns = latest_capture_utc_ns_; + return true; +} + +bool UVCCamera::convert_capture_frame_to_bgr_(const AVFrame* frame, cv::Mat& bgr_frame) +{ + bgr_frame.release(); + if (!frame) { + return false; + } + + std::lock_guard conversion_lock(capture_conversion_mutex_); + AVFrame* software_frame = av_frame_alloc(); + if (!software_frame) { + return false; + } + + const AVFrame* source_frame = frame; + int result = 0; + if (frame->hw_frames_ctx) { + result = av_hwframe_transfer_data(software_frame, frame, 0); + if (result >= 0) { + result = av_frame_copy_props(software_frame, frame); + } + if (result < 0) { + CMVR_LOG(ERROR) << "[UVCCamera] failed to prepare BGR source frame: " + << ffmpeg_error_string(result); + av_frame_free(&software_frame); + return false; + } + source_frame = software_frame; + } + + const auto decoded_format = static_cast(source_frame->format); + const auto source_format = normalize_deprecated_yuv_format(decoded_format); + const bool source_full_range = source_frame->color_range == AVCOL_RANGE_JPEG + || source_format != decoded_format; + capture_sws_context_ = sws_getCachedContext( + capture_sws_context_, + source_frame->width, + source_frame->height, + source_format, + width_, + height_, + AV_PIX_FMT_BGR24, + SWS_BILINEAR, + nullptr, + nullptr, + nullptr); + if (!capture_sws_context_) { + CMVR_LOG(ERROR) << "[UVCCamera] failed to create BGR conversion context"; + av_frame_free(&software_frame); + return false; + } + + const int* color_coefficients = sws_getCoefficients( + swscale_colorspace(source_frame->colorspace)); + result = sws_setColorspaceDetails( + capture_sws_context_, + color_coefficients, + source_full_range ? 1 : 0, + color_coefficients, + 1, + 0, + 1 << 16, + 1 << 16); + if (result < 0) { + CMVR_LOG(ERROR) << "[UVCCamera] failed to configure BGR color conversion: " + << ffmpeg_error_string(result); + av_frame_free(&software_frame); + return false; + } + + bgr_frame.create(height_, width_, CV_8UC3); + uint8_t* destination_data[] = {bgr_frame.data, nullptr, nullptr, nullptr}; + int destination_linesize[] = { + static_cast(bgr_frame.step[0]), 0, 0, 0 + }; + const int converted_rows = sws_scale( + capture_sws_context_, + source_frame->data, + source_frame->linesize, + 0, + source_frame->height, + destination_data, + destination_linesize); + av_frame_free(&software_frame); + if (converted_rows != height_) { + CMVR_LOG(ERROR) << "[UVCCamera] BGR conversion returned " << converted_rows + << " rows, expected " << height_; + bgr_frame.release(); + return false; + } + return true; } void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { @@ -248,6 +782,22 @@ void UVCCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& int depth.release(); } +void UVCCamera::cleanup_recording_resources_() { + if (packet_) { + av_packet_free(&packet_); + } + + if (format_context_) { + if (!(format_context_->oformat->flags & AVFMT_NOFILE) && format_context_->pb) { + avio_closep(&format_context_->pb); + } + avformat_free_context(format_context_); + format_context_ = nullptr; + } + + stream_ = nullptr; +} + void UVCCamera::startRecording(const std::string &video_path) { std::lock_guard lock(ctrl_mtx_); clear_error_(); @@ -270,27 +820,18 @@ void UVCCamera::startRecording(const std::string &video_path) { return; } - auto cleanup_recording_resources = [this]() { - if (packet_) av_packet_free(&packet_); - if (stream_ && stream_->codecpar->extradata) av_free(stream_->codecpar->extradata); - if (format_context_) { - if (!(format_context_->oformat->flags & AVFMT_NOFILE) && format_context_->pb) avio_closep(&format_context_->pb); - avformat_free_context(format_context_); - } - stream_ = nullptr; - format_context_ = nullptr; - state_.is_recording = false; - }; - try { current_video_path_ = video_path; std::string temp_path = current_video_path_ + ".temp"; // 临时文件 // 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; @@ -298,7 +839,8 @@ void UVCCamera::startRecording(const std::string &video_path) { CMVR_LOG(ERROR) << "[UVCCamera] (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) { @@ -314,7 +856,7 @@ void UVCCamera::startRecording(const std::string &video_path) { state_.is_error = true; state_.error_message = "failed to create video stream"; CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; - cleanup_recording_resources(); + cleanup_recording_resources_(); return; } stream_ = stream; @@ -323,7 +865,9 @@ void UVCCamera::startRecording(const std::string &video_path) { codecpar->codec_id = codec_id; codecpar->width = width_; codecpar->height = height_; - stream->time_base = {1, fps_}; // 时间基：1/fps（每帧间隔1个时间单位） + // Use a fine-grained time base because the actual UVC frame rate can + // differ significantly from the configured frame rate. + stream->time_base = {1, 1'000'000}; // 4. 复制编码器的extradata（如H.264的SPS/PPS，确保播放器能解析） if (rgbEncoder_ && rgbEncoder_->codec_context && rgbEncoder_->codec_context->extradata) { @@ -332,7 +876,7 @@ void UVCCamera::startRecording(const std::string &video_path) { state_.is_error = true; state_.error_message = "failed to allocate extradata"; CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; - cleanup_recording_resources(); + cleanup_recording_resources_(); return; } memcpy(codecpar->extradata, rgbEncoder_->codec_context->extradata, rgbEncoder_->codec_context->extradata_size); @@ -345,7 +889,7 @@ void UVCCamera::startRecording(const std::string &video_path) { state_.is_error = true; state_.error_message = "failed to open output file: " + temp_path; CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; - cleanup_recording_resources(); + cleanup_recording_resources_(); return; } } @@ -355,7 +899,7 @@ void UVCCamera::startRecording(const std::string &video_path) { state_.is_error = true; state_.error_message = "failed to write file header"; CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; - cleanup_recording_resources(); + cleanup_recording_resources_(); return; } @@ -365,7 +909,7 @@ void UVCCamera::startRecording(const std::string &video_path) { state_.is_error = true; state_.error_message = "failed to allocate AVPacket"; CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; - cleanup_recording_resources(); + cleanup_recording_resources_(); return; } //不在录像也不在流传输，但是采集线程没有退出时。 @@ -398,7 +942,8 @@ void UVCCamera::startRecording(const std::string &video_path) { recording_thread_ = make_shared(&UVCCamera::recording_worker_, this); } catch (const std::exception& e) { - cleanup_recording_resources(); + cleanup_recording_resources_(); + state_.is_recording = false; state_.is_error = true; state_.error_message = "[UVCCamera] (startRecording): " + std::string(e.what()); CMVR_LOG(ERROR) << state_.error_message; @@ -414,36 +959,24 @@ void UVCCamera::stopRecording() { CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message; return; } - if (!state_.is_recording) { + const bool has_recording_resources = + recording_thread_ || packet_ || format_context_ || stream_; + if (!state_.is_recording && !has_recording_resources) { CMVR_LOG(WARNING) << "[UVCCamera] (stopRecording): not recording"; return; } // 1. 停止录像线程 state_.is_recording = false; - if (recording_thread_ && recording_thread_->joinable()) { - recording_thread_->join(); + if (recording_thread_) { + if (recording_thread_->joinable()) { + recording_thread_->join(); + } recording_thread_.reset(); } // 2. 清理FFmpeg资源 - if (packet_) { - av_packet_free(&packet_); - packet_ = nullptr; - } - if (stream_ && stream_->codecpar->extradata) { - av_free(stream_->codecpar->extradata); - stream_->codecpar->extradata = nullptr; - stream_->codecpar->extradata_size = 0; - } - if (format_context_) { - if (!(format_context_->oformat->flags & AVFMT_NOFILE) && format_context_->pb) { - avio_closep(&format_context_->pb); // 关闭文件 - } - avformat_free_context(format_context_); // 释放格式上下文 - format_context_ = nullptr; - } - stream_ = nullptr; + cleanup_recording_resources_(); // 3. 重命名临时文件为目标文件 std::string temp_path = current_video_path_ + ".temp"; @@ -465,81 +998,141 @@ void UVCCamera::resumeRecording() { } void UVCCamera::streaming_worker_() { - + AVFrame* frame = av_frame_alloc(); + if (!frame) { + is_streaming_running = false; + state_.is_error = true; + state_.error_message = "failed to allocate streaming frame"; + CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message; + return; + } try { // 计算理论上每帧之间的间隔时间(毫秒) const int frame_interval = 1000 / fps_; bool success = false; is_streaming_running = true; - cv::Mat frame; - int64_t frame_count = 0; + uint64_t capture_sequence = 0; + int64_t frame_capture_monotonic_ns = 0; + int64_t frame_capture_utc_ns = 0; + uint64_t frame_sequence = 0; + const uint64_t stream_epoch = static_cast( + std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count()); + + constexpr int kTimingWindowFrames = 30; + int timing_frame_count = 0; + int timing_encoded_count = 0; + int64_t capture_us = 0; + int64_t copy_us = 0; + int64_t resize_us = 0; + int64_t encode_us = 0; + int64_t processing_us = 0; + auto timing_window_start = std::chrono::steady_clock::now(); + // 处于流传输或者录像状态时就不退出线程 while (state_.is_streaming || state_.is_recording) { // 记录当前帧处理开始时间 - auto frame_start_time = std::chrono::high_resolution_clock::now(); - if (!cap_.read(frame) || frame.empty()) { + const auto frame_start_time = std::chrono::steady_clock::now(); + if (!wait_for_capture_frame_(frame, + capture_sequence, + frame_capture_monotonic_ns, + frame_capture_utc_ns)) { + std::string capture_error; + { + std::lock_guard capture_lock(capture_frame_mutex_); + capture_error = capture_error_; + } state_.is_error = true; - state_.error_message = "failed to read frame"; + state_.error_message = capture_error.empty() + ? "timed out waiting for camera frame" + : capture_error; CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message; break; } + const auto capture_end_time = std::chrono::steady_clock::now(); // 以下为编码部分，用于流模式 StreamFrameData frame_data; // 保存图像数据 - { - frame.copyTo(frame_data.rgbImage); // 执行深拷贝 - } + const auto copy_end_time = std::chrono::steady_clock::now(); + // rgb图像编码 - cv::Mat rgb_to_encode = frame_data.rgbImage; - if (encode_width_ > 0 && encode_height_ > 0 && - (frame_data.rgbImage.cols != encode_width_ || frame_data.rgbImage.rows != encode_height_)) { - cv::resize(frame_data.rgbImage, - rgb_to_encode, - cv::Size(encode_width_, encode_height_), - 0.0, - 0.0, - cv::INTER_LINEAR); - } + const auto resize_end_time = std::chrono::steady_clock::now(); + 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 = CameraStreamEncoder::encode( + rgbEncoder_, frame, frame_data.rgbFrame, frame_data.bKey, encode_options); + const auto encode_end_time = std::chrono::steady_clock::now(); + if (success) { + const uint64_t sequence = frame_sequence++; + frame_data.stream_epoch = stream_epoch; + frame_data.sequence = sequence; + frame_data.source_frame_number = capture_sequence; + frame_data.capture_monotonic_ns = frame_capture_monotonic_ns; + frame_data.capture_utc_ns = frame_capture_utc_ns; + frame_data.pts = static_cast(sequence); + frame_data.dts = frame_data.pts; + frame_data.time_base_num = 1; + frame_data.time_base_den = fps_; + frame_data.duration = 1; frame_data.fps = fps_; frame_data.width = encode_width_; frame_data.height = encode_height_; frame_data.codec = codec_; stream_frame_buffer_->push(frame_data); + ++timing_encoded_count; } - else - { + + capture_us += std::chrono::duration_cast( + capture_end_time - frame_start_time).count(); + copy_us += std::chrono::duration_cast( + copy_end_time - capture_end_time).count(); + resize_us += std::chrono::duration_cast( + resize_end_time - copy_end_time).count(); + encode_us += std::chrono::duration_cast( + encode_end_time - resize_end_time).count(); + processing_us += std::chrono::duration_cast( + encode_end_time - frame_start_time).count(); + ++timing_frame_count; + + if (timing_frame_count == kTimingWindowFrames) { + const auto now = std::chrono::steady_clock::now(); + const auto window_us = std::chrono::duration_cast( + now - timing_window_start).count(); + const double actual_fps = window_us > 0 + ? timing_frame_count * 1'000'000.0 / static_cast(window_us) + : 0.0; + const double divisor = static_cast(timing_frame_count) * 1000.0; + + CMVR_LOG(INFO) << "[UVCCamera] pipeline timing" + << ", device=" << serial_ + << ", codec=" << codec_ + << ", actual_fps=" << actual_fps + << ", encoded=" << timing_encoded_count << "/" << timing_frame_count + << ", capture_ms=" << capture_us / divisor + << ", copy_ms=" << copy_us / divisor + << ", resize_ms=" << resize_us / divisor + << ", encode_ms=" << encode_us / divisor + << ", processing_ms=" << processing_us / divisor; + + timing_frame_count = 0; + timing_encoded_count = 0; + capture_us = 0; + copy_us = 0; + resize_us = 0; + encode_us = 0; + processing_us = 0; + timing_window_start = now; + } + + if (!success) { std::this_thread::sleep_for(std::chrono::milliseconds(frame_interval)); continue; } - // 计算从帧开始到现在的总耗时 - auto total_duration = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - frame_start_time - ).count(); - // 计算需要休眠的时间(确保总耗时达到frame_interval) - int sleep_time = frame_interval - total_duration; - // 只有当需要休眠的时间为正数时才休眠 - if (sleep_time > 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time)); - } } is_streaming_running = false; @@ -558,12 +1151,15 @@ void UVCCamera::streaming_worker_() { state_.error_message = e.what(); CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message; } + av_frame_free(&frame); } void UVCCamera::recording_worker_() { is_recording_running = true; const int frame_interval = 1000 / fps_; bool is_first_key = false; + int64_t first_capture_monotonic_ns = 0; + int64_t last_packet_pts = AV_NOPTS_VALUE; try { //保证当前采集线程正常运行 while (state_.is_recording && is_streaming_running) { @@ -606,25 +1202,33 @@ void UVCCamera::recording_worker_() { packet_->size = static_cast(frame_data.rgbFrame.size()); packet_->stream_index = stream_->index; // 指定流索引 - // 设置时间戳（递增） - packet_->pts = frame_count_++; - packet_->dts = packet_->pts; - packet_->duration = 1; // 每帧持续1个时间单位（根据time_base） + // Preserve real capture time so dropped/late frames do not speed up playback. + if (first_capture_monotonic_ns == 0) { + first_capture_monotonic_ns = frame_data.capture_monotonic_ns; + } + const int64_t elapsed_ns = std::max( + 0, frame_data.capture_monotonic_ns - first_capture_monotonic_ns); + int64_t packet_pts = av_rescale_q( + elapsed_ns, AVRational{1, 1'000'000'000}, stream_->time_base); + if (last_packet_pts != AV_NOPTS_VALUE && packet_pts <= last_packet_pts) { + packet_pts = last_packet_pts + 1; + } + packet_->pts = packet_pts; + packet_->dts = packet_pts; + packet_->duration = av_rescale_q( + 1, AVRational{1, fps_}, stream_->time_base); + last_packet_pts = packet_pts; + ++frame_count_; // 标记关键帧 if (frame_data.bKey) { packet_->flags |= AV_PKT_FLAG_KEY; } - // 转换时间基 - av_packet_rescale_ts(packet_, {1, fps_}, stream_->time_base); - // 写入帧 if (av_interleaved_write_frame(format_context_, packet_) < 0) { CMVR_LOG(ERROR) << "写入第" << frame_count_ << "帧失败"; } - - std::this_thread::sleep_for(std::chrono::milliseconds(33)); } // 7. 写入文件尾（完成封装） @@ -680,10 +1284,22 @@ bool UVCCamera::startStreaming() stream_thread_.reset(); } } + + // A recording session may already own the encoder worker. The streaming + // state still has to reflect the new client lease so stopping recording + // does not terminate the worker while clients are consuming frames. + state_.is_streaming = true; //开启流采集线程 if (!stream_thread_) { - state_.is_streaming = true; - stream_thread_ = make_shared(&UVCCamera::streaming_worker_, this); + try { + stream_thread_ = make_shared(&UVCCamera::streaming_worker_, this); + } catch (const std::exception& e) { + state_.is_streaming = stream_count_ > 0; + state_.is_error = true; + state_.error_message = "failed to start streaming worker: " + std::string(e.what()); + CMVR_LOG(ERROR) << "[UVCCamera] (startStreaming): " << state_.error_message; + return false; + } //延时100ms，等待流线程获取图像 std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -695,7 +1311,9 @@ bool UVCCamera::startStreaming() void UVCCamera::stopStreaming() { std::lock_guard lock(ctrl_mtx_); - stream_count_--; + if (stream_count_ > 0) { + --stream_count_; + } if (stream_count_ == 0) { // 当前已经没有正在使用的流了，编码采集线程状态修改 diff --git a/cmvr-es/service/grpc/include/grpc_camera_service.h b/cmvr-es/service/grpc/include/grpc_camera_service.h index d9bc1782..62e72277 100644 --- a/cmvr-es/service/grpc/include/grpc_camera_service.h +++ b/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* stream) override; grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; private: device::DeviceManager& dmgr_; + CameraStreamLowLatencyConfig stream_config_; //双向流读写线程 std::shared_ptr read_thread_ = nullptr; diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index 3f116d9b..449249ae 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/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 +#include +#include +#include #include #include @@ -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 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 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(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(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(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(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 (!frame_data.rgbFrame.empty()) { + if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) && + !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); @@ -403,29 +529,27 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con 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); + 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++); - 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(); + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): end,id=" << dev_id; return grpc::Status::OK; } - catch (exception &e) { + catch (const exception &e) { 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); @@ -437,9 +561,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con try { //读取首次传递的数据，获取设备id api::GetRGBDImagesStreamCommand_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] (GetRGBDImagesStream): start,id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { api::GetRGBDImagesStreamCommand_Feedback response; @@ -449,9 +575,17 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con 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; - dev->startStreaming(); - CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start streaming success, id=" << dev_id; size_t index = 0; while (true) { @@ -463,9 +597,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con api::GetRGBDImagesStreamCommand_Feedback response; cmvr::device::StreamFrameData frame_data; - dev->getEncodedFrame(frame_data,index); - if (!frame_data.rgbFrame.empty()) { + 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); @@ -480,17 +616,15 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con 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); + 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++); - grpc::WriteOptions options; - options.set_last_message(); if (!stream->Write(response)) { CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; break; @@ -498,11 +632,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con } } CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id; - dev->stopStreaming(); return grpc::Status::OK; } - catch (exception &e) { + 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); @@ -513,9 +647,13 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte try { //读取首次传递的数据，获取设备id api::GetRGBImageStreamCommand_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] (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(dev_id); if (!dev) { api::GetRGBImageStreamCommand_Feedback response; @@ -525,57 +663,259 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte 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::max(); + 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 client_eof_requested{false}; + std::atomic request_stream_closed{false}; + std::atomic 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(frame_age_ns) / 1'000'000.0; + const double write_ms = static_cast(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()) { - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; + 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; } - 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)); + 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; } - - 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); - 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]); + const auto& frame = *read->value; + const auto descriptor = frame.descriptor; + if (!descriptor) { + continue; } - - response.set_seq_no(nFrameCount++); - - if (!stream->Write(response)) { - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; + const uint64_t now_ns = static_cast( + std::chrono::duration_cast( + 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; } - last_sent_index = next_index; + 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(descriptor->width)); + response.mutable_color_frame()->set_height(static_cast(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(std::min( + frame.sequence, + static_cast(std::numeric_limits::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::steady_clock::now() - write_started).count() / 1000.0; + break; + } + last_write_duration = std::chrono::duration_cast( + 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); + } } - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id; - dev->stopStreaming(); + 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 (exception &e) { + 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); diff --git a/protos/cmvr/api/camera_command.proto b/protos/cmvr/api/camera_command.proto index 03a8f359..7305955a 100644 --- a/protos/cmvr/api/camera_command.proto +++ b/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 { } - diff --git a/protos/cmvr/api/camera_service.proto b/protos/cmvr/api/camera_service.proto index cb9b2acd..9ab7d7fa 100644 --- a/protos/cmvr/api/camera_service.proto +++ b/protos/cmvr/api/camera_service.proto @@ -14,8 +14,9 @@ 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) {} rpc GetRGBDImagesStream(stream GetRGBDImagesStreamCommand.Request) returns (stream GetRGBDImagesStreamCommand.Feedback) {} -} \ No newline at end of file +} diff --git a/protos/cmvr/config/camera_config/camera_config.proto b/protos/cmvr/config/camera_config/camera_config.proto index ce225f41..49f50754 100644 --- a/protos/cmvr/config/camera_config/camera_config.proto +++ b/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; } } diff --git a/protos/cmvr/config/device_manager_config/device_manager_config.proto b/protos/cmvr/config/device_manager_config/device_manager_config.proto index 751e6f9a..5e635331 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/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; diff --git a/protos/cmvr/config/joint_limits_config.proto b/protos/cmvr/config/joint_limits_config.proto index 268c05af..c3da751b 100644 --- a/protos/cmvr/config/joint_limits_config.proto +++ b/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 {