diff --git a/CMakeLists.txt b/CMakeLists.txt index 92bcb5b6..2d5a7b21 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,38 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") include(FindExternalLib) set(ARCH "x86") setup_external_libs(${ARCH}) + +# Intel oneVPL / VA-API runtime. The shared libraries in lib/ are installed +# by setup_external_libs(); the VA-API driver plugin directory is installed +# separately because it must retain its dri layout. +set(INTEL_MEDIA_STACK_ROOT + "${PROJECT_SOURCE_DIR}/dependency/${ARCH}/third_party/intel-media-stack/vpl-2.17" +) +set(INTEL_MEDIA_DRIVER_DIR "${INTEL_MEDIA_STACK_ROOT}/lib/dri") +set(INTEL_IHD_DRIVER "${INTEL_MEDIA_DRIVER_DIR}/iHD_drv_video.so") + +if(NOT EXISTS "${INTEL_IHD_DRIVER}") + message(FATAL_ERROR "Intel iHD VA-API driver not found: ${INTEL_IHD_DRIVER}") +endif() + +message(STATUS "Intel media stack: ${INTEL_MEDIA_STACK_ROOT}") +install( + DIRECTORY "${INTEL_MEDIA_DRIVER_DIR}/" + DESTINATION lib/dri +) +install(CODE [=[ + find_program(CMVR_PATCHELF_EXECUTABLE patchelf REQUIRED) + set(_cmvr_ihd_driver + "${CMAKE_INSTALL_PREFIX}/lib/dri/iHD_drv_video.so" + ) + execute_process( + COMMAND "${CMVR_PATCHELF_EXECUTABLE}" + --set-rpath "$ORIGIN/.." + "${_cmvr_ihd_driver}" + COMMAND_ERROR_IS_FATAL ANY + ) +]=]) + # 在调用 setup_external_libs 之后 message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}") message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}") diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 28a7d288..eea33acb 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -7,6 +7,9 @@ #pragma once +#include +#include + #include "devices/abstract_device.h" #include "common/base/logging/logger.h" #include "motor/motor_protocol_interface.h" @@ -212,6 +215,11 @@ namespace cmvr::device{ return protocol_->getQd(node_id_); } + // Returns the actual motor current in mA when supported by the driver. + virtual std::int16_t getCurrent() { + return 0; + } + // 使用的通讯协议 virtual void setProtocol(std::shared_ptr protocol) { diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h index 02150507..21727952 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h @@ -121,6 +121,12 @@ public: return true; } + bool readSdoString(int motor_id, + std::uint16_t index, + std::uint8_t subindex, + std::string& value, + std::size_t max_size = 128); + private: struct PdoEntryRuntime { EthercatPdoEntryConfig cfg; diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp index d6148be4..d29e64b6 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp @@ -1025,6 +1025,68 @@ bool EthercatMotorBusRuntime::readSdoRaw_(const int motor_id, return true; } +bool EthercatMotorBusRuntime::readSdoString(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + std::string& value, + const std::size_t max_size) +{ + value.clear(); + if (max_size == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid SDO string buffer size: " + << max_size << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", motor_id=" << motor_id; + return false; + } + + const auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing motor for SDO string read: " + << motor_id << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + if (!master_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized for SDO string read: " + << id_ << ", motor_id=" << motor_id + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + + std::vector data(max_size); + std::size_t result_size = 0; + std::uint32_t abort_code = 0; + const auto& slave = slave_it->second; + const int result = ecrt_master_sdo_upload( + master_, + static_cast(slave.cfg.position()), + index, + subindex, + data.data(), + data.size(), + &result_size, + &abort_code); + if (result != 0 || result_size > data.size()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to read SDO string: " + << "group=" << id_ << ", motor_id=" << motor_id + << ", slave_position=" << slave.cfg.position() + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", max_size=" << max_size + << ", result=" << result + << ", result_size=" << result_size + << ", abort_code=" << hexIndex_(abort_code); + return false; + } + + const auto string_end = std::find(data.begin(), data.begin() + result_size, 0); + value.assign(reinterpret_cast(data.data()), + static_cast(string_end - data.begin())); + return true; +} + std::uint32_t EthercatMotorBusRuntime::pdoEntryKey_(const std::uint16_t index, const std::uint8_t subindex) { diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h index 0993dd57..782f119f 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h @@ -62,6 +62,7 @@ inline EthercatPdoMapping createEyouCia402PdoMapping() entry(msgs::CIA402_ACTUAL_POSITION_6064, 0x00, 32, "Actual Position"), entry(msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, 32, "Actual Velocity"), entry(msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, 16, "Actual Torque"), + entry(msgs::CIA402_ACTUAL_CURRENT_6078, 0x00, 16, "Actual Current"), entry(msgs::CIA402_MODE_DISPLAY_6061, 0x00, 8, "Mode Of Operation Display"), entry(msgs::CIA402_ERROR_CODE_603F, 0x00, 16, "Error Code"), entry(0x0000, 0x00, 8, "Padding"), diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h index 4bdc2154..067c97ba 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h @@ -24,6 +24,7 @@ public: void setLimitQd(double qd) override; bool calibrateZeroQ() override; bool brakeRelease() override; + std::int16_t getCurrent() override; static bool commandCyclicPositionsAtomic( const std::vector>& motors, diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h index c177a313..cf85e802 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h @@ -3,6 +3,7 @@ #include #include +#include #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h" @@ -24,6 +25,11 @@ public: std::int64_t counts_per_joint_revolution, std::int32_t& zeroed_position) override; bool brakeRelease(std::uint8_t node_id) override; + bool readActualCurrent(std::uint8_t node_id, + std::int16_t& current_value) const; + bool readMotorIdentity(std::uint8_t node_id, + std::string& motor_model, + std::string& motor_version) const; private: std::shared_ptr bus_runtime_; diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h index cee1369d..bcaaae8b 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h @@ -5,6 +5,9 @@ namespace cmvr::device::eyou { +inline constexpr std::uint16_t EYOU_DEVICE_NAME_1008 = 0x1008; +inline constexpr std::uint16_t EYOU_SOFTWARE_VERSION_100A = 0x100A; + inline constexpr std::uint16_t EYOU_SOFT_LIMIT_STATE_2003 = 0x2003; inline constexpr std::uint16_t EYOU_BRAKE_CONTROL_2014 = 0x2014; diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp index 1e0eeb5f..d5e3b47f 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp @@ -153,8 +153,11 @@ void Cia402StatusMonitor::reportStatuswordTransition_( const StatusSample& previous, const StatusSample& current) const { - const bool status_changed = + const bool device_state_changed = !had_previous || !previous.read_ok || + (previous.statusword & 0x006F) != (current.statusword & 0x006F); + const bool status_changed = + device_state_changed || previous.status_problem != current.status_problem || (previous.statusword & 0x0888) != (current.statusword & 0x0888); if (current.status_problem && status_changed) { @@ -178,9 +181,20 @@ void Cia402StatusMonitor::reportStatuswordTransition_( previous.status_problem) { CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] recovered" << ", node=" << static_cast(node_id) + << ", current_statusword=0x" << std::hex << std::uppercase + << std::setw(4) << std::setfill('0') << current.statusword + << std::dec << std::nouppercase << std::setfill(' ') + << ' ' << deviceStateName_(current.statusword) << ", previous_statusword=0x" << std::hex << std::uppercase << std::setw(4) << std::setfill('0') << previous.statusword << std::dec << std::nouppercase << std::setfill(' '); + } else if (device_state_changed) { + CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] 0x" + << std::hex << std::uppercase << std::setw(4) + << std::setfill('0') << current.statusword + << std::dec << std::nouppercase << std::setfill(' ') + << ' ' << deviceStateName_(current.statusword) + << ", node=" << static_cast(node_id); } } diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp index 20fcbfa2..d294bdb0 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp @@ -6,12 +6,31 @@ #include #include #include +#include +#include +#include #include #include "common/base/logging/logger.h" namespace cmvr::device { +namespace { + +struct MotorCurrentInfo { + double adc_amperes_per_count; + double rated_current_amperes; +}; + +const std::map kMotorCurrentMap = { + {"EuPH11", {0.005371094, 3.8}}, + {"EuPH14", {0.007672991, 3.9}}, + {"EuPH17", {0.007672991, 3.9}}, + {"EuPH20", {0.013427734, 6.9}}, +}; + +} // namespace + EyouMotor::EyouMotor(const config::MotorConfigItem& config, std::shared_ptr cia402_protocol, std::unique_ptr vendor_adapter) @@ -46,6 +65,7 @@ bool EyouMotor::init() CMVR_LOG(ERROR) << "[EyouMotor] failed to init vendor adapter: " << info_.joint_name; return false; } + configureCurrentConversion_(); cia402_protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_); cia402_protocol_->setLimitQd(node_id_, info_.limit_qd); @@ -212,6 +232,29 @@ bool EyouMotor::brakeRelease() return vendor_adapter_->brakeRelease(node_id_); } +std::int16_t EyouMotor::getCurrent() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_()) { + return 0; + } + + std::int16_t raw_current = 0; + if (!vendor_adapter_->readActualCurrent(node_id_, raw_current)) { + CMVR_LOG(ERROR) << "[EyouMotor] failed to read actual current: " + << info_.joint_name; + return 0; + } + + const double current_ma = static_cast(raw_current) * + current_scale_ma_per_count_; + const double clamped_current_ma = std::clamp( + current_ma, + static_cast(std::numeric_limits::min()), + static_cast(std::numeric_limits::max())); + return static_cast(clamped_current_ma); +} + bool EyouMotor::hasDependencies_() const { if (!cia402_protocol_ || !vendor_adapter_) { @@ -260,6 +303,37 @@ bool EyouMotor::writeVendorVelocityLimit_() const return vendor_adapter_->writeVelocityLimit(node_id_, radPerSecToCounts_(info_.limit_qd)); } +void EyouMotor::configureCurrentConversion_() +{ + std::string motor_model; + std::string motor_version; + if (!vendor_adapter_->readMotorIdentity(node_id_, motor_model, motor_version)) { + CMVR_LOG(WARNING) << "[EyouMotor] failed to read motor identity; actual current " + << "will use the raw PDO value: " << info_.joint_name; + return; + } + + std::string model_prefix = motor_model; + const auto dash_pos = motor_model.find('-'); + if (dash_pos != std::string::npos) { + model_prefix = motor_model.substr(0, dash_pos); + } else if (motor_model.rfind("EuPH", 0) == 0 && motor_model.size() >= 6) { + model_prefix = motor_model.substr(0, 6); + } + + const auto current_info = kMotorCurrentMap.find(model_prefix); + if (current_info == kMotorCurrentMap.end()) { + CMVR_LOG(WARNING) << "[EyouMotor] unsupported motor model for current conversion: " + << motor_model << ", actual current will use the raw PDO value"; + return; + } + + const bool is_v145 = motor_version.find("V145") != std::string::npos; + current_scale_ma_per_count_ = is_v145 + ? current_info->second.rated_current_amperes + : current_info->second.adc_amperes_per_count * 1000.0; +} + std::int32_t EyouMotor::radToCounts_(const double angle_rad) const { const double rev = angle_rad / (2.0 * M_PI); diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp index 04e0fcfa..99848f49 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp @@ -373,4 +373,25 @@ bool EyouMotorAdapter::brakeRelease(const std::uint8_t node_id) return false; } +bool EyouMotorAdapter::readActualCurrent(const std::uint8_t node_id, + std::int16_t& current_value) const +{ + return bus_runtime_ && + bus_runtime_->readPdo( + node_id, msgs::CIA402_ACTUAL_CURRENT_6078, 0x00, current_value); +} + +bool EyouMotorAdapter::readMotorIdentity(const std::uint8_t node_id, + std::string& motor_model, + std::string& motor_version) const +{ + if (!bus_runtime_) { + return false; + } + return bus_runtime_->readSdoString( + node_id, eyou::EYOU_DEVICE_NAME_1008, 0x00, motor_model) && + bus_runtime_->readSdoString( + node_id, eyou::EYOU_SOFTWARE_VERSION_100A, 0x00, motor_version); +} + } // namespace cmvr::device diff --git a/cmvr-es/manager/media_source_hub/include/media_source_hub.h b/cmvr-es/manager/media_source_hub/include/media_source_hub.h index c7dcb553..39daa7bd 100644 --- a/cmvr-es/manager/media_source_hub/include/media_source_hub.h +++ b/cmvr-es/manager/media_source_hub/include/media_source_hub.h @@ -123,6 +123,10 @@ private: std::shared_ptr impl_; }; +// Compatibility alias for older protocol tests and integrations. New code should +// use MediaSourceHub directly. +using MediaSourceManager = MediaSourceHub; + } // namespace cmvr::media #endif // CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H diff --git a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp b/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp index 36b21653..de6f1338 100644 --- a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp +++ b/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp @@ -32,10 +32,12 @@ std::string normalizedCodec(std::string codec) { Codec videoCodec(const std::string& value) { const std::string codec = normalizedCodec(value); - if (codec == "h264" || codec == "avc" || codec == "avc1" || codec == "libx264") { + if (codec == "h264" || codec == "avc" || codec == "avc1" || + codec == "libx264" || codec == "h264qsv") { return Codec::H264; } - if (codec == "h265" || codec == "hevc" || codec == "hvc1" || codec == "libx265") { + if (codec == "h265" || codec == "hevc" || codec == "hvc1" || + codec == "libx265" || codec == "h265qsv" || codec == "hevcqsv") { return Codec::H265; } return Codec::UNKNOWN; diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index 5ca371ef..43cec91a 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -203,7 +203,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); @@ -256,6 +258,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); @@ -312,6 +317,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); @@ -324,7 +335,8 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - // color image + // color image is serialized in the existing OpenCV BGR byte order; + // clients convert it once when constructing an RGB image. if (color_image.type() == CV_8UC3) { response->mutable_color_frame()->set_type(api::FrameData::U8C3); } @@ -353,6 +365,9 @@ 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); @@ -504,11 +519,12 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con !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); response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size()); response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey); - response.mutable_depth_frame()->set_codec(frame_data.codec); - response.mutable_depth_frame()->set_width(frame_data.width); - response.mutable_depth_frame()->set_height(frame_data.height); + response.mutable_depth_frame()->set_codec("none"); + response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width); + response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height); response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); @@ -590,11 +606,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con response.mutable_color_frame()->set_width(frame_data.width); response.mutable_color_frame()->set_height(frame_data.height); + response.mutable_depth_frame()->set_type(api::FrameData::U16C1); response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size()); response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey); - response.mutable_depth_frame()->set_codec(frame_data.codec); - response.mutable_depth_frame()->set_width(frame_data.width); - response.mutable_depth_frame()->set_height(frame_data.height); + response.mutable_depth_frame()->set_codec("none"); + response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width); + response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height); response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); @@ -667,7 +684,40 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte 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; @@ -713,15 +763,25 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte }; 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; } 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) { @@ -765,6 +825,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte "Unsupported camera stream codec or payload format: " + dev_id); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); stream->Write(response); + exit_reason = "unsupported_stream"; break; } if (read->dropped_since_last_read > 0 || read->generation_changed || frame.discontinuity) { @@ -808,7 +869,17 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte const auto write_started = std::chrono::steady_clock::now(); if (!stream->Write(response)) { - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; + 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(