Merge xtkuang_dev into linbo_dev
This commit is contained in:
commit
ae4b54d9f7
12
.gitignore
vendored
12
.gitignore
vendored
@ -7,3 +7,15 @@
|
||||
/third_party/osqp/
|
||||
/third_party/OsqpEigen/
|
||||
/output
|
||||
|
||||
# Generated development artifacts
|
||||
compile_commands.json
|
||||
MUJOCO_LOG.TXT
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*.log
|
||||
|
||||
# Generated plots and vision debug output
|
||||
/data/*.png
|
||||
/data/*.svg
|
||||
/python/vision_servo/red_point.png
|
||||
|
||||
@ -9,6 +9,10 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
#set(CMAKE_CXX_STANDARD_REQUIRED True)
|
||||
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
|
||||
|
||||
# Tests are opt-in so production builds keep the existing footprint.
|
||||
option(BUILD_TESTING "Build the test targets" OFF)
|
||||
include(CTest)
|
||||
|
||||
|
||||
|
||||
# Install to <source>/output
|
||||
|
||||
@ -1,6 +0,0 @@
|
||||
Fri Jul 24 15:39:05 2026
|
||||
ERROR: could not create window
|
||||
|
||||
Fri Jul 24 15:40:37 2026
|
||||
ERROR: could not create window
|
||||
|
||||
@ -6,11 +6,16 @@ add_subdirectory(hardware)
|
||||
add_subdirectory(algorithms)
|
||||
add_subdirectory(simulate)
|
||||
add_subdirectory(devices)
|
||||
add_subdirectory(manager/control_authority_manager)
|
||||
add_subdirectory(manager/safety_manager)
|
||||
add_subdirectory(manager/device_manager)
|
||||
add_subdirectory(manager/media_source_hub)
|
||||
add_subdirectory(service/grpc/stop_all)
|
||||
add_subdirectory(manager/media_source_manager)
|
||||
add_subdirectory(service/quic_edge)
|
||||
add_subdirectory(task)
|
||||
add_subdirectory(task/quic_edge_task)
|
||||
add_subdirectory(service/grpc/client)
|
||||
add_subdirectory(task/ume_teleop_task)
|
||||
add_subdirectory(manager/task_manager)
|
||||
add_subdirectory(service)
|
||||
add_subdirectory(runtime)
|
||||
|
||||
22
cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt
Normal file
22
cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt
Normal file
@ -0,0 +1,22 @@
|
||||
add_library(ume_legacy_controller SHARED
|
||||
src/ume_legacy_controller.cpp
|
||||
src/pinocchio_ume_legacy_model_adapter.cpp
|
||||
)
|
||||
|
||||
target_include_directories(ume_legacy_controller
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
)
|
||||
|
||||
target_link_libraries(ume_legacy_controller
|
||||
PRIVATE
|
||||
pinocchio_default
|
||||
pinocchio_parsers
|
||||
)
|
||||
|
||||
add_library(
|
||||
cmvr_es::algorithms::ume_legacy
|
||||
ALIAS ume_legacy_controller
|
||||
)
|
||||
|
||||
install(TARGETS ume_legacy_controller LIBRARY DESTINATION lib)
|
||||
@ -34,20 +34,3 @@ add_executable(support_functions_test
|
||||
)
|
||||
target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es)
|
||||
target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog)
|
||||
|
||||
#add_executable(image_display_test
|
||||
# utils/visualization/image_display_test.cpp
|
||||
#)
|
||||
#
|
||||
#target_link_libraries(image_display_test
|
||||
# PRIVATE
|
||||
# cmvr_es::common
|
||||
# cmvr_es::perception
|
||||
# cmvr_es::device::realsense_camera
|
||||
# cmvr_es::proto
|
||||
# glog
|
||||
# gtest
|
||||
# gtest_main
|
||||
# pthread
|
||||
#)
|
||||
#
|
||||
|
||||
@ -2,6 +2,7 @@
|
||||
#define CMVR_ES_AGV_TYPES_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
@ -90,6 +91,14 @@ enum class AgvTaskType {
|
||||
Custom
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 固定距离平移使用的距离参考模式。
|
||||
*/
|
||||
enum class AgvTranslationMode {
|
||||
Odometry = 0,
|
||||
Localization
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief AGV 车体坐标系下的平面速度。
|
||||
*
|
||||
@ -101,6 +110,20 @@ struct AgvVelocity {
|
||||
double wz{0.0};
|
||||
};
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief AGV 车体坐标系下的固定距离平移参数。
|
||||
*/
|
||||
struct AgvTranslation {
|
||||
double distance{0.0};
|
||||
double vx{0.0};
|
||||
double vy{0.0};
|
||||
AgvTranslationMode mode{AgvTranslationMode::Odometry};
|
||||
};
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 导航通用运动约束和执行选项。
|
||||
*
|
||||
@ -114,7 +137,14 @@ struct AgvMotionOptions {
|
||||
double reach_distance{0.0};
|
||||
double reach_angle{0.0};
|
||||
double speed_ratio{1.0};
|
||||
bool asynchronous{true};
|
||||
// 导航默认同步阻塞；调用方只有显式设为 true 才在任务接受后立即返回。
|
||||
bool asynchronous{false};
|
||||
int wait_timeout_ms{0};
|
||||
int poll_interval_ms{0};
|
||||
int blocked_timeout_ms{0};
|
||||
// 不带 RPC 框架依赖的取消检查。同步导航等待期间可由
|
||||
// 上层绑定 deadline/cancel；驱动不得在函数返回后保留该回调。
|
||||
std::function<bool()> cancellation_requested;
|
||||
};
|
||||
|
||||
/**
|
||||
@ -220,7 +250,7 @@ struct AgvPathSegment {
|
||||
/**
|
||||
* @brief AGV 扫图过程中产生的数据文件。
|
||||
*
|
||||
* content 可保存控制器返回的二进制内容，例如 SRC1100 的 rawmap zip 包。
|
||||
* content 可保存控制器返回的二进制内容，例如 SEER Robokit 的 rawmap zip 包。
|
||||
*/
|
||||
struct AgvMappingDataFile {
|
||||
std::string name;
|
||||
|
||||
@ -1,609 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
namespace cmvr::common {
|
||||
|
||||
/**
|
||||
* @brief 图像像素坐标。
|
||||
*
|
||||
* 坐标系约定：
|
||||
* - `u`：图像列坐标，向右为正，单位像素。
|
||||
* - `v`：图像行坐标，向下为正，单位像素。
|
||||
*/
|
||||
struct Pixel {
|
||||
int u{0}; // 图像像素列坐标，单位像素。
|
||||
int v{0}; // 图像像素行坐标，单位像素。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief BGR 颜色定义。
|
||||
*/
|
||||
struct Color {
|
||||
uint8_t b{0}; // 蓝色通道，范围 [0, 255]。
|
||||
uint8_t g{255}; // 绿色通道，范围 [0, 255]。
|
||||
uint8_t r{0}; // 红色通道，范围 [0, 255]。
|
||||
|
||||
/**
|
||||
* @brief 转为 OpenCV 的 `cv::Scalar`。
|
||||
*/
|
||||
cv::Scalar toCvScalar() const {
|
||||
return cv::Scalar(static_cast<double>(b),
|
||||
static_cast<double>(g),
|
||||
static_cast<double>(r));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 预定义红色。
|
||||
*/
|
||||
static Color red() { return Color{0, 0, 255}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义绿色。
|
||||
*/
|
||||
static Color green() { return Color{0, 255, 0}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义蓝色。
|
||||
*/
|
||||
static Color blue() { return Color{255, 0, 0}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义黄色。
|
||||
*/
|
||||
static Color yellow() { return Color{0, 255, 255}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义白色。
|
||||
*/
|
||||
static Color white() { return Color{255, 255, 255}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义黑色。
|
||||
*/
|
||||
static Color black() { return Color{0, 0, 0}; }
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 图像上的文本叠加配置。
|
||||
*/
|
||||
struct TextOverlay {
|
||||
std::string text; // 需要显示的字符串内容。
|
||||
Pixel position_px{}; // 文本左下角在图像像素坐标系中的位置。
|
||||
Color color{Color::green()}; // 文本颜色。
|
||||
double font_scale{0.7}; // OpenCV 字体缩放系数。
|
||||
int thickness{2}; // 文本线宽，单位像素。
|
||||
int font_face{cv::FONT_HERSHEY_SIMPLEX}; // OpenCV 字体类型。
|
||||
bool draw_background{false}; // 是否绘制文本背景框。
|
||||
Color background_color{Color::black()}; // 文本背景框颜色。
|
||||
int background_padding_px{2}; // 文本背景框四周留白，单位像素。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 图像上的点叠加配置。
|
||||
*/
|
||||
struct PointOverlay {
|
||||
Pixel position_px{}; // 点中心在图像像素坐标系中的位置。
|
||||
Color color{Color::red()}; // 点颜色。
|
||||
int radius_px{5}; // 圆点半径，单位像素。
|
||||
int thickness{2}; // 线宽，`-1` 表示填充。
|
||||
std::string label; // 点旁边附带显示的字符串；为空时不显示。
|
||||
Pixel label_offset_px{8, -8}; // 标签相对点中心的像素偏移。
|
||||
double label_font_scale{0.6}; // 点标签字体缩放系数。
|
||||
int label_thickness{2}; // 点标签线宽，单位像素。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 图像上的圆圈叠加配置。
|
||||
*/
|
||||
struct CircleOverlay {
|
||||
Pixel center_px{}; // 圆心在图像像素坐标系中的位置。
|
||||
int radius_px{10}; // 圆半径，单位像素。
|
||||
Color color{Color::yellow()}; // 圆圈颜色。
|
||||
int thickness{2}; // 圆圈线宽，单位像素，`-1` 表示填充。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 图像上的十字叠加配置。
|
||||
*/
|
||||
struct CrossOverlay {
|
||||
Pixel center_px{}; // 十字中心在图像像素坐标系中的位置。
|
||||
int arm_length_px{8}; // 单侧十字臂长度，单位像素。
|
||||
Color color{Color::blue()}; // 十字颜色。
|
||||
int thickness{2}; // 十字线宽，单位像素。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 通用图像叠加显示接口。
|
||||
*
|
||||
* 该接口不依赖任何感知或跟踪类，只抽象三类输入：
|
||||
* - 待显示的图像数据；
|
||||
* - 文本叠加项；
|
||||
* - 点叠加项。
|
||||
*/
|
||||
class ImageDisplay {
|
||||
public:
|
||||
virtual ~ImageDisplay() = default;
|
||||
|
||||
/**
|
||||
* @brief 设置窗口名称。
|
||||
* @param window_name 显示窗口名称。
|
||||
*/
|
||||
virtual void setWindowName(const std::string& window_name) = 0;
|
||||
|
||||
/**
|
||||
* @brief 设置待显示的图像。
|
||||
* @param image 输入图像，支持灰度图、BGR 图或 BGRA 图。
|
||||
*/
|
||||
virtual void setImage(const cv::Mat& image) = 0;
|
||||
|
||||
/**
|
||||
* @brief 清空所有叠加项，但保留当前图像。
|
||||
*/
|
||||
virtual void clearOverlays() = 0;
|
||||
|
||||
/**
|
||||
* @brief 添加文本叠加项。
|
||||
* @param overlay 文本叠加配置。
|
||||
*/
|
||||
virtual void showText(const TextOverlay& overlay) = 0;
|
||||
|
||||
/**
|
||||
* @brief 按位置、字符串、颜色和大小直接添加文本。
|
||||
* @param text 需要显示的字符串内容。
|
||||
* @param position_px 文本左下角在图像像素坐标系中的位置。
|
||||
* @param color 文本颜色。
|
||||
* @param font_scale OpenCV 字体缩放系数。
|
||||
* @param thickness 文本线宽，单位像素。
|
||||
*/
|
||||
virtual void showText(const std::string& text,
|
||||
const Pixel& position_px,
|
||||
const Color& color,
|
||||
double font_scale = 0.7,
|
||||
int thickness = 2) = 0;
|
||||
|
||||
/**
|
||||
* @brief 添加点叠加项。
|
||||
* @param overlay 点叠加配置。
|
||||
*/
|
||||
virtual void showPoint(const PointOverlay& overlay) = 0;
|
||||
|
||||
/**
|
||||
* @brief 按位置、颜色和大小直接添加点。
|
||||
* @param position_px 点中心在图像像素坐标系中的位置。
|
||||
* @param color 点颜色。
|
||||
* @param radius_px 圆点半径，单位像素。
|
||||
* @param thickness 线宽，`-1` 表示填充。
|
||||
* @param label 点旁边附带显示的字符串；为空时不显示。
|
||||
*/
|
||||
virtual void showPoint(const Pixel& position_px,
|
||||
const Color& color,
|
||||
int radius_px = 5,
|
||||
int thickness = 2,
|
||||
const std::string& label = {}) = 0;
|
||||
|
||||
/**
|
||||
* @brief 显示圆圈叠加项。
|
||||
* @param overlay 圆圈叠加配置。
|
||||
*/
|
||||
virtual void showCircle(const CircleOverlay& overlay) = 0;
|
||||
|
||||
/**
|
||||
* @brief 按位置、半径、颜色和线宽直接显示圆圈。
|
||||
* @param center_px 圆心在图像像素坐标系中的位置。
|
||||
* @param radius_px 圆半径，单位像素。
|
||||
* @param color 圆圈颜色。
|
||||
* @param thickness 圆圈线宽，单位像素，`-1` 表示填充。
|
||||
*/
|
||||
virtual void showCircle(const Pixel& center_px,
|
||||
int radius_px,
|
||||
const Color& color,
|
||||
int thickness = 2) = 0;
|
||||
|
||||
/**
|
||||
* @brief 显示十字叠加项。
|
||||
* @param overlay 十字叠加配置。
|
||||
*/
|
||||
virtual void showCross(const CrossOverlay& overlay) = 0;
|
||||
|
||||
/**
|
||||
* @brief 按位置、尺寸、颜色和线宽直接显示十字。
|
||||
* @param center_px 十字中心在图像像素坐标系中的位置。
|
||||
* @param arm_length_px 单侧十字臂长度，单位像素。
|
||||
* @param color 十字颜色。
|
||||
* @param thickness 十字线宽，单位像素。
|
||||
*/
|
||||
virtual void showCross(const Pixel& center_px,
|
||||
int arm_length_px,
|
||||
const Color& color,
|
||||
int thickness = 2) = 0;
|
||||
|
||||
/**
|
||||
* @brief 判断当前是否已有待显示图像。
|
||||
*/
|
||||
virtual bool hasImage() const = 0;
|
||||
|
||||
/**
|
||||
* @brief 将当前图像和叠加项渲染到输出图像。
|
||||
* @param rendered_image 输出的渲染结果图像，BGR 三通道。
|
||||
* @return 渲染成功返回 `true`。
|
||||
*/
|
||||
virtual bool render(cv::Mat& rendered_image) const = 0;
|
||||
|
||||
/**
|
||||
* @brief 显示当前渲染结果。
|
||||
* @param wait_key_ms `cv::waitKey` 等待时间，单位毫秒。
|
||||
* @return 返回 `cv::waitKey` 的按键值；若没有可显示图像则返回 `-1`。
|
||||
*/
|
||||
virtual int show(int wait_key_ms = 1) = 0;
|
||||
|
||||
/**
|
||||
* @brief 关闭显示窗口。
|
||||
*/
|
||||
virtual void close() = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 基于 OpenCV HighGUI 的图像叠加显示实现。
|
||||
*/
|
||||
class OpenCvImageCvDisplay final : public ImageDisplay {
|
||||
public:
|
||||
/**
|
||||
* @brief 构造显示器。
|
||||
* @param window_name 显示窗口名称。
|
||||
*/
|
||||
explicit OpenCvImageCvDisplay(std::string window_name = "image_overlay")
|
||||
: window_name_(std::move(window_name)) {}
|
||||
|
||||
/**
|
||||
* @brief 设置窗口名称。
|
||||
* @param window_name 显示窗口名称。
|
||||
*/
|
||||
void setWindowName(const std::string& window_name) override {
|
||||
window_name_ = window_name;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置待显示图像。
|
||||
* @param image 输入图像，支持灰度图、BGR 图或 BGRA 图。
|
||||
*/
|
||||
void setImage(const cv::Mat& image) override {
|
||||
image_ = image.clone();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 清空所有叠加项，但保留当前图像。
|
||||
*/
|
||||
void clearOverlays() override {
|
||||
texts_.clear();
|
||||
points_.clear();
|
||||
circles_.clear();
|
||||
crosses_.clear();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 添加文本叠加项。
|
||||
* @param overlay 文本叠加配置。
|
||||
*/
|
||||
void showText(const TextOverlay& overlay) override {
|
||||
texts_.push_back(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 按位置、字符串、颜色和大小直接添加文本。
|
||||
* @param text 需要显示的字符串内容。
|
||||
* @param position_px 文本左下角在图像像素坐标系中的位置。
|
||||
* @param color 文本颜色。
|
||||
* @param font_scale OpenCV 字体缩放系数。
|
||||
* @param thickness 文本线宽，单位像素。
|
||||
*/
|
||||
void showText(const std::string& text,
|
||||
const Pixel& position_px,
|
||||
const Color& color,
|
||||
double font_scale = 0.7,
|
||||
int thickness = 2) override {
|
||||
TextOverlay overlay;
|
||||
overlay.text = text;
|
||||
overlay.position_px = position_px;
|
||||
overlay.color = color;
|
||||
overlay.font_scale = font_scale;
|
||||
overlay.thickness = thickness;
|
||||
showText(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 添加点叠加项。
|
||||
* @param overlay 点叠加配置。
|
||||
*/
|
||||
void showPoint(const PointOverlay& overlay) override {
|
||||
points_.push_back(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 按位置、颜色和大小直接添加点。
|
||||
* @param position_px 点中心在图像像素坐标系中的位置。
|
||||
* @param color 点颜色。
|
||||
* @param radius_px 圆点半径，单位像素。
|
||||
* @param thickness 线宽，`-1` 表示填充。
|
||||
* @param label 点旁边附带显示的字符串；为空时不显示。
|
||||
*/
|
||||
void showPoint(const Pixel& position_px,
|
||||
const Color& color,
|
||||
int radius_px = 5,
|
||||
int thickness = 2,
|
||||
const std::string& label = {}) override {
|
||||
PointOverlay overlay;
|
||||
overlay.position_px = position_px;
|
||||
overlay.color = color;
|
||||
overlay.radius_px = radius_px;
|
||||
overlay.thickness = thickness;
|
||||
overlay.label = label;
|
||||
showPoint(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 显示圆圈叠加项。
|
||||
* @param overlay 圆圈叠加配置。
|
||||
*/
|
||||
void showCircle(const CircleOverlay& overlay) override {
|
||||
circles_.push_back(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 按位置、半径、颜色和线宽直接显示圆圈。
|
||||
* @param center_px 圆心在图像像素坐标系中的位置。
|
||||
* @param radius_px 圆半径，单位像素。
|
||||
* @param color 圆圈颜色。
|
||||
* @param thickness 圆圈线宽，单位像素，`-1` 表示填充。
|
||||
*/
|
||||
void showCircle(const Pixel& center_px,
|
||||
int radius_px,
|
||||
const Color& color,
|
||||
int thickness = 2) override {
|
||||
CircleOverlay overlay;
|
||||
overlay.center_px = center_px;
|
||||
overlay.radius_px = radius_px;
|
||||
overlay.color = color;
|
||||
overlay.thickness = thickness;
|
||||
showCircle(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 显示十字叠加项。
|
||||
* @param overlay 十字叠加配置。
|
||||
*/
|
||||
void showCross(const CrossOverlay& overlay) override {
|
||||
crosses_.push_back(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 按位置、尺寸、颜色和线宽直接显示十字。
|
||||
* @param center_px 十字中心在图像像素坐标系中的位置。
|
||||
* @param arm_length_px 单侧十字臂长度，单位像素。
|
||||
* @param color 十字颜色。
|
||||
* @param thickness 十字线宽，单位像素。
|
||||
*/
|
||||
void showCross(const Pixel& center_px,
|
||||
int arm_length_px,
|
||||
const Color& color,
|
||||
int thickness = 2) override {
|
||||
CrossOverlay overlay;
|
||||
overlay.center_px = center_px;
|
||||
overlay.arm_length_px = arm_length_px;
|
||||
overlay.color = color;
|
||||
overlay.thickness = thickness;
|
||||
showCross(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 判断当前是否已有待显示图像。
|
||||
*/
|
||||
bool hasImage() const override {
|
||||
return !image_.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将当前图像和叠加项渲染到输出图像。
|
||||
* @param rendered_image 输出的渲染结果图像，BGR 三通道。
|
||||
* @return 渲染成功返回 `true`。
|
||||
*/
|
||||
bool render(cv::Mat& rendered_image) const override {
|
||||
if (!toBgrImage(image_, rendered_image)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (const CircleOverlay& overlay : circles_) {
|
||||
drawCircle(rendered_image, overlay);
|
||||
}
|
||||
for (const CrossOverlay& overlay : crosses_) {
|
||||
drawCross(rendered_image, overlay);
|
||||
}
|
||||
for (const PointOverlay& overlay : points_) {
|
||||
drawPoint(rendered_image, overlay);
|
||||
}
|
||||
for (const TextOverlay& overlay : texts_) {
|
||||
drawText(rendered_image, overlay);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 显示当前渲染结果。
|
||||
* @param wait_key_ms `cv::waitKey` 等待时间，单位毫秒。
|
||||
* @return 返回 `cv::waitKey` 的按键值；若没有可显示图像则返回 `-1`。
|
||||
*/
|
||||
int show(int wait_key_ms = 1) override {
|
||||
cv::Mat rendered_image;
|
||||
if (!render(rendered_image)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!window_created_) {
|
||||
cv::namedWindow(window_name_, cv::WINDOW_NORMAL);
|
||||
window_created_ = true;
|
||||
}
|
||||
cv::imshow(window_name_, rendered_image);
|
||||
return cv::waitKey(wait_key_ms);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 关闭显示窗口。
|
||||
*/
|
||||
void close() override {
|
||||
if (!window_created_) {
|
||||
return;
|
||||
}
|
||||
cv::destroyWindow(window_name_);
|
||||
window_created_ = false;
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 将输入图像转换为可叠加绘制的 BGR 三通道图像。
|
||||
* @param input 输入图像。
|
||||
* @param output 输出 BGR 图像。
|
||||
* @return 转换成功返回 `true`。
|
||||
*/
|
||||
static bool toBgrImage(const cv::Mat& input, cv::Mat& output) {
|
||||
if (input.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (input.channels() == 1) {
|
||||
cv::cvtColor(input, output, cv::COLOR_GRAY2BGR);
|
||||
return true;
|
||||
}
|
||||
if (input.channels() == 3) {
|
||||
output = input.clone();
|
||||
return true;
|
||||
}
|
||||
if (input.channels() == 4) {
|
||||
cv::cvtColor(input, output, cv::COLOR_BGRA2BGR);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在图像上绘制文本叠加项。
|
||||
* @param image 目标图像，BGR 三通道。
|
||||
* @param overlay 文本叠加配置。
|
||||
*/
|
||||
static void drawText(cv::Mat& image, const TextOverlay& overlay) {
|
||||
const cv::Point origin(overlay.position_px.u, overlay.position_px.v);
|
||||
const int thickness = std::max(1, overlay.thickness);
|
||||
const double font_scale = std::max(0.0, overlay.font_scale);
|
||||
|
||||
if (overlay.draw_background) {
|
||||
int baseline = 0;
|
||||
const cv::Size text_size = cv::getTextSize(overlay.text,
|
||||
overlay.font_face,
|
||||
font_scale,
|
||||
thickness,
|
||||
&baseline);
|
||||
const int pad = std::max(0, overlay.background_padding_px);
|
||||
const cv::Point top_left(origin.x - pad, origin.y - text_size.height - pad);
|
||||
const cv::Point bottom_right(origin.x + text_size.width + pad, origin.y + baseline + pad);
|
||||
cv::rectangle(image,
|
||||
top_left,
|
||||
bottom_right,
|
||||
overlay.background_color.toCvScalar(),
|
||||
cv::FILLED);
|
||||
}
|
||||
|
||||
cv::putText(image,
|
||||
overlay.text,
|
||||
origin,
|
||||
overlay.font_face,
|
||||
font_scale,
|
||||
overlay.color.toCvScalar(),
|
||||
thickness,
|
||||
cv::LINE_AA);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在图像上绘制点叠加项。
|
||||
* @param image 目标图像，BGR 三通道。
|
||||
* @param overlay 点叠加配置。
|
||||
*/
|
||||
static void drawPoint(cv::Mat& image, const PointOverlay& overlay) {
|
||||
const cv::Point center(overlay.position_px.u, overlay.position_px.v);
|
||||
const int radius = std::max(1, overlay.radius_px);
|
||||
cv::circle(image,
|
||||
center,
|
||||
radius,
|
||||
overlay.color.toCvScalar(),
|
||||
overlay.thickness,
|
||||
cv::LINE_AA);
|
||||
|
||||
if (!overlay.label.empty()) {
|
||||
TextOverlay label_overlay;
|
||||
label_overlay.text = overlay.label;
|
||||
label_overlay.position_px = Pixel{center.x + overlay.label_offset_px.u,
|
||||
center.y + overlay.label_offset_px.v};
|
||||
label_overlay.color = overlay.color;
|
||||
label_overlay.font_scale = overlay.label_font_scale;
|
||||
label_overlay.thickness = overlay.label_thickness;
|
||||
drawText(image, label_overlay);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在图像上绘制圆圈叠加项。
|
||||
* @param image 目标图像，BGR 三通道。
|
||||
* @param overlay 圆圈叠加配置。
|
||||
*/
|
||||
static void drawCircle(cv::Mat& image, const CircleOverlay& overlay) {
|
||||
const cv::Point center(overlay.center_px.u, overlay.center_px.v);
|
||||
const int radius = std::max(1, overlay.radius_px);
|
||||
const int thickness = (overlay.thickness == 0) ? 1 : overlay.thickness;
|
||||
cv::circle(image,
|
||||
center,
|
||||
radius,
|
||||
overlay.color.toCvScalar(),
|
||||
thickness,
|
||||
cv::LINE_AA);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在图像上绘制十字叠加项。
|
||||
* @param image 目标图像，BGR 三通道。
|
||||
* @param overlay 十字叠加配置。
|
||||
*/
|
||||
static void drawCross(cv::Mat& image, const CrossOverlay& overlay) {
|
||||
const cv::Point center(overlay.center_px.u, overlay.center_px.v);
|
||||
const int arm_length = std::max(1, overlay.arm_length_px);
|
||||
const int thickness = std::max(1, overlay.thickness);
|
||||
const cv::Scalar color = overlay.color.toCvScalar();
|
||||
cv::line(image,
|
||||
cv::Point(center.x - arm_length, center.y),
|
||||
cv::Point(center.x + arm_length, center.y),
|
||||
color,
|
||||
thickness,
|
||||
cv::LINE_AA);
|
||||
cv::line(image,
|
||||
cv::Point(center.x, center.y - arm_length),
|
||||
cv::Point(center.x, center.y + arm_length),
|
||||
color,
|
||||
thickness,
|
||||
cv::LINE_AA);
|
||||
}
|
||||
|
||||
std::string window_name_; // 显示窗口名称。
|
||||
cv::Mat image_; // 当前待显示图像。
|
||||
std::vector<TextOverlay> texts_; // 当前待绘制的文本叠加列表。
|
||||
std::vector<PointOverlay> points_; // 当前待绘制的点叠加列表。
|
||||
std::vector<CircleOverlay> circles_; // 当前待绘制的圆圈叠加列表。
|
||||
std::vector<CrossOverlay> crosses_; // 当前待绘制的十字叠加列表。
|
||||
bool window_created_{false}; // 显示窗口是否已经创建。
|
||||
};
|
||||
|
||||
} // namespace cmvr::common
|
||||
File diff suppressed because it is too large
Load Diff
@ -36,13 +36,13 @@ config/cmvr_es.pb.txt
|
||||
| 大类 | 抽象接口 | 类别工厂 | 当前可选后端 |
|
||||
| --- | --- | --- | --- |
|
||||
| Camera | [`camera/abstract_camera.h`](camera/abstract_camera.h) | [`camera/camera_factory.h`](camera/camera_factory.h) | UVC、RealSense、Hikvision |
|
||||
| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SRC1100 |
|
||||
| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、AUBO、Huayan |
|
||||
| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SEER Robokit |
|
||||
| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、[AUBO](arm/aubo_arm/README.md)、Huayan、UME |
|
||||
| DexHand | [`dexhand/abstract_dexhand.h`](dexhand/abstract_dexhand.h) | [`dexhand/dexhand_factory.h`](dexhand/dexhand_factory.h) | RH56DFTP、PX6AXGen3 |
|
||||
| Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg |
|
||||
| Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg |
|
||||
| BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot |
|
||||
| MotorSystem | `motor/motor_system/` | DeviceFactory 直接创建 | CAN/MuJoCo motor group |
|
||||
| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT |
|
||||
|
||||
代码目录存在不等于已经接入配置创建链：
|
||||
|
||||
@ -222,7 +222,7 @@ CameraDeviceConfig / AGVDeviceConfig / ... 的外层 id
|
||||
|
||||
## 摄像头与麦克风实时流
|
||||
|
||||
设备实现抽象流接口后，由 [`../manager/media_source_hub/`](../manager/media_source_hub/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。
|
||||
设备实现抽象流接口后，由 [`../manager/media_source_manager/`](../manager/media_source_manager/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。
|
||||
|
||||
当前 Hub 轨道：
|
||||
|
||||
@ -312,7 +312,7 @@ adapter 检测到描述变化后创建新 descriptor，设备后端不要自行
|
||||
- 满队列覆盖旧数据是实时媒体的预期行为；
|
||||
- `waitEncodedFrame()` 必须有有限 timeout，不能永久阻塞。
|
||||
|
||||
MediaSourceHub Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。
|
||||
MediaSourceManager Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。
|
||||
|
||||
发布后的 `MediaFrame`、`TrackDescriptor` 和 payload 不可再修改。
|
||||
|
||||
@ -334,19 +334,17 @@ MediaSourceHub Subscription 同样是单消费者对象，不同协议或客户
|
||||
无硬件参考测试：
|
||||
|
||||
- [`camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp`](camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp)
|
||||
- [`../manager/media_source_hub/tests/media_source_hub_test.cpp`](../manager/media_source_hub/tests/media_source_hub_test.cpp)
|
||||
|
||||
```bash
|
||||
cmake -S . -B build \
|
||||
-DCMVR_ARCH=x86 \
|
||||
-DBUILD_TESTING=ON \
|
||||
-DCMVR_MEDIA_SOURCE_HUB_BUILD_TESTS=ON
|
||||
-DBUILD_TESTING=ON
|
||||
|
||||
cmake --build build -j"$(nproc)"
|
||||
|
||||
ctest \
|
||||
--test-dir build \
|
||||
-R 'hikvision_camera_callback_test|media_source_hub_test' \
|
||||
-R '^hikvision_camera_callback_test$' \
|
||||
--output-on-failure
|
||||
```
|
||||
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
add_subdirectory(my_agv)
|
||||
add_subdirectory(src1100)
|
||||
add_subdirectory(seer_robokit)
|
||||
|
||||
add_library(agv INTERFACE)
|
||||
|
||||
@ -8,7 +8,7 @@ target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(agv
|
||||
INTERFACE
|
||||
cmvr_es::device::my_agv
|
||||
cmvr_es::device::src1100_agv
|
||||
cmvr_es::device::seer_robokit_agv
|
||||
cmvr_es::proto
|
||||
)
|
||||
|
||||
|
||||
@ -15,6 +15,12 @@
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
enum class AgvActionKind {
|
||||
NavigateToPose,
|
||||
NavigateToStation,
|
||||
FollowPath,
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief AGV/移动底盘设备抽象基类。
|
||||
*
|
||||
@ -29,9 +35,19 @@ public:
|
||||
DeviceKind kind() const noexcept override { return DeviceKind::AGV; }
|
||||
|
||||
/**
|
||||
* @brief 获取 AGV 运行状态快照。
|
||||
* @brief Whether this backend provides terminal-state and stopped-motion
|
||||
* confirmation plus bounded cancellation suitable for synchronous
|
||||
* Action execution.
|
||||
*/
|
||||
virtual AgvRuntimeState runtimeState() const { return {}; }
|
||||
virtual bool supportsSynchronousAction(AgvActionKind) const noexcept
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取 AGV 运行状态快照。
|
||||
*/
|
||||
virtual AgvRuntimeState runtimeState() const { return {}; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前导航任务状态。
|
||||
@ -54,6 +70,13 @@ public:
|
||||
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "clearFault not implemented");
|
||||
}
|
||||
|
||||
|
||||
virtual AgvResult relocalize(const math::Pose2d& pose)
|
||||
{
|
||||
(void)pose;
|
||||
return AgvResult::failure(AgvErrorCode::UnsupportedCommand,"relocalize not implemented");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发起到世界/地图位姿的导航任务。
|
||||
*/
|
||||
@ -85,12 +108,41 @@ public:
|
||||
/**
|
||||
* @brief 发起显式站点到站点路径导航任务。
|
||||
*/
|
||||
virtual AgvResult followPath(const std::vector<AgvPathSegment>& path)
|
||||
virtual AgvResult followPath(
|
||||
const std::vector<AgvPathSegment>& path)
|
||||
{
|
||||
(void)path;
|
||||
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented");
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 按指定速度执行固定距离平移。
|
||||
*
|
||||
* 返回成功表示控制器已经接受命令，不表示运动已经完成。
|
||||
*/
|
||||
virtual AgvResult translate(const AgvTranslation& translation)
|
||||
{
|
||||
(void)translation;
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::UnsupportedCommand,
|
||||
"translate not implemented");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发起显式站点到站点路径导航任务，并指定同步/异步选项。
|
||||
*
|
||||
* 保留单参数虚函数以兼容已有派生类；旧实现会由本重载转发。
|
||||
*/
|
||||
virtual AgvResult followPath(
|
||||
const std::vector<AgvPathSegment>& path,
|
||||
const AgvMotionOptions& options)
|
||||
{
|
||||
(void)options;
|
||||
return followPath(path);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 暂停当前导航任务，如果设备支持。
|
||||
*/
|
||||
@ -138,6 +190,19 @@ public:
|
||||
return setVelocity(AgvVelocity{});
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 确认 AGV 已进入可安全释放控制权的停止状态。
|
||||
*
|
||||
* 该接口只在导航任务已终止且底盘速度经过连续采样确认为零后返回
|
||||
* 成功；仅收到取消、停止或零速度命令的应答不构成成功。
|
||||
*/
|
||||
virtual AgvResult confirmMotionStopped()
|
||||
{
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::UnsupportedCommand,
|
||||
"confirmMotionStopped not implemented");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 查询 AGV 可用地图名称列表。
|
||||
*/
|
||||
|
||||
29
cmvr-es/devices/agv/seer_robokit/CMakeLists.txt
Normal file
29
cmvr-es/devices/agv/seer_robokit/CMakeLists.txt
Normal file
@ -0,0 +1,29 @@
|
||||
add_library(seer_robokit_agv SHARED
|
||||
src/seer_robokit_agv.cpp
|
||||
src/seer_robokit_transport.cpp
|
||||
src/seer_robokit_control.cpp
|
||||
src/seer_robokit_status.cpp
|
||||
src/seer_robokit_navigation.cpp
|
||||
src/seer_robokit_navigation_wait.cpp
|
||||
src/seer_robokit_map.cpp
|
||||
include/seer_robokit_agv.h
|
||||
include/seer_robokit_protocol.h
|
||||
include/seer_robokit_utils.h
|
||||
include/seer_robokit_navigation_utils.h
|
||||
include/seer_robokit_pgv_utils.h
|
||||
)
|
||||
|
||||
target_include_directories(seer_robokit_agv
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
${PROJECT_SOURCE_DIR}/cmvr-es
|
||||
)
|
||||
|
||||
target_link_libraries(seer_robokit_agv
|
||||
PUBLIC
|
||||
cmvr_es::proto
|
||||
jsoncpp
|
||||
)
|
||||
|
||||
add_library(cmvr_es::device::seer_robokit_agv ALIAS seer_robokit_agv)
|
||||
install(TARGETS seer_robokit_agv LIBRARY DESTINATION lib)
|
||||
375
cmvr-es/devices/agv/seer_robokit/README.md
Normal file
375
cmvr-es/devices/agv/seer_robokit/README.md
Normal file
@ -0,0 +1,375 @@
|
||||
# 仙工 SEER Robokit AGV 适配器
|
||||
|
||||
`SeerRobokitAgv` 将仙工 SEER Robokit TCP/IP API 适配为 CMVR 的通用
|
||||
`AbstractAGV`/`cmvr.api.AgvService`。厂商命令号、端口、抢占控制权、状态轮询、
|
||||
地图格式转换和错误码解析都封装在本目录内。
|
||||
|
||||
本项目现场使用的控制器型号仍是 SRC1100，所以设备实例 ID 保持为
|
||||
`src1100`；它只用于配置关联和 gRPC 路由，不再作为驱动实现名称。后端配置字段
|
||||
使用 `seer_robokit_agv`，目录、类、库和测试统一使用 `seer_robokit` /
|
||||
`SeerRobokitAgv` 命名。
|
||||
|
||||
从旧版本升级时，外部部署配置必须同步使用 `seer_robokit_agv { ... }`，并把
|
||||
配置路径更新为 `devices/agv/seer_robokit.pb.txt`；设备实例 ID 保持不变。程序、
|
||||
外部配置和部署脚本需要原子升级，不能把旧字段或旧路径与新二进制混用。
|
||||
|
||||
返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。
|
||||
|
||||
## 代码与配置
|
||||
|
||||
所有驱动头文件统一放在 `include/`，实现文件统一放在 `src/`；测试源码独立放在
|
||||
`tests/`。除 `seer_robokit_agv.h` 外，其余头文件均为驱动内部实现细节。
|
||||
|
||||
- 公共类声明：[`include/seer_robokit_agv.h`](include/seer_robokit_agv.h)
|
||||
- 导航轮询工具：
|
||||
[`include/seer_robokit_navigation_utils.h`](include/seer_robokit_navigation_utils.h)
|
||||
- PGV 参数转换：
|
||||
[`include/seer_robokit_pgv_utils.h`](include/seer_robokit_pgv_utils.h)
|
||||
- 协议常量：[`include/seer_robokit_protocol.h`](include/seer_robokit_protocol.h)
|
||||
- 通用解析工具：[`include/seer_robokit_utils.h`](include/seer_robokit_utils.h)
|
||||
- 生命周期和连接：[`src/seer_robokit_agv.cpp`](src/seer_robokit_agv.cpp)
|
||||
- TCP 帧与收发：[`src/seer_robokit_transport.cpp`](src/seer_robokit_transport.cpp)
|
||||
- 控制权与受控命令：[`src/seer_robokit_control.cpp`](src/seer_robokit_control.cpp)
|
||||
- 状态与推送缓存：[`src/seer_robokit_status.cpp`](src/seer_robokit_status.cpp)
|
||||
- 导航命令：[`src/seer_robokit_navigation.cpp`](src/seer_robokit_navigation.cpp)
|
||||
- 阻塞等待与停车确认：
|
||||
[`src/seer_robokit_navigation_wait.cpp`](src/seer_robokit_navigation_wait.cpp)
|
||||
- 地图和建图：[`src/seer_robokit_map.cpp`](src/seer_robokit_map.cpp)
|
||||
- 设备配置：
|
||||
[`../../../config/devices/agv/seer_robokit.pb.txt`](../../../config/devices/agv/seer_robokit.pb.txt)
|
||||
- DeviceManager 配置：
|
||||
[`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt)
|
||||
- gRPC API：
|
||||
[`../../../../protos/cmvr/api/agv_service.proto`](../../../../protos/cmvr/api/agv_service.proto)、
|
||||
[`../../../../protos/cmvr/api/agv_command.proto`](../../../../protos/cmvr/api/agv_command.proto)、
|
||||
[`../../../../protos/cmvr/api/agv_utils.proto`](../../../../protos/cmvr/api/agv_utils.proto)
|
||||
|
||||
## 配置和启动
|
||||
|
||||
现场配置至少需要修改控制器 IP；端口通常保持仙工默认值：
|
||||
|
||||
```textproto
|
||||
agv {
|
||||
agvs {
|
||||
id: "src1100"
|
||||
seer_robokit_agv {
|
||||
ip: "192.168.192.5"
|
||||
port_status: 19204
|
||||
port_control: 19205
|
||||
port_nav: 19206
|
||||
port_config: 19207
|
||||
port_other: 19210
|
||||
port_push: 19301
|
||||
recv_timeout_ms: 1000
|
||||
control_nick_name: "cmvr-es"
|
||||
enable_state_push: true
|
||||
state_push_interval_ms: 200
|
||||
enable_map_update: true
|
||||
map_update_interval_ms: 1000
|
||||
map_update_history_size: 8
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
还要在 `device_manager.pb.txt` 中确认同一个设备 id，并在完成现场安全检查后把
|
||||
`enable` 改为 `true`。源码默认配置故意保持关闭。
|
||||
|
||||
```textproto
|
||||
devices {
|
||||
id: "src1100"
|
||||
type: DEVICE_TYPE_AGV
|
||||
config_file: "devices/agv/seer_robokit.pb.txt"
|
||||
enable: true
|
||||
}
|
||||
```
|
||||
|
||||
构建、安装并启动：
|
||||
|
||||
```bash
|
||||
cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
|
||||
cmake --build build -j2
|
||||
cmake --install build
|
||||
./output/bin/cmvr_es
|
||||
```
|
||||
|
||||
`output/bin/cmvr_es` 默认读取 `output/bin/config/`。修改源码配置后需要重新安装，
|
||||
或通过程序支持的外部配置入口启动，不能只修改源码文件后继续使用旧的
|
||||
`output/` 配置。
|
||||
|
||||
## 控制器端口和命令
|
||||
|
||||
| 端口 | 主要用途 | 当前使用的命令 |
|
||||
| --- | --- | --- |
|
||||
| `19204` | 状态、站点、地图和建图文件 | `1004`、`1007`、`1020`、`1101`、`1110`、`1300`、`1301`、`1780`、`1800` |
|
||||
| `19205` | 底盘控制 | `2000`、`2010`、`2022` |
|
||||
| `19206` | 导航任务 | `3001`、`3002`、`3003`、`3051`、`3066`、`3067` |
|
||||
| `19207` | 控制权、清错、地图上传下载 | `4005`、`4009`、`4010`、`4011` |
|
||||
| `19210` | 开始/停止建图 | `6100`、`6101` |
|
||||
| `19301` | 机器人状态推送 | `9300`/`19300` 配置，`19301` 推送 |
|
||||
|
||||
所有会改变机器人或控制器状态的调用都在 SEER Robokit 子类内部先通过 `4005`
|
||||
抢权，负载为稳定的 `nick_name`，成功后才发送实际命令。普通命令集中走
|
||||
`sendControlledCommand_`；`emergencyStop` 为保证 `2000` 和导航取消之间不被
|
||||
插入其他命令，会在同一个控制序列锁内只抢一次权。只读查询不抢权。不要在
|
||||
gRPC 客户端另做一套租约逻辑。
|
||||
|
||||
## gRPC 接口概览
|
||||
|
||||
默认示例端点为 `127.0.0.1:50052`；远程部署时替换为 CMVR 服务所在主机，
|
||||
不是 SEER Robokit 原生 TCP 端口。
|
||||
|
||||
| gRPC 方法 | SEER Robokit 行为 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `getRuntimeState` | 推送缓存，缺失时查询 `1004/1007/1300` | 只读 |
|
||||
| `getNavigationStatus` | 跟踪任务查询 `1110`，无精确上下文时回退 `1020` | 只读；同步等待另用 `1101` 确认停车 |
|
||||
| `emergencyStop` | `2000`，再执行 `3003` 或 `3067` | 软件停止，不替代硬件急停 |
|
||||
| `clearFault` | `4009` | 抢权后发送无请求体命令，清除可恢复故障 |
|
||||
| `navigateToPose` | `3051` + `freeGo` | 地图绝对位姿，仅双轮差速底盘 |
|
||||
| `navigateToStation` | `3051` | 站点路径导航；PGV 二次定位也使用此方法 |
|
||||
| `followPath` | `3066` | 仙工“指定路径导航”，与 `3051` 不同 |
|
||||
| `pauseNavigation` / `resumeNavigation` | `3001` / `3002` | 导航控制 |
|
||||
| `cancelNavigation` | `3003`，路径队列使用 `3067` | 取消当前跟踪任务 |
|
||||
| `setVelocity` / `stopVelocityControl` | `2010` | 车体速度；停止时发送全零速度 |
|
||||
| `listMaps` / `listStations` | `1300` / `1301` | 只读 |
|
||||
| `switchMap` | `2022` | 会改变定位所用地图 |
|
||||
| `uploadMap` / `downloadMap` | `4010` / `4011` | 上传会抢权，下载只读 |
|
||||
| `startMapping` / `stopMapping` | `6100` / `6101` | 建图控制 |
|
||||
| `streamMap` | `1780/1800` 加内部解析和缓存 | 对外发送统一 2D/3D 地图，不暴露 `.smap` 原始格式 |
|
||||
|
||||
查询运行状态：
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/getRuntimeState
|
||||
```
|
||||
|
||||
查询导航状态：
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/getNavigationStatus
|
||||
```
|
||||
|
||||
列出地图和当前地图站点：
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/listMaps
|
||||
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/listStations
|
||||
```
|
||||
|
||||
## 导航的同步语义
|
||||
|
||||
`navigateToPose`、`navigateToStation` 和 `followPath` 默认同步阻塞。控制器接受
|
||||
命令后，适配器继续轮询精确任务状态，并结合 `1101` 状态确认底盘已经停车；
|
||||
到达、失败、取消、遇障停止或超时后才返回。`waitTimeoutMs` 为 `0` 时使用
|
||||
适配器默认值，当前为 10 分钟；`pollIntervalMs` 为 `0` 时当前使用 200 ms。
|
||||
|
||||
连续观察到障碍阻挡且底盘已经停止后，适配器会主动取消该导航；清理结果不明确
|
||||
时还可能发送软件停止。任务不会在障碍消失后由本次调用自动恢复。等待超时、
|
||||
RPC cancel 和 deadline 到期也会进入安全取消及停车确认，因此函数返回时间可能
|
||||
晚于最初发现障碍或取消请求的时刻。
|
||||
|
||||
调用方的 gRPC deadline 必须大于预计行程时间和 `waitTimeoutMs`。RPC 被取消或
|
||||
deadline 到期时，适配器会进入安全取消/停车确认流程。显式设置
|
||||
`"asynchronous":true` 后不会等待任务终态：站点导航和指定路径导航在控制器
|
||||
接受后返回；自由导航仍会做最长约 1.5 秒的启动确认。异步成功不代表已经到点。
|
||||
|
||||
`AgvMotionOptions` 中，SEER Robokit 的 `3051` 导航当前支持：
|
||||
|
||||
| gRPC 字段 | 控制器字段 | 单位 |
|
||||
| --- | --- | --- |
|
||||
| `maxSpeed` | `max_speed` | m/s |
|
||||
| `maxAngularSpeed` | `max_wspeed` | rad/s |
|
||||
| `maxAcceleration` | `max_acc` | m/s² |
|
||||
| `maxAngularAcceleration` | `max_wacc` | rad/s² |
|
||||
| `reachDistance` | `reach_dist` | m |
|
||||
| `reachAngle` | `reach_angle` | rad |
|
||||
|
||||
`asynchronous`、`waitTimeoutMs` 和 `pollIntervalMs` 由适配器本地执行。
|
||||
`speedRatio` 当前没有对应的 SEER Robokit 序列化字段。`followPath` 的运动选项当前只
|
||||
控制同步/异步等待、超时和轮询；在没有确认 `3066` 的速度字段前，不会猜测性地
|
||||
写入每个路径段。
|
||||
|
||||
## 固定路径导航的 PGV 二次定位
|
||||
|
||||
仙工文档 [“路径导航 / 2. 固定路径导航 PGV 二次定位调整”](https://seer-group.feishu.cn/wiki/Q26SwaNoGisuLWk2vCxcPfVWn2e)
|
||||
说明 PGV 参数是 `3051 / robot_task_gotarget_req` 的顶层可选字段。因此在 CMVR
|
||||
中应调用 `navigateToStation`，不是 `followPath`。后者对应另一条
|
||||
`3066 / 指定路径导航` 协议，现有仙工资料和仓库历史都没有证明 `3066` 支持
|
||||
PGV 字段。
|
||||
|
||||
PGV 参数通过 `adapterParams.values` 传入。protobuf map 的值是字符串，
|
||||
SEER Robokit 适配器会在任何状态查询、抢权和运动命令之前完成校验，再转换为控制器
|
||||
要求的 JSON `bool`/`number`：
|
||||
|
||||
| `adapterParams.values` 键 | 输出 JSON 类型 | 含义 |
|
||||
| --- | --- | --- |
|
||||
| `use_pgv` | `bool` | 使用上视 PGV |
|
||||
| `use_down_pgv` | `bool` | 使用下视 PGV |
|
||||
| `pgv_adjust_dist` | `number` | 最大调整半径，必须为有限非负数；用于仙工第 3/4 种调整方式 |
|
||||
| `pgv_adjust_cx` | `number` | 调整范围圆心在二维码坐标系下的 X 偏移；用于第 4 种方式 |
|
||||
| `pgv_adjust_cy` | `number` | 调整范围圆心在二维码坐标系下的 Y 偏移；用于第 4 种方式 |
|
||||
| `pgv_x_adjust` | `number` | 仅调整小车 X 方向误差；用于第 2 种方式 |
|
||||
|
||||
所有数字都必须是完整、有限的数字字符串；偏移量允许正负。适配器不臆造
|
||||
调整半径上限，也不假定上视和下视一定互斥，这些约束应由实际 PGV 安装、标定和
|
||||
当前控制器版本确定。显式的 `"false"` 和 `"0"` 仍会作为原生布尔值和数值
|
||||
发给控制器；没有给出的字段不会发送。第 2/3/4 种方式由控制器和站点配置决定，
|
||||
本接口只传递与所选方式匹配的调整参数。
|
||||
|
||||
一旦请求中出现任意 PGV 键，适配器只允许同时出现 `source_id`、`task_id` 和
|
||||
上述 PGV 字段；`operation`、`jack_height`、脚本名或未知扩展字段都会在状态
|
||||
查询和抢权前被拒绝，避免一次 PGV 导航意外夹带顶升、货叉、IO 或脚本动作。
|
||||
没有 PGV 键的既有站点导航扩展语义保持不变。
|
||||
|
||||
上视 PGV 示例。该命令会让机器人导航到 `AP1`，只能在确认地图、站点、PGV
|
||||
标定、行驶区域和急停人员后执行：
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{
|
||||
"header":{"deviceId":"src1100"},
|
||||
"stationId":"AP1",
|
||||
"options":{
|
||||
"maxSpeed":0.15,
|
||||
"maxAcceleration":0.15,
|
||||
"asynchronous":false,
|
||||
"waitTimeoutMs":300000,
|
||||
"pollIntervalMs":200
|
||||
},
|
||||
"adapterParams":{"values":{
|
||||
"use_pgv":"true",
|
||||
"pgv_adjust_dist":"0.3",
|
||||
"pgv_adjust_cx":"-0.3",
|
||||
"pgv_adjust_cy":"0"
|
||||
}}
|
||||
}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/navigateToStation
|
||||
```
|
||||
|
||||
下视 PGV 使用同一接口，把 `use_down_pgv` 设为字符串 `"true"`；其他调整
|
||||
字段是否需要传入取决于现场定位方案。如果控制器版本要求明确起点，可在同一个
|
||||
map 中增加 `"source_id":"实际起点站点"`；默认起点为 `SELF_POSITION`。
|
||||
|
||||
仙工在线文档当前有两处拼写不一致：
|
||||
|
||||
- 代码块出现了损坏字段 `pgv_adjustuse_pgv_dist`；适配器会拒绝它，正确字段是
|
||||
`pgv_adjust_dist`；
|
||||
- 表格写成 `pgv_ajdust_cy`，而示例和仓库旧版序列化代码使用
|
||||
`pgv_adjust_cy`。适配器兼容接收前者，但只向控制器输出规范字段
|
||||
`pgv_adjust_cy`；两个拼写同时出现会因歧义被拒绝。
|
||||
|
||||
C++ 调用同样复用通用扩展参数：
|
||||
|
||||
```cpp
|
||||
cmvr::device::AgvMotionOptions options;
|
||||
options.max_speed = 0.15;
|
||||
options.max_acceleration = 0.15;
|
||||
|
||||
cmvr::device::AgvAdapterParams adapter;
|
||||
adapter.values["use_pgv"] = "true";
|
||||
adapter.values["pgv_adjust_dist"] = "0.3";
|
||||
adapter.values["pgv_adjust_cx"] = "-0.3";
|
||||
adapter.values["pgv_adjust_cy"] = "0";
|
||||
|
||||
const auto result = agv.navigateToStation("AP1", options, adapter);
|
||||
```
|
||||
|
||||
## 其他导航和控制示例
|
||||
|
||||
自由导航使用地图绝对坐标，不是“相对当前位置移动多少米”。示例只展示请求
|
||||
结构，发送前必须读取当前位姿并确认目标在同一地图的安全区域：
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{
|
||||
"header":{"deviceId":"src1100"},
|
||||
"pose":{"x":1.0,"y":0.0,"theta":0.0},
|
||||
"options":{"maxSpeed":0.15,"maxAcceleration":0.15}
|
||||
}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/navigateToPose
|
||||
```
|
||||
|
||||
显式站点路径使用 `3066`：
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{
|
||||
"header":{"deviceId":"src1100"},
|
||||
"path":[
|
||||
{"sourceStation":"LM1","targetStation":"LM2"},
|
||||
{"sourceStation":"LM2","targetStation":"AP1"}
|
||||
]
|
||||
}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/followPath
|
||||
```
|
||||
|
||||
暂停、继续和取消的请求体直接是 `CommandHeader.Request`，没有外层 `header`：
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
|
||||
127.0.0.1:50052 cmvr.api.AgvService/pauseNavigation
|
||||
|
||||
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
|
||||
127.0.0.1:50052 cmvr.api.AgvService/resumeNavigation
|
||||
|
||||
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
|
||||
127.0.0.1:50052 cmvr.api.AgvService/cancelNavigation
|
||||
```
|
||||
|
||||
差速底盘的 `vy` 应保持 `0`。低层速度控制不等价于导航，并可能与已有任务
|
||||
冲突；只应在专门的速度控制测试流程中使用：
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"},"velocity":{"vx":0.05,"vy":0,"wz":0}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/setVelocity
|
||||
|
||||
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
|
||||
127.0.0.1:50052 cmvr.api.AgvService/stopVelocityControl
|
||||
```
|
||||
|
||||
## 错误返回
|
||||
|
||||
控制器响应中的非零 `ret_code` 和 `err_msg` 会保留在 `AgvResult.message`，并由
|
||||
gRPC 同时写入 transport status message 和反馈头的 `errorMessage`。非 OK RPC
|
||||
下，标准客户端通常不会交付响应体，因此跨客户端应以 transport status message
|
||||
为准，不要依赖反馈头仍然可见。例如：
|
||||
|
||||
```text
|
||||
SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose
|
||||
```
|
||||
|
||||
控制器仅返回“已接收”不等于导航完成；同步接口仍要等待精确任务终态和停车
|
||||
确认。若发送后连接中断且控制器是否执行已无法确定，错误会明确提示 outcome
|
||||
unknown，调用方不能自动重发运动命令，应先查询状态并取消或停止。
|
||||
|
||||
## 安全边界
|
||||
|
||||
- 仙工文档明确把 `3051` 定位为任务链或验证测试等单车场景接口；不要把它当作
|
||||
多车调度接口，否则可能出现路径/速度不连续等危险行为。
|
||||
- `emergencyStop` 是控制器软件停止，不是功能安全急停；真实系统必须保留可达的
|
||||
硬件急停、安全激光、碰撞条和独立安全链。
|
||||
- 首次 PGV 测试应在低速、空载、隔离区域进行，并先核对二维码坐标系、传感器
|
||||
上/下视方向、调整半径和中心偏移的标定值。
|
||||
- PGV 同步成功目前能证明精确 `3051` 任务进入终态，并连续确认两次零速度；
|
||||
仙工文档没有明确 `Completed` 是否一定覆盖 PGV 二次调整的全部阶段，仍需实机
|
||||
验证后才能据此联动机械臂。异步成功更不代表 PGV 调整完成。
|
||||
- 地图切换、地图上传和开始建图会改变控制器状态，也会先抢占控制权；不要和
|
||||
现场调度系统并行操作。
|
||||
367
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h
Normal file
367
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h
Normal file
@ -0,0 +1,367 @@
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_AGV_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_AGV_H
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <deque>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <json/json.h>
|
||||
|
||||
#include "cmvr/config/agv_config/agv_config.pb.h"
|
||||
#include "devices/agv/abstract_agv.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
class SeerRobokitAgvTestPeer;
|
||||
|
||||
class SeerRobokitAgv final : public AbstractAGV {
|
||||
public:
|
||||
explicit SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg);
|
||||
~SeerRobokitAgv() override;
|
||||
|
||||
std::string typeName() const override { return "SeerRobokitAgv"; }
|
||||
|
||||
bool supportsSynchronousAction(AgvActionKind kind) const noexcept override
|
||||
{
|
||||
switch (kind) {
|
||||
case AgvActionKind::NavigateToPose:
|
||||
case AgvActionKind::NavigateToStation:
|
||||
case AgvActionKind::FollowPath:
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool init() override;
|
||||
bool start() override;
|
||||
bool stop() override;
|
||||
bool update() override;
|
||||
|
||||
AgvRuntimeState runtimeState() const override;
|
||||
AgvNavigationStatus navigationStatus() const override;
|
||||
|
||||
AgvResult emergencyStop() override;
|
||||
AgvResult clearFault() override;
|
||||
AgvResult relocalize(const math::Pose2d& pose) override;
|
||||
|
||||
|
||||
AgvResult navigateToPose(
|
||||
const math::Pose2d& pose,
|
||||
const AgvMotionOptions& options = {},
|
||||
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
|
||||
AgvResult navigateToStation(
|
||||
const std::string& station_id,
|
||||
const AgvMotionOptions& options = {},
|
||||
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
|
||||
AgvResult followPath(
|
||||
const std::vector<AgvPathSegment>& path) override;
|
||||
AgvResult followPath(
|
||||
const std::vector<AgvPathSegment>& path,
|
||||
const AgvMotionOptions& options) override;
|
||||
AgvResult translate(const AgvTranslation& translation) override;
|
||||
AgvResult pauseNavigation() override;
|
||||
AgvResult resumeNavigation() override;
|
||||
AgvResult cancelNavigation() override;
|
||||
|
||||
AgvResult setVelocity(const AgvVelocity& velocity) override;
|
||||
AgvResult confirmMotionStopped() override;
|
||||
|
||||
AgvResult listMaps(std::vector<std::string>& maps) const override;
|
||||
AgvResult listStations(std::vector<AgvStation>& stations) const override;
|
||||
AgvResult switchMap(const std::string& map_name) override;
|
||||
AgvResult uploadMap(const std::string& map_name, const std::string& content) override;
|
||||
AgvResult downloadMap(const std::string& map_name, std::string& content) const override;
|
||||
AgvResult startMapping(const AgvMappingOptions& options = {}) override;
|
||||
AgvResult getMappingData(int start_index, AgvMappingData& data) const override;
|
||||
AgvResult getUnifiedMapUpdate(
|
||||
std::uint64_t after_sequence,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const override;
|
||||
AgvResult stopMapping() override;
|
||||
|
||||
|
||||
|
||||
private:
|
||||
friend class SeerRobokitAgvTestPeer;
|
||||
|
||||
struct Ports {
|
||||
int status{19204};
|
||||
int control{19205};
|
||||
int navigation{19206};
|
||||
int config{19207};
|
||||
int other{19210};
|
||||
int push{19301};
|
||||
};
|
||||
|
||||
struct PoseTaskStatus {
|
||||
bool found{false};
|
||||
int state{0};
|
||||
int type{0};
|
||||
bool type_present{false};
|
||||
|
||||
double progress{0.0};
|
||||
bool progress_present{false};
|
||||
|
||||
double remaining_distance{0.0};
|
||||
bool distance_present{false};
|
||||
|
||||
std::string closest_target;
|
||||
std::string source_name;
|
||||
std::string target_name;
|
||||
std::string detail;
|
||||
};
|
||||
struct NavigationSnapshot {
|
||||
int task_status{0};
|
||||
int task_type{0};
|
||||
bool task_status_present{false};
|
||||
bool task_type_present{false};
|
||||
bool blocked{false};
|
||||
bool blocked_present{false};
|
||||
int block_reason{-1};
|
||||
std::string block_reason_raw;
|
||||
bool velocity_present{false};
|
||||
double vx{0.0};
|
||||
double vy{0.0};
|
||||
double w{0.0};
|
||||
bool emergency{false};
|
||||
std::string target_id;
|
||||
std::string active_faults;
|
||||
bool only_recoverable_blocking_faults{false};
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
enum class CommandTransmissionState {
|
||||
NotSent,
|
||||
PossiblySent,
|
||||
};
|
||||
|
||||
struct TrackedNavigationContext {
|
||||
std::string token;
|
||||
std::vector<std::string> task_ids;
|
||||
AgvTaskType type{AgvTaskType::None};
|
||||
std::string target_id;
|
||||
std::vector<std::string> target_ids;
|
||||
std::uint64_t navigation_generation{0};
|
||||
std::chrono::steady_clock::time_point accepted_at{};
|
||||
bool synchronous_wait{false};
|
||||
};
|
||||
|
||||
struct PoseTaskContext {
|
||||
std::string task_id;
|
||||
math::Pose2d target{};
|
||||
double reach_distance{0.0};
|
||||
double reach_angle{0.0};
|
||||
std::uint64_t navigation_generation{0};
|
||||
std::uint64_t controller_fault_sequence_at_start{0};
|
||||
std::uint64_t control_attempt_sequence_at_start{0};
|
||||
std::uint64_t controller_fault_channel_epoch_at_start{0};
|
||||
};
|
||||
|
||||
AgvResult connect_();
|
||||
AgvResult disconnect_();
|
||||
AgvResult emergencyStopTrackedNavigation_(
|
||||
const TrackedNavigationContext* expected_navigation);
|
||||
AgvResult connectSocket_(int& sock, int port);
|
||||
AgvResult ensureOtherSocket_();
|
||||
void closeSocket_(int& sock) const;
|
||||
bool connected_() const;
|
||||
|
||||
AgvResult acquireControl_() const;
|
||||
AgvResult confirmPoseNavigationStarted_(
|
||||
const PoseTaskContext& context,
|
||||
bool accept_paused,
|
||||
const AgvMotionOptions& options) const;
|
||||
AgvResult waitForPoseNavigationTerminal_(
|
||||
const PoseTaskContext& pose_context,
|
||||
const TrackedNavigationContext& navigation_context,
|
||||
const AgvMotionOptions& options);
|
||||
AgvResult waitForTrackedNavigationTerminal_(
|
||||
const TrackedNavigationContext& context,
|
||||
const AgvMotionOptions& options);
|
||||
AgvResult queryNavigationSnapshot_(NavigationSnapshot& snapshot) const;
|
||||
AgvResult cancelTrackedNavigation_(
|
||||
const TrackedNavigationContext& context,
|
||||
std::uint64_t& accepted_generation);
|
||||
AgvResult waitForCanceledTaskToStop_(
|
||||
const TrackedNavigationContext& context,
|
||||
const AgvMotionOptions& options,
|
||||
const std::string& reason,
|
||||
bool require_global_stopped = false);
|
||||
AgvResult failAndCancelTrackedNavigation_(
|
||||
const TrackedNavigationContext& context,
|
||||
const AgvMotionOptions& options,
|
||||
AgvErrorCode error_code,
|
||||
const std::string& reason);
|
||||
AgvResult queryPoseTaskStatus_(
|
||||
const std::string& task_id,
|
||||
PoseTaskStatus& status) const;
|
||||
AgvResult queryTaskStatuses_(
|
||||
const std::vector<std::string>& task_ids,
|
||||
std::vector<PoseTaskStatus>& statuses) const;
|
||||
bool poseTargetReached_(
|
||||
const PoseTaskContext& context,
|
||||
std::string& detail) const;
|
||||
std::string cachedControllerFaultDetail_(
|
||||
std::uint64_t after_sequence = 0,
|
||||
int wait_ms = 0,
|
||||
std::uint64_t* associated_control_attempt = nullptr) const;
|
||||
int controllerFaultCaptureGraceMs_() const;
|
||||
int controllerFaultStateMaxAgeMs_() const;
|
||||
std::string freeNavigationFaultStateUnavailableDetail_() const;
|
||||
void rememberPoseTask_(const PoseTaskContext& context) const;
|
||||
void advancePoseTaskGeneration_(
|
||||
std::uint64_t navigation_generation,
|
||||
std::uint64_t control_attempt_sequence) const;
|
||||
void advancePoseTaskControlAttempt_(
|
||||
std::uint64_t control_attempt_sequence) const;
|
||||
void clearPoseTask_(std::uint64_t navigation_generation) const;
|
||||
void clearPoseTaskIfTaskId_(const std::string& task_id) const;
|
||||
bool currentPoseTask_(PoseTaskContext& context) const;
|
||||
void rememberTrackedNavigation_(
|
||||
const TrackedNavigationContext& context) const;
|
||||
void advanceTrackedNavigationGeneration_(
|
||||
std::uint64_t navigation_generation) const;
|
||||
void clearTrackedNavigation_(std::uint64_t navigation_generation) const;
|
||||
void clearTrackedNavigationIfToken_(const std::string& token) const;
|
||||
bool currentTrackedNavigation_(
|
||||
TrackedNavigationContext& context) const;
|
||||
AgvResult sendControlledCommand_(int sock,
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response,
|
||||
std::uint64_t* accepted_navigation_generation = nullptr,
|
||||
std::uint64_t* controller_fault_sequence_at_attempt = nullptr,
|
||||
std::uint64_t* control_attempt_sequence = nullptr,
|
||||
PoseTaskContext* pose_context_to_publish = nullptr,
|
||||
bool reject_if_active_controller_fault = false,
|
||||
TrackedNavigationContext* navigation_context_to_publish = nullptr,
|
||||
bool preserve_tracked_navigation = false,
|
||||
const std::string* expected_navigation_token = nullptr,
|
||||
const std::function<bool()>* cancellation_requested = nullptr,
|
||||
const TrackedNavigationContext* expected_active_navigation = nullptr) const;
|
||||
AgvResult sendCommand_(int sock,
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response,
|
||||
CommandTransmissionState* transmission_state = nullptr) const;
|
||||
AgvResult sendCommandRaw_(int sock,
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
std::string* response_payload,
|
||||
CommandTransmissionState* transmission_state = nullptr) const;
|
||||
AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
|
||||
AgvResult configurePush_();
|
||||
void startPushThread_();
|
||||
void stopPushThread_();
|
||||
void pushLoop_();
|
||||
void invalidateControllerFaultState_();
|
||||
AgvRuntimeState queryRuntimeState_() const;
|
||||
void updateCachedRuntimeState_(const Json::Value& payload);
|
||||
void startMapUpdateThread_();
|
||||
void stopMapUpdateThread_();
|
||||
void mapUpdateLoop_();
|
||||
AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const;
|
||||
AgvResult parseMapFileToUpdates_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
||||
AgvResult parseSeerRobokitMapArchive_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
||||
AgvResult parseSeerRobokitMap2D_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const;
|
||||
AgvResult parseSeerRobokitMap3D_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const;
|
||||
void cacheMapUpdates_(std::vector<AgvUnifiedMapUpdate> updates) const;
|
||||
bool findCachedMapUpdate_(
|
||||
std::uint64_t after_sequence,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const;
|
||||
bool mapUpdateMatches_(
|
||||
const AgvUnifiedMapUpdate& update,
|
||||
const AgvMapStreamOptions& options) const;
|
||||
|
||||
static std::vector<std::uint8_t> buildFrame_(std::uint16_t command, const std::string& payload);
|
||||
static std::string toJsonString_(const Json::Value& value);
|
||||
static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
|
||||
static std::string extractJson_(const std::string& raw);
|
||||
static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
|
||||
static void applyMotionOptions_(
|
||||
Json::Value& payload,
|
||||
const AgvMotionOptions& options,
|
||||
bool include_reach_options = true);
|
||||
static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
|
||||
static AgvResult resultFromResponse_(const Json::Value& response);
|
||||
|
||||
config::SeerRobokitAgvConfig config_;
|
||||
std::string ip_;
|
||||
std::string control_nick_name_;
|
||||
int recv_timeout_ms_{1000};
|
||||
Ports ports_;
|
||||
bool state_push_enabled_{false};
|
||||
bool map_update_enabled_{false};
|
||||
int map_update_interval_ms_{1000};
|
||||
std::size_t map_update_history_size_{8};
|
||||
|
||||
mutable std::mutex mutex_;
|
||||
mutable std::mutex status_io_mutex_;
|
||||
mutable std::mutex control_sequence_mutex_;
|
||||
mutable std::atomic<std::uint64_t> navigation_generation_{0};
|
||||
mutable std::atomic<std::uint64_t> pose_task_sequence_{0};
|
||||
mutable std::atomic<std::uint64_t> control_attempt_sequence_{0};
|
||||
mutable std::atomic<std::uint64_t> controller_fault_channel_epoch_{0};
|
||||
mutable std::mutex pose_task_mutex_;
|
||||
mutable PoseTaskContext pose_task_context_;
|
||||
mutable std::mutex tracked_navigation_mutex_;
|
||||
mutable TrackedNavigationContext tracked_navigation_context_;
|
||||
mutable int sock_status_{-1};
|
||||
mutable int sock_control_{-1};
|
||||
mutable int sock_navigation_{-1};
|
||||
mutable int sock_config_{-1};
|
||||
mutable int sock_other_{-1};
|
||||
mutable int sock_push_{-1};
|
||||
std::string last_error_;
|
||||
|
||||
std::atomic<bool> push_running_{false};
|
||||
std::thread push_thread_;
|
||||
mutable std::mutex runtime_state_mutex_;
|
||||
mutable std::condition_variable runtime_state_cv_;
|
||||
AgvRuntimeState cached_runtime_state_;
|
||||
bool cached_runtime_state_valid_{false};
|
||||
std::uint64_t controller_fault_sequence_{0};
|
||||
bool controller_fault_state_observed_{false};
|
||||
std::chrono::steady_clock::time_point controller_fault_state_observed_at_{};
|
||||
std::string active_controller_fault_detail_;
|
||||
double last_controller_fault_timestamp_{0.0};
|
||||
std::string last_controller_fault_detail_;
|
||||
std::uint64_t last_controller_fault_control_attempt_{0};
|
||||
|
||||
mutable std::atomic<bool> map_update_running_{false};
|
||||
mutable std::thread map_update_thread_;
|
||||
mutable std::mutex map_update_mutex_;
|
||||
mutable std::condition_variable map_update_cv_;
|
||||
mutable std::deque<AgvUnifiedMapUpdate> cached_map_updates_;
|
||||
mutable std::uint64_t map_sequence_{0};
|
||||
mutable int next_mapping_index_{0};
|
||||
mutable std::size_t last_map_content_hash_{0};
|
||||
mutable std::string map_session_id_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_AGV_H
|
||||
@ -0,0 +1,303 @@
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "devices/agv/abstract_agv.h"
|
||||
|
||||
namespace cmvr::device::seer_robokit::navigation {
|
||||
|
||||
constexpr auto kPoseNavigationStartTimeout = std::chrono::milliseconds(1500);
|
||||
constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50);
|
||||
constexpr int kPoseNavigationRequiredRunningSamples = 2;
|
||||
constexpr auto kDefaultNavigationWaitTimeout =
|
||||
std::chrono::milliseconds(600000);
|
||||
constexpr auto kDefaultNavigationBlockedTimeout =
|
||||
std::chrono::milliseconds(60000);
|
||||
constexpr auto kDefaultNavigationPollInterval =
|
||||
std::chrono::milliseconds(200);
|
||||
constexpr auto kMaximumNavigationPollInterval =
|
||||
std::chrono::milliseconds(5000);
|
||||
constexpr auto kNavigationCancellationCheckInterval =
|
||||
std::chrono::milliseconds(50);
|
||||
constexpr auto kNavigationCancelPollInterval =
|
||||
std::chrono::milliseconds(100);
|
||||
constexpr auto kNavigationCancelConfirmationTimeout =
|
||||
std::chrono::milliseconds(3000);
|
||||
constexpr int kRequiredCompletedStopSamples = 2;
|
||||
constexpr double kNavigationStopVelocityTolerance = 0.005;
|
||||
constexpr int kRobotBlockedFaultCode = 52200;
|
||||
constexpr int kMinimumControllerFaultCaptureGraceMs = 250;
|
||||
constexpr int kMaximumControllerFaultCaptureGraceMs = 5000;
|
||||
constexpr int kDefaultControllerFaultPushIntervalMs = 1000;
|
||||
constexpr int kControllerFaultPushJitterMs = 100;
|
||||
constexpr int kMinimumControllerFaultStateMaxAgeMs = 2000;
|
||||
constexpr int kControllerFaultStateMaxAgeIntervals = 5;
|
||||
constexpr double kDefaultPoseReachDistance = 0.05;
|
||||
constexpr double kDefaultPoseReachAngle = 0.10;
|
||||
constexpr double kTwoPi = 6.28318530717958647692;
|
||||
|
||||
static inline bool exactTaskStateIsActive(const int state)
|
||||
{
|
||||
return state >= 1 && state <= 3;
|
||||
}
|
||||
|
||||
static inline bool exactTaskStateIsKnownTerminal(const int state)
|
||||
{
|
||||
return state >= 4 && state <= 7;
|
||||
}
|
||||
|
||||
static inline bool globalTaskStateIsKnownTerminal(const int state)
|
||||
{
|
||||
return state == 0 || exactTaskStateIsKnownTerminal(state);
|
||||
}
|
||||
|
||||
// Some SRC firmware versions report station navigation as task type 3.
|
||||
// Exact task ids and target ids remain the primary ownership evidence.
|
||||
static inline bool exactTrackedNavigationTaskTypeMatches(
|
||||
const AgvTaskType expected_type,
|
||||
const int actual_type)
|
||||
{
|
||||
switch (expected_type) {
|
||||
case AgvTaskType::NavigateToPose:
|
||||
return actual_type == 1;
|
||||
case AgvTaskType::NavigateToStation:
|
||||
case AgvTaskType::FollowPath:
|
||||
return actual_type == 2 || actual_type == 3;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
static inline bool globalNavigationTaskTypeMatches(
|
||||
const AgvTaskType expected_type,
|
||||
const int actual_type)
|
||||
{
|
||||
switch (expected_type) {
|
||||
case AgvTaskType::NavigateToPose:
|
||||
return actual_type == 1;
|
||||
case AgvTaskType::NavigateToStation:
|
||||
return actual_type == 2 || actual_type == 3;
|
||||
case AgvTaskType::FollowPath:
|
||||
return actual_type == 3;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
static inline double angleDistance(const double lhs, const double rhs)
|
||||
{
|
||||
return std::abs(std::remainder(lhs - rhs, kTwoPi));
|
||||
}
|
||||
|
||||
static inline AgvResult withUnknownControllerOutcome(AgvResult result)
|
||||
{
|
||||
const auto code = result.ok() ? AgvErrorCode::CommandFailed : result.code;
|
||||
std::string detail = result.message.empty() ? "unknown transport or protocol error" : result.message;
|
||||
detail +=
|
||||
"; SEER Robokit controller outcome is unknown after the command attempt; "
|
||||
"the command may already have taken effect; do not issue another motion "
|
||||
"command automatically; query status and cancel or stop first";
|
||||
return AgvResult::failure(code, detail);
|
||||
}
|
||||
|
||||
static inline std::string makePoseTaskId(
|
||||
const std::string& device_id,
|
||||
const std::uint64_t task_sequence)
|
||||
{
|
||||
const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
|
||||
return prefix + "_pose_" + std::to_string(timestamp)
|
||||
+ "_" + std::to_string(task_sequence);
|
||||
}
|
||||
|
||||
static inline std::string makeNavigationTaskId(
|
||||
const std::string& device_id,
|
||||
const char* kind,
|
||||
const std::uint64_t task_sequence)
|
||||
{
|
||||
const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
|
||||
return prefix + "_" + kind + "_" + std::to_string(timestamp)
|
||||
+ "_" + std::to_string(task_sequence);
|
||||
}
|
||||
|
||||
static inline const char* blockReasonName(const int reason)
|
||||
{
|
||||
switch (reason) {
|
||||
case 0:
|
||||
return "ultrasonic";
|
||||
case 1:
|
||||
return "laser";
|
||||
case 2:
|
||||
return "fallingdown";
|
||||
case 3:
|
||||
return "collision";
|
||||
case 4:
|
||||
return "infrared";
|
||||
case 5:
|
||||
return "locked";
|
||||
default:
|
||||
return "unknown";
|
||||
}
|
||||
}
|
||||
|
||||
static inline std::string invalidMotionOption(const AgvMotionOptions& options)
|
||||
{
|
||||
const auto non_negative_error = [](const double value, const char* field) {
|
||||
if (!std::isfinite(value)) {
|
||||
return std::string(field) + " must be finite";
|
||||
}
|
||||
if (value < 0.0) {
|
||||
return std::string(field) + " must be non-negative";
|
||||
}
|
||||
return std::string{};
|
||||
};
|
||||
|
||||
if (auto error = non_negative_error(options.max_speed, "max_speed");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(
|
||||
options.max_angular_speed,
|
||||
"max_angular_speed");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(
|
||||
options.max_acceleration,
|
||||
"max_acceleration");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(
|
||||
options.max_angular_acceleration,
|
||||
"max_angular_acceleration");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(
|
||||
options.reach_distance,
|
||||
"reach_distance");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(options.reach_angle, "reach_angle");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(options.speed_ratio, "speed_ratio");
|
||||
!error.empty()) return error;
|
||||
if (options.wait_timeout_ms < 0) {
|
||||
return "wait_timeout_ms must be non-negative";
|
||||
}
|
||||
if (options.blocked_timeout_ms < 0) {
|
||||
return "blocked_timeout_ms must be non-negative";
|
||||
}
|
||||
if (options.poll_interval_ms < 0) {
|
||||
return "poll_interval_ms must be non-negative";
|
||||
}
|
||||
if (options.poll_interval_ms
|
||||
> kMaximumNavigationPollInterval.count()) {
|
||||
return "poll_interval_ms must not exceed "
|
||||
+ std::to_string(kMaximumNavigationPollInterval.count());
|
||||
}
|
||||
if (options.wait_timeout_ms > 0
|
||||
&& options.poll_interval_ms > options.wait_timeout_ms) {
|
||||
return "poll_interval_ms must not exceed wait_timeout_ms";
|
||||
}
|
||||
return {};
|
||||
}
|
||||
|
||||
static inline std::chrono::milliseconds navigationWaitTimeout(
|
||||
const AgvMotionOptions& options)
|
||||
{
|
||||
return options.wait_timeout_ms > 0
|
||||
? std::chrono::milliseconds(options.wait_timeout_ms)
|
||||
: kDefaultNavigationWaitTimeout;
|
||||
}
|
||||
|
||||
static inline std::chrono::milliseconds navigationBlockedTimeout(
|
||||
const AgvMotionOptions& options)
|
||||
{
|
||||
return options.blocked_timeout_ms > 0
|
||||
? std::chrono::milliseconds(options.blocked_timeout_ms)
|
||||
: kDefaultNavigationBlockedTimeout;
|
||||
}
|
||||
|
||||
static inline std::chrono::milliseconds navigationPollInterval(
|
||||
const AgvMotionOptions& options)
|
||||
{
|
||||
if (options.poll_interval_ms <= 0) {
|
||||
return kDefaultNavigationPollInterval;
|
||||
}
|
||||
return std::chrono::milliseconds(
|
||||
std::max(options.poll_interval_ms, 20));
|
||||
}
|
||||
|
||||
static inline bool navigationCancellationRequested(const AgvMotionOptions& options)
|
||||
{
|
||||
return options.cancellation_requested
|
||||
&& options.cancellation_requested();
|
||||
}
|
||||
|
||||
static inline void sleepForNavigationPoll(
|
||||
const std::chrono::milliseconds poll_interval,
|
||||
const std::chrono::steady_clock::time_point overall_deadline,
|
||||
const AgvMotionOptions& options)
|
||||
{
|
||||
const auto poll_deadline = std::min(
|
||||
overall_deadline,
|
||||
std::chrono::steady_clock::now() + poll_interval);
|
||||
while (std::chrono::steady_clock::now() < poll_deadline
|
||||
&& !navigationCancellationRequested(options)) {
|
||||
const auto remaining = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
poll_deadline - std::chrono::steady_clock::now());
|
||||
if (remaining <= std::chrono::milliseconds::zero()) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::min(
|
||||
kNavigationCancellationCheckInterval,
|
||||
remaining));
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Snapshot>
|
||||
static inline bool navigationStopped(const Snapshot& snapshot)
|
||||
{
|
||||
return snapshot.velocity_present
|
||||
&& std::abs(snapshot.vx) <= kNavigationStopVelocityTolerance
|
||||
&& std::abs(snapshot.vy) <= kNavigationStopVelocityTolerance
|
||||
&& std::abs(snapshot.w) <= kNavigationStopVelocityTolerance;
|
||||
}
|
||||
|
||||
static inline AgvResult reconciledNavigationResult(
|
||||
const AgvResult& command_result,
|
||||
AgvResult terminal_result)
|
||||
{
|
||||
if (command_result.ok()) {
|
||||
return terminal_result;
|
||||
}
|
||||
if (terminal_result.ok()) {
|
||||
terminal_result.message =
|
||||
"SEER Robokit navigation completed after an indeterminate command "
|
||||
"acknowledgment; initial_detail=" + command_result.message;
|
||||
return terminal_result;
|
||||
}
|
||||
terminal_result.message =
|
||||
"SEER Robokit navigation command acknowledgment was indeterminate: "
|
||||
+ command_result.message + "; status reconciliation: "
|
||||
+ terminal_result.message;
|
||||
return terminal_result;
|
||||
}
|
||||
|
||||
static inline bool parseFiniteDouble(const std::string& value, double& parsed)
|
||||
{
|
||||
std::size_t consumed = 0;
|
||||
try {
|
||||
parsed = std::stod(value, &consumed);
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
return consumed == value.size() && std::isfinite(parsed);
|
||||
}
|
||||
|
||||
} // namespace cmvr::device::seer_robokit::navigation
|
||||
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
|
||||
@ -0,0 +1,141 @@
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <json/json.h>
|
||||
|
||||
#include "devices/agv/abstract_agv.h"
|
||||
#include "seer_robokit_navigation_utils.h"
|
||||
#include "seer_robokit_utils.h"
|
||||
|
||||
namespace cmvr::device::seer_robokit::pgv {
|
||||
|
||||
constexpr char kUsePgv[] = "use_pgv";
|
||||
constexpr char kPgvAdjustDist[] = "pgv_adjust_dist";
|
||||
constexpr char kPgvAdjustCx[] = "pgv_adjust_cx";
|
||||
constexpr char kPgvAdjustCy[] = "pgv_adjust_cy";
|
||||
constexpr char kPgvXAdjust[] = "pgv_x_adjust";
|
||||
constexpr char kUseDownPgv[] = "use_down_pgv";
|
||||
|
||||
// These spellings currently appear in the vendor document, but conflict with
|
||||
// its own field table/example and the repository's older working serializer.
|
||||
constexpr char kMalformedAdjustDist[] = "pgv_adjustuse_pgv_dist";
|
||||
constexpr char kAdjustCyDocumentAlias[] = "pgv_ajdust_cy";
|
||||
|
||||
static inline bool isPgvAdjustmentKey(const std::string& key)
|
||||
{
|
||||
return key == kUsePgv
|
||||
|| key == kPgvAdjustDist
|
||||
|| key == kPgvAdjustCx
|
||||
|| key == kPgvAdjustCy
|
||||
|| key == kPgvXAdjust
|
||||
|| key == kUseDownPgv
|
||||
|| key == kMalformedAdjustDist
|
||||
|| key == kAdjustCyDocumentAlias;
|
||||
}
|
||||
|
||||
static inline bool hasPgvAdjustmentParams(const AgvAdapterParams& params)
|
||||
{
|
||||
for (const auto& [key, value] : params.values) {
|
||||
(void)value;
|
||||
if (isPgvAdjustmentKey(key)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse the string-valued generic adapter parameters into the native JSON
|
||||
* types required by SEER Robokit API 3051. Returns an error string without
|
||||
* modifying controller state; an empty string means success.
|
||||
*/
|
||||
static inline std::string applyPgvAdjustmentParams(
|
||||
Json::Value& payload,
|
||||
const AgvAdapterParams& params)
|
||||
{
|
||||
if (params.getString(kMalformedAdjustDist)) {
|
||||
return std::string(kMalformedAdjustDist)
|
||||
+ " is a vendor-document typo; use " + kPgvAdjustDist;
|
||||
}
|
||||
if (hasPgvAdjustmentParams(params)) {
|
||||
for (const auto& [key, value] : params.values) {
|
||||
(void)value;
|
||||
if (!isPgvAdjustmentKey(key)
|
||||
&& key != "source_id"
|
||||
&& key != "task_id") {
|
||||
return "PGV adjustment must not be combined with adapter "
|
||||
"field " + key;
|
||||
}
|
||||
}
|
||||
}
|
||||
const auto adjust_cy = params.getString(kPgvAdjustCy);
|
||||
const auto adjust_cy_alias = params.getString(kAdjustCyDocumentAlias);
|
||||
if (adjust_cy && adjust_cy_alias) {
|
||||
return std::string(kPgvAdjustCy) + " and its vendor-document alias "
|
||||
+ kAdjustCyDocumentAlias + " must not both be set";
|
||||
}
|
||||
|
||||
const auto apply_bool = [&payload, ¶ms](const char* key) {
|
||||
if (!params.getString(key)) {
|
||||
return std::string{};
|
||||
}
|
||||
const auto parsed = params.getBool(key);
|
||||
if (!parsed) {
|
||||
return std::string(key)
|
||||
+ " must be a boolean string such as true or false";
|
||||
}
|
||||
detail::jsonMember(payload, key) = *parsed;
|
||||
return std::string{};
|
||||
};
|
||||
if (auto error = apply_bool(kUsePgv); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
if (auto error = apply_bool(kUseDownPgv); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
|
||||
const auto apply_number = [&payload, ¶ms](
|
||||
const char* key,
|
||||
const bool non_negative) {
|
||||
const auto raw = params.getString(key);
|
||||
if (!raw) {
|
||||
return std::string{};
|
||||
}
|
||||
double parsed = 0.0;
|
||||
if (!navigation::parseFiniteDouble(*raw, parsed)) {
|
||||
return std::string(key) + " must be a complete finite number";
|
||||
}
|
||||
if (non_negative && parsed < 0.0) {
|
||||
return std::string(key) + " must be non-negative";
|
||||
}
|
||||
detail::jsonMember(payload, key) = parsed;
|
||||
return std::string{};
|
||||
};
|
||||
if (auto error = apply_number(kPgvAdjustDist, true); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
if (auto error = apply_number(kPgvAdjustCx, false); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
if (adjust_cy_alias) {
|
||||
double parsed = 0.0;
|
||||
if (!navigation::parseFiniteDouble(*adjust_cy_alias, parsed)) {
|
||||
return std::string(kAdjustCyDocumentAlias)
|
||||
+ " must be a complete finite number";
|
||||
}
|
||||
detail::jsonMember(payload, kPgvAdjustCy) = parsed;
|
||||
} else if (auto error = apply_number(kPgvAdjustCy, false);
|
||||
!error.empty()) {
|
||||
return error;
|
||||
}
|
||||
if (auto error = apply_number(kPgvXAdjust, false); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
return {};
|
||||
}
|
||||
|
||||
} // namespace cmvr::device::seer_robokit::pgv
|
||||
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
|
||||
@ -0,0 +1,41 @@
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace cmvr::device::seer_robokit::protocol {
|
||||
|
||||
constexpr std::uint16_t kRobotStatusLoc = 1004;
|
||||
constexpr std::uint16_t kRobotStatusBattery = 1007;
|
||||
constexpr std::uint16_t kRobotStatusAll2 = 1101;
|
||||
constexpr std::uint16_t kRobotStatusTask = 1020;
|
||||
constexpr std::uint16_t kRobotStatusTaskPackage = 1110;
|
||||
constexpr std::uint16_t kRobotStatusMap = 1300;
|
||||
constexpr std::uint16_t kRobotStatusStation = 1301;
|
||||
constexpr std::uint16_t kRobotStatusMappingFileList = 1780;
|
||||
constexpr std::uint16_t kRobotStatusDownloadFile = 1800;
|
||||
constexpr std::uint16_t kRobotControlStop = 2000;
|
||||
constexpr std::uint16_t kRobotControlReloc = 2002;
|
||||
constexpr std::uint16_t kRobotControlMotion = 2010;
|
||||
constexpr std::uint16_t kRobotControlLoadMap = 2022;
|
||||
constexpr std::uint16_t kRobotTaskPause = 3001;
|
||||
constexpr std::uint16_t kRobotTaskResume = 3002;
|
||||
constexpr std::uint16_t kRobotTaskCancel = 3003;
|
||||
constexpr std::uint16_t kRobotTaskGoTarget = 3051;
|
||||
constexpr std::uint16_t kRobotTaskTranslate = 3055;
|
||||
constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
|
||||
constexpr std::uint16_t kRobotTaskClearTargetList = 3067;
|
||||
constexpr std::uint16_t kRobotConfigLock = 4005;
|
||||
constexpr std::uint16_t kRobotConfigClearFault = 4009;
|
||||
constexpr std::uint16_t kRobotConfigUploadMap = 4010;
|
||||
constexpr std::uint16_t kRobotConfigDownloadMap = 4011;
|
||||
constexpr std::uint16_t kRobotOtherStartMapping = 6100;
|
||||
constexpr std::uint16_t kRobotOtherStopMapping = 6101;
|
||||
constexpr std::uint16_t kRobotPushConfigReq = 9300;
|
||||
constexpr std::uint16_t kRobotPushConfigRes = 19300;
|
||||
constexpr std::uint16_t kRobotPush = 19301;
|
||||
constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U;
|
||||
|
||||
} // namespace cmvr::device::seer_robokit::protocol
|
||||
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
|
||||
@ -0,0 +1,92 @@
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_UTILS_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_UTILS_H
|
||||
|
||||
#include <cerrno>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
|
||||
#include <json/json.h>
|
||||
|
||||
namespace cmvr::device::seer_robokit::detail {
|
||||
|
||||
static inline std::string systemError()
|
||||
{
|
||||
return std::strerror(errno);
|
||||
}
|
||||
|
||||
static inline Json::Value& jsonMember(
|
||||
Json::Value& value,
|
||||
const char* key)
|
||||
{
|
||||
return *value.demand(key, key + std::strlen(key));
|
||||
}
|
||||
|
||||
static inline Json::Value& jsonMember(
|
||||
Json::Value& value,
|
||||
const std::string& key)
|
||||
{
|
||||
return *value.demand(key.data(), key.data() + key.size());
|
||||
}
|
||||
|
||||
static inline const Json::Value* jsonFind(
|
||||
const Json::Value& value,
|
||||
const char* key)
|
||||
{
|
||||
return value.find(key, key + std::strlen(key));
|
||||
}
|
||||
|
||||
static inline Json::Value jsonGet(
|
||||
const Json::Value& value,
|
||||
const char* key,
|
||||
const Json::Value& fallback)
|
||||
{
|
||||
const auto* found = jsonFind(value, key);
|
||||
return found ? *found : fallback;
|
||||
}
|
||||
|
||||
static inline double nowSeconds()
|
||||
{
|
||||
const auto now = std::chrono::system_clock::now().time_since_epoch();
|
||||
return std::chrono::duration<double>(now).count();
|
||||
}
|
||||
|
||||
static inline bool jsonHas(
|
||||
const Json::Value& value,
|
||||
const char* key)
|
||||
{
|
||||
return jsonFind(value, key) != nullptr;
|
||||
}
|
||||
|
||||
static inline bool hasNumericControllerRetCode(
|
||||
const Json::Value& response)
|
||||
{
|
||||
const auto* ret_code = jsonFind(response, "ret_code");
|
||||
return ret_code
|
||||
&& (ret_code->isInt()
|
||||
|| ret_code->isUInt()
|
||||
|| ret_code->isInt64()
|
||||
|| ret_code->isUInt64());
|
||||
}
|
||||
|
||||
static inline std::string jsonValueToString(const Json::Value& value)
|
||||
{
|
||||
if (value.isString()) return value.asString();
|
||||
if (value.isBool()) return value.asBool() ? "true" : "false";
|
||||
if (value.isInt64() || value.isInt()) {
|
||||
return std::to_string(value.asInt64());
|
||||
}
|
||||
if (value.isUInt64() || value.isUInt()) {
|
||||
return std::to_string(value.asUInt64());
|
||||
}
|
||||
if (value.isDouble()) return std::to_string(value.asDouble());
|
||||
if (value.isNull()) return {};
|
||||
|
||||
Json::StreamWriterBuilder builder;
|
||||
builder["indentation"] = "";
|
||||
return Json::writeString(builder, value);
|
||||
}
|
||||
|
||||
} // namespace cmvr::device::seer_robokit::detail
|
||||
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_UTILS_H
|
||||
175
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp
Normal file
175
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp
Normal file
@ -0,0 +1,175 @@
|
||||
#include "seer_robokit_agv.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <cstddef>
|
||||
#include <mutex>
|
||||
|
||||
#include "common/base/logging/logger.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr int kDefaultMapUpdateIntervalMs = 1000;
|
||||
constexpr std::size_t kDefaultMapUpdateHistorySize = 8;
|
||||
|
||||
} // namespace
|
||||
|
||||
SeerRobokitAgv::SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg)
|
||||
: config_(cfg),
|
||||
ip_(cfg.ip()),
|
||||
control_nick_name_(
|
||||
cfg.control_nick_name().empty()
|
||||
? (cfg.id().empty() ? "cmvr-es" : "cmvr-es:" + cfg.id())
|
||||
: cfg.control_nick_name()),
|
||||
recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000),
|
||||
state_push_enabled_(cfg.enable_state_push()),
|
||||
map_update_enabled_(cfg.enable_map_update()),
|
||||
map_update_interval_ms_(cfg.map_update_interval_ms() > 0 ? cfg.map_update_interval_ms() : kDefaultMapUpdateIntervalMs),
|
||||
map_update_history_size_(cfg.map_update_history_size() > 0 ? cfg.map_update_history_size() : kDefaultMapUpdateHistorySize)
|
||||
{
|
||||
id_ = cfg.id();
|
||||
if (cfg.port_status() > 0) ports_.status = cfg.port_status();
|
||||
if (cfg.port_control() > 0) ports_.control = cfg.port_control();
|
||||
if (cfg.port_nav() > 0) ports_.navigation = cfg.port_nav();
|
||||
if (cfg.port_config() > 0) ports_.config = cfg.port_config();
|
||||
if (cfg.port_other() > 0) ports_.other = cfg.port_other();
|
||||
if (cfg.port_push() > 0) ports_.push = cfg.port_push();
|
||||
|
||||
const auto result = connect_();
|
||||
if (!result.ok()) {
|
||||
CMVR_LOG(ERROR) << "[SeerRobokitAgv] Auto connect failed"
|
||||
<< ", id=" << id_
|
||||
<< ", ip=" << ip_
|
||||
<< ", error=" << result.message;
|
||||
}
|
||||
}
|
||||
|
||||
SeerRobokitAgv::~SeerRobokitAgv()
|
||||
{
|
||||
(void)disconnect_();
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::init()
|
||||
{
|
||||
return !id_.empty() && !ip_.empty();
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::start()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::stop()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::update()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::connect_()
|
||||
{
|
||||
const auto lifecycle_generation =
|
||||
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
|
||||
clearPoseTask_(lifecycle_generation);
|
||||
clearTrackedNavigation_(lifecycle_generation);
|
||||
stopPushThread_();
|
||||
stopMapUpdateThread_();
|
||||
|
||||
{
|
||||
// Status requests may wait for a controller receive timeout without
|
||||
// holding mutex_. Serialize lifecycle changes with that channel before
|
||||
// replacing or closing its descriptor.
|
||||
std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
closeSocket_(sock_status_);
|
||||
closeSocket_(sock_control_);
|
||||
closeSocket_(sock_navigation_);
|
||||
closeSocket_(sock_config_);
|
||||
closeSocket_(sock_other_);
|
||||
closeSocket_(sock_push_);
|
||||
|
||||
if (ip_.empty()) {
|
||||
return AgvResult::failure(AgvErrorCode::InvalidArgument, "SEER Robokit AGV ip is empty");
|
||||
}
|
||||
|
||||
const auto close_all = [this]() {
|
||||
closeSocket_(sock_status_);
|
||||
closeSocket_(sock_control_);
|
||||
closeSocket_(sock_navigation_);
|
||||
closeSocket_(sock_config_);
|
||||
closeSocket_(sock_other_);
|
||||
closeSocket_(sock_push_);
|
||||
};
|
||||
|
||||
if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) {
|
||||
close_all();
|
||||
return result;
|
||||
}
|
||||
if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) {
|
||||
close_all();
|
||||
return result;
|
||||
}
|
||||
if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) {
|
||||
close_all();
|
||||
return result;
|
||||
}
|
||||
if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) {
|
||||
close_all();
|
||||
return result;
|
||||
}
|
||||
|
||||
if (state_push_enabled_) {
|
||||
const auto result = connectSocket_(sock_push_, ports_.push);
|
||||
if (!result.ok()) {
|
||||
CMVR_LOG(ERROR) << "[SeerRobokitAgv] Connect push port failed"
|
||||
<< ", id=" << id_
|
||||
<< ", port=" << ports_.push
|
||||
<< ", error=" << result.message;
|
||||
closeSocket_(sock_push_);
|
||||
}
|
||||
}
|
||||
last_error_.clear();
|
||||
}
|
||||
|
||||
if (state_push_enabled_ && sock_push_ >= 0) {
|
||||
const auto result = configurePush_();
|
||||
if (result.ok()) {
|
||||
startPushThread_();
|
||||
} else {
|
||||
CMVR_LOG(ERROR) << "[SeerRobokitAgv] Configure push failed"
|
||||
<< ", id=" << id_
|
||||
<< ", error=" << result.message;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
closeSocket_(sock_push_);
|
||||
}
|
||||
}
|
||||
if (map_update_enabled_) {
|
||||
startMapUpdateThread_();
|
||||
}
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::disconnect_()
|
||||
{
|
||||
const auto lifecycle_generation =
|
||||
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
|
||||
clearPoseTask_(lifecycle_generation);
|
||||
clearTrackedNavigation_(lifecycle_generation);
|
||||
stopMapUpdateThread_();
|
||||
stopPushThread_();
|
||||
std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
closeSocket_(sock_status_);
|
||||
closeSocket_(sock_control_);
|
||||
closeSocket_(sock_navigation_);
|
||||
closeSocket_(sock_config_);
|
||||
closeSocket_(sock_other_);
|
||||
closeSocket_(sock_push_);
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
375
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp
Normal file
375
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp
Normal file
@ -0,0 +1,375 @@
|
||||
#include "seer_robokit_agv.h"
|
||||
#include "seer_robokit_navigation_utils.h"
|
||||
#include "seer_robokit_protocol.h"
|
||||
#include "seer_robokit_utils.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
using namespace seer_robokit::navigation;
|
||||
using namespace seer_robokit::protocol;
|
||||
using namespace seer_robokit::detail;
|
||||
|
||||
AgvResult SeerRobokitAgv::acquireControl_() const
|
||||
{
|
||||
Json::Value payload(Json::objectValue);
|
||||
jsonMember(payload, "nick_name") = control_nick_name_;
|
||||
|
||||
Json::Value response;
|
||||
auto result = sendCommand_(sock_config_, kRobotConfigLock, payload, &response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::sendControlledCommand_(
|
||||
const int sock,
|
||||
const std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response,
|
||||
std::uint64_t* accepted_navigation_generation,
|
||||
std::uint64_t* controller_fault_sequence_at_attempt,
|
||||
std::uint64_t* control_attempt_sequence,
|
||||
PoseTaskContext* pose_context_to_publish,
|
||||
const bool reject_if_active_controller_fault,
|
||||
TrackedNavigationContext* navigation_context_to_publish,
|
||||
const bool preserve_tracked_navigation,
|
||||
const std::string* expected_navigation_token,
|
||||
const std::function<bool()>* cancellation_requested,
|
||||
const TrackedNavigationContext* expected_active_navigation) const
|
||||
{
|
||||
const auto canceled_before_send = [cancellation_requested]() {
|
||||
return cancellation_requested
|
||||
&& *cancellation_requested
|
||||
&& (*cancellation_requested)();
|
||||
};
|
||||
if (canceled_before_send()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit command was not sent because the caller canceled the "
|
||||
"operation before control authority was acquired");
|
||||
}
|
||||
|
||||
const auto expected_context_is_current =
|
||||
[this, expected_active_navigation]() {
|
||||
if (!expected_active_navigation) {
|
||||
return true;
|
||||
}
|
||||
TrackedNavigationContext active_context;
|
||||
return currentTrackedNavigation_(active_context)
|
||||
&& active_context.token
|
||||
== expected_active_navigation->token
|
||||
&& active_context.navigation_generation
|
||||
== expected_active_navigation->navigation_generation
|
||||
&& active_context.type
|
||||
== expected_active_navigation->type
|
||||
&& navigation_generation_.load(std::memory_order_relaxed)
|
||||
== expected_active_navigation->navigation_generation;
|
||||
};
|
||||
|
||||
// Conditional cancel ownership checks are deliberately performed without
|
||||
// the control sequencing mutex. A slow 1110/1101 response must never
|
||||
// prevent emergencyStop() from acquiring authority and sending 2000.
|
||||
// The exact local token/generation/type is revalidated under the control
|
||||
// lock both before and after authority acquisition below.
|
||||
if (expected_active_navigation) {
|
||||
if (!expected_context_is_current()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit did not start conditional navigation cancel "
|
||||
"preflight because the tracked task was already replaced or "
|
||||
"ended");
|
||||
}
|
||||
|
||||
std::vector<PoseTaskStatus> statuses;
|
||||
const auto exact_result = queryTaskStatuses_(
|
||||
expected_active_navigation->task_ids,
|
||||
statuses);
|
||||
if (!exact_result.ok()) {
|
||||
return AgvResult::failure(
|
||||
exact_result.code,
|
||||
"SEER Robokit did not send the conditional navigation cancel "
|
||||
"because exact task ownership preflight failed: "
|
||||
+ exact_result.message);
|
||||
}
|
||||
const bool all_exact_tasks_terminal = !statuses.empty()
|
||||
&& std::all_of(
|
||||
statuses.begin(),
|
||||
statuses.end(),
|
||||
[](const PoseTaskStatus& status) {
|
||||
return status.found
|
||||
&& exactTaskStateIsKnownTerminal(status.state);
|
||||
});
|
||||
if (all_exact_tasks_terminal) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
const bool exact_task_still_active = std::any_of(
|
||||
statuses.begin(),
|
||||
statuses.end(),
|
||||
[](const PoseTaskStatus& status) {
|
||||
return status.found
|
||||
&& exactTaskStateIsActive(status.state);
|
||||
});
|
||||
|
||||
NavigationSnapshot snapshot;
|
||||
const auto snapshot_result = queryNavigationSnapshot_(snapshot);
|
||||
if (!snapshot_result.ok()) {
|
||||
return AgvResult::failure(
|
||||
snapshot_result.code,
|
||||
"SEER Robokit did not send the conditional navigation cancel "
|
||||
"because 1101 ownership preflight was unavailable: "
|
||||
+ snapshot_result.message);
|
||||
}
|
||||
const bool global_active =
|
||||
exactTaskStateIsActive(snapshot.task_status);
|
||||
const bool global_type_matches =
|
||||
globalNavigationTaskTypeMatches(
|
||||
expected_active_navigation->type,
|
||||
snapshot.task_type);
|
||||
const bool target_conflicts = global_active
|
||||
&& !snapshot.target_id.empty()
|
||||
&& !expected_active_navigation->target_ids.empty()
|
||||
&& std::find(
|
||||
expected_active_navigation->target_ids.begin(),
|
||||
expected_active_navigation->target_ids.end(),
|
||||
snapshot.target_id)
|
||||
== expected_active_navigation->target_ids.end();
|
||||
if (global_active
|
||||
&& (!global_type_matches
|
||||
|| target_conflicts)) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit did not send the conditional navigation cancel "
|
||||
"because 1101 reports another active task: "
|
||||
+ snapshot.detail);
|
||||
}
|
||||
|
||||
if (!exact_task_still_active) {
|
||||
const bool clearing_path_queue =
|
||||
expected_active_navigation->type
|
||||
== AgvTaskType::FollowPath;
|
||||
const bool terminal_target_matches =
|
||||
expected_active_navigation->type
|
||||
!= AgvTaskType::NavigateToStation
|
||||
|| (!snapshot.target_id.empty()
|
||||
&& snapshot.target_id
|
||||
== expected_active_navigation->target_id);
|
||||
if (globalTaskStateIsKnownTerminal(snapshot.task_status)
|
||||
&& snapshot.task_status != 0
|
||||
&& !clearing_path_queue
|
||||
&& global_type_matches
|
||||
&& terminal_target_matches) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Keep the permission acquisition and the following write ordered with
|
||||
// respect to other control RPCs in this process. Channel I/O serialization
|
||||
// is separate, so this must remain a distinct lock.
|
||||
std::lock_guard<std::mutex> sequence_lock(control_sequence_mutex_);
|
||||
if (expected_navigation_token) {
|
||||
TrackedNavigationContext active_context;
|
||||
const bool has_active_context =
|
||||
currentTrackedNavigation_(active_context);
|
||||
const bool expected_context_matches = expected_active_navigation
|
||||
? (has_active_context
|
||||
&& active_context.token
|
||||
== expected_active_navigation->token
|
||||
&& active_context.navigation_generation
|
||||
== expected_active_navigation->navigation_generation
|
||||
&& active_context.type
|
||||
== expected_active_navigation->type
|
||||
&& navigation_generation_.load(std::memory_order_relaxed)
|
||||
== expected_active_navigation->navigation_generation)
|
||||
: (has_active_context
|
||||
&& active_context.token == *expected_navigation_token);
|
||||
if (!expected_context_matches) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit did not send the conditional navigation cancel "
|
||||
"because the tracked task was already replaced or ended; "
|
||||
"expected_token=" + *expected_navigation_token
|
||||
+ (active_context.token.empty()
|
||||
? std::string(", active_token=<none>")
|
||||
: ", active_token=" + active_context.token));
|
||||
}
|
||||
}
|
||||
const auto attempt_sequence =
|
||||
control_attempt_sequence_.fetch_add(
|
||||
1,
|
||||
std::memory_order_relaxed) + 1;
|
||||
if (control_attempt_sequence) {
|
||||
*control_attempt_sequence = attempt_sequence;
|
||||
}
|
||||
if (pose_context_to_publish) {
|
||||
pose_context_to_publish->control_attempt_sequence_at_start =
|
||||
attempt_sequence;
|
||||
}
|
||||
|
||||
const auto authority = acquireControl_();
|
||||
if (!authority.ok()) {
|
||||
const std::string detail = authority.message.empty() ? "unknown error" : authority.message;
|
||||
return AgvResult::failure(
|
||||
authority.code,
|
||||
"SEER Robokit acquire control authority failed: " + detail);
|
||||
}
|
||||
if (expected_active_navigation && !expected_context_is_current()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit did not send the conditional navigation cancel because "
|
||||
"the tracked token, generation, or type changed while control "
|
||||
"authority was being acquired");
|
||||
}
|
||||
if (canceled_before_send()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit command was not sent because the caller canceled the "
|
||||
"operation while control authority was being acquired");
|
||||
}
|
||||
std::string controller_fault_gate_error;
|
||||
if (controller_fault_sequence_at_attempt
|
||||
|| pose_context_to_publish
|
||||
|| reject_if_active_controller_fault) {
|
||||
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
|
||||
if (controller_fault_sequence_at_attempt) {
|
||||
*controller_fault_sequence_at_attempt =
|
||||
controller_fault_sequence_;
|
||||
}
|
||||
if (pose_context_to_publish) {
|
||||
pose_context_to_publish->controller_fault_sequence_at_start =
|
||||
controller_fault_sequence_;
|
||||
pose_context_to_publish
|
||||
->controller_fault_channel_epoch_at_start =
|
||||
controller_fault_channel_epoch_.load(
|
||||
std::memory_order_relaxed);
|
||||
}
|
||||
if (reject_if_active_controller_fault) {
|
||||
if (!state_push_enabled_) {
|
||||
controller_fault_gate_error =
|
||||
"controller fault state is unavailable because state push "
|
||||
"is disabled";
|
||||
} else if (!active_controller_fault_detail_.empty()) {
|
||||
controller_fault_gate_error =
|
||||
"the controller reported a fault or invalid fault state: "
|
||||
+ active_controller_fault_detail_;
|
||||
} else if (!controller_fault_state_observed_) {
|
||||
controller_fault_gate_error =
|
||||
"no state push containing fatals/errors has been observed";
|
||||
} else {
|
||||
const auto fault_state_age =
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::steady_clock::now()
|
||||
- controller_fault_state_observed_at_)
|
||||
.count();
|
||||
if (fault_state_age > controllerFaultStateMaxAgeMs_()) {
|
||||
controller_fault_gate_error =
|
||||
"the most recent fatals/errors state push is stale "
|
||||
"(age_ms=" + std::to_string(fault_state_age)
|
||||
+ ", max_age_ms="
|
||||
+ std::to_string(controllerFaultStateMaxAgeMs_())
|
||||
+ ")";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!controller_fault_gate_error.empty()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::Fault,
|
||||
"SEER Robokit free-navigation command was not sent because "
|
||||
+ controller_fault_gate_error);
|
||||
}
|
||||
if (canceled_before_send()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit command was not sent because the caller canceled the "
|
||||
"operation before the controller command write");
|
||||
}
|
||||
if (canceled_before_send()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit command was not sent because the caller canceled the "
|
||||
"operation immediately before the controller command write");
|
||||
}
|
||||
const auto publish_navigation_generation =
|
||||
[this,
|
||||
accepted_navigation_generation,
|
||||
pose_context_to_publish,
|
||||
navigation_context_to_publish,
|
||||
preserve_tracked_navigation]() {
|
||||
const auto generation =
|
||||
navigation_generation_.fetch_add(
|
||||
1,
|
||||
std::memory_order_relaxed) + 1;
|
||||
*accepted_navigation_generation = generation;
|
||||
if (pose_context_to_publish) {
|
||||
pose_context_to_publish->navigation_generation = generation;
|
||||
rememberPoseTask_(*pose_context_to_publish);
|
||||
}
|
||||
if (navigation_context_to_publish) {
|
||||
navigation_context_to_publish->navigation_generation =
|
||||
generation;
|
||||
navigation_context_to_publish->accepted_at =
|
||||
std::chrono::steady_clock::now();
|
||||
rememberTrackedNavigation_(
|
||||
*navigation_context_to_publish);
|
||||
} else if (preserve_tracked_navigation) {
|
||||
advanceTrackedNavigationGeneration_(generation);
|
||||
} else {
|
||||
clearTrackedNavigation_(generation);
|
||||
}
|
||||
};
|
||||
CommandTransmissionState transmission_state =
|
||||
CommandTransmissionState::NotSent;
|
||||
auto result = sendCommand_(
|
||||
sock,
|
||||
command,
|
||||
payload,
|
||||
response,
|
||||
&transmission_state);
|
||||
if (!result.ok()) {
|
||||
if (accepted_navigation_generation
|
||||
&& transmission_state
|
||||
== CommandTransmissionState::PossiblySent) {
|
||||
// Once the control write has been attempted, a timeout, disconnect,
|
||||
// wrong response opcode, or malformed JSON cannot prove rejection:
|
||||
// the controller may already have executed the command.
|
||||
publish_navigation_generation();
|
||||
return withUnknownControllerOutcome(std::move(result));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
if (!accepted_navigation_generation) {
|
||||
return result;
|
||||
}
|
||||
if (!response) {
|
||||
publish_navigation_generation();
|
||||
return withUnknownControllerOutcome(AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SEER Robokit cannot confirm navigation command without a response"));
|
||||
}
|
||||
if (!hasNumericControllerRetCode(*response)) {
|
||||
publish_navigation_generation();
|
||||
return withUnknownControllerOutcome(resultFromResponse_(*response));
|
||||
}
|
||||
result = resultFromResponse_(*response);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
|
||||
// Advance only after the controller accepted the command, and do it before
|
||||
// releasing control_sequence_mutex_. This prevents a failed cancel/pause or
|
||||
// failed authority acquisition from falsely reporting a pose task canceled,
|
||||
// while preserving the controller's actual command order under concurrency.
|
||||
publish_navigation_generation();
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
1047
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp
Normal file
1047
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp
Normal file
File diff suppressed because it is too large
Load Diff
1543
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp
Normal file
1543
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp
Normal file
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
725
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp
Normal file
725
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp
Normal file
@ -0,0 +1,725 @@
|
||||
#include "seer_robokit_agv.h"
|
||||
#include "seer_robokit_protocol.h"
|
||||
#include "seer_robokit_utils.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <mutex>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <sys/socket.h>
|
||||
#include <thread>
|
||||
|
||||
#include <google/protobuf/repeated_ptr_field.h>
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
using namespace seer_robokit::protocol;
|
||||
using namespace seer_robokit::detail;
|
||||
|
||||
namespace {
|
||||
|
||||
bool hasFaultArray(const Json::Value& value, const char* key)
|
||||
{
|
||||
const auto* found = jsonFind(value, key);
|
||||
return found && found->isArray() && !found->empty();
|
||||
}
|
||||
|
||||
void appendStringArray(Json::Value& value, const char* key, const google::protobuf::RepeatedPtrField<std::string>& strings)
|
||||
{
|
||||
if (strings.empty()) {
|
||||
return;
|
||||
}
|
||||
Json::Value array(Json::arrayValue);
|
||||
for (const auto& item : strings) {
|
||||
array.append(item);
|
||||
}
|
||||
jsonMember(value, key) = array;
|
||||
}
|
||||
|
||||
AgvMode modeFromTaskState(const int state)
|
||||
{
|
||||
switch (state) {
|
||||
case 2:
|
||||
return AgvMode::Auto;
|
||||
case 3:
|
||||
return AgvMode::Paused;
|
||||
case 5:
|
||||
return AgvMode::Fault;
|
||||
case 6:
|
||||
return AgvMode::Stopped;
|
||||
default:
|
||||
return AgvMode::Idle;
|
||||
}
|
||||
}
|
||||
|
||||
AgvTaskState toTaskState(const int value)
|
||||
{
|
||||
switch (value) {
|
||||
case 1:
|
||||
return AgvTaskState::Waiting;
|
||||
case 2:
|
||||
return AgvTaskState::Running;
|
||||
case 3:
|
||||
return AgvTaskState::Paused;
|
||||
case 4:
|
||||
return AgvTaskState::Completed;
|
||||
case 5:
|
||||
case 7:
|
||||
return AgvTaskState::Failed;
|
||||
case 6:
|
||||
return AgvTaskState::Canceled;
|
||||
case 0:
|
||||
default:
|
||||
return AgvTaskState::None;
|
||||
}
|
||||
}
|
||||
|
||||
AgvTaskType toTaskType(const int value)
|
||||
{
|
||||
switch (value) {
|
||||
case 1:
|
||||
return AgvTaskType::NavigateToPose;
|
||||
case 2:
|
||||
return AgvTaskType::NavigateToStation;
|
||||
case 3:
|
||||
return AgvTaskType::FollowPath;
|
||||
case 100:
|
||||
return AgvTaskType::Custom;
|
||||
default:
|
||||
return AgvTaskType::None;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
AgvRuntimeState SeerRobokitAgv::runtimeState() const
|
||||
{
|
||||
AgvRuntimeState cached_state;
|
||||
bool has_cached_state = false;
|
||||
if (state_push_enabled_) {
|
||||
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
|
||||
if (cached_runtime_state_valid_) {
|
||||
cached_state = cached_runtime_state_;
|
||||
has_cached_state = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (has_cached_state) {
|
||||
std::string adapter_error;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
cached_state.connected = connected_();
|
||||
adapter_error = last_error_;
|
||||
}
|
||||
if (!adapter_error.empty()) {
|
||||
if (cached_state.last_error.empty()) {
|
||||
cached_state.last_error = adapter_error;
|
||||
} else if (cached_state.last_error != adapter_error) {
|
||||
cached_state.last_error += "; adapter_error=" + adapter_error;
|
||||
}
|
||||
}
|
||||
if (!cached_state.connected) {
|
||||
cached_state.mode = AgvMode::Disconnected;
|
||||
}
|
||||
return cached_state;
|
||||
}
|
||||
|
||||
return queryRuntimeState_();
|
||||
}
|
||||
|
||||
AgvRuntimeState SeerRobokitAgv::queryRuntimeState_() const
|
||||
{
|
||||
AgvRuntimeState state;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
state.connected = connected_();
|
||||
state.last_error = last_error_;
|
||||
}
|
||||
state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected;
|
||||
|
||||
Json::Value loc;
|
||||
if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) {
|
||||
state.pose.x = jsonGet(loc, "x", 0.0).asDouble();
|
||||
state.pose.y = jsonGet(loc, "y", 0.0).asDouble();
|
||||
state.pose.theta = jsonGet(loc, "angle", 0.0).asDouble();
|
||||
state.localized = jsonGet(loc, "confidence", 0.0).asDouble() > 0.0;
|
||||
state.current_station = jsonGet(loc, "current_station", "").asString();
|
||||
}
|
||||
|
||||
Json::Value battery;
|
||||
if (sendCommand_(sock_status_, kRobotStatusBattery, Json::Value(Json::objectValue), &battery).ok()) {
|
||||
state.battery.percentage = jsonGet(battery, "battery_level", 0.0).asDouble();
|
||||
state.battery.temperature = jsonGet(battery, "battery_temp", 0.0).asDouble();
|
||||
state.battery.charging = jsonGet(battery, "charging", false).asBool();
|
||||
state.battery.voltage = jsonGet(battery, "voltage", 0.0).asDouble();
|
||||
state.battery.current = jsonGet(battery, "current", 0.0).asDouble();
|
||||
}
|
||||
|
||||
Json::Value map;
|
||||
if (sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &map).ok()) {
|
||||
state.current_map = jsonGet(map, "current_map", "").asString();
|
||||
}
|
||||
|
||||
const auto nav = navigationStatus();
|
||||
state.moving = nav.state == AgvTaskState::Running;
|
||||
state.fault = nav.state == AgvTaskState::Failed;
|
||||
state.mode = state.fault ? AgvMode::Fault : modeFromTaskState(static_cast<int>(nav.state));
|
||||
return state;
|
||||
}
|
||||
|
||||
AgvNavigationStatus SeerRobokitAgv::navigationStatus() const
|
||||
{
|
||||
AgvNavigationStatus status;
|
||||
std::string missing_pose_task_detail;
|
||||
for (int attempt = 0; attempt < 2; ++attempt) {
|
||||
PoseTaskContext pose_context;
|
||||
if (!currentPoseTask_(pose_context)) {
|
||||
break;
|
||||
}
|
||||
const auto observed_navigation_generation =
|
||||
navigation_generation_.load(std::memory_order_relaxed);
|
||||
if (pose_context.navigation_generation
|
||||
!= observed_navigation_generation) {
|
||||
continue;
|
||||
}
|
||||
|
||||
PoseTaskStatus task_status;
|
||||
const auto result = queryPoseTaskStatus_(pose_context.task_id, task_status);
|
||||
PoseTaskContext latest_context;
|
||||
if (navigation_generation_.load(std::memory_order_relaxed)
|
||||
!= observed_navigation_generation
|
||||
|| !currentPoseTask_(latest_context)
|
||||
|| latest_context.navigation_generation
|
||||
!= pose_context.navigation_generation
|
||||
|| latest_context.task_id != pose_context.task_id) {
|
||||
continue;
|
||||
}
|
||||
|
||||
status.type = AgvTaskType::NavigateToPose;
|
||||
const auto fault_monitoring_unavailable =
|
||||
[this, &pose_context]() {
|
||||
if (controller_fault_channel_epoch_.load(
|
||||
std::memory_order_relaxed)
|
||||
!= pose_context
|
||||
.controller_fault_channel_epoch_at_start) {
|
||||
return std::string(
|
||||
"the controller fault push channel changed or was "
|
||||
"invalidated after the free-navigation command was "
|
||||
"accepted");
|
||||
}
|
||||
return freeNavigationFaultStateUnavailableDetail_();
|
||||
};
|
||||
if (!result.ok()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message = result.message;
|
||||
return status;
|
||||
}
|
||||
if (!task_status.found || task_status.state == 404) {
|
||||
std::uint64_t missing_task_fault_control_attempt = 0;
|
||||
const std::string missing_task_fault =
|
||||
cachedControllerFaultDetail_(
|
||||
pose_context.controller_fault_sequence_at_start,
|
||||
controllerFaultCaptureGraceMs_(),
|
||||
&missing_task_fault_control_attempt);
|
||||
PoseTaskContext post_missing_context;
|
||||
if (navigation_generation_.load(std::memory_order_relaxed)
|
||||
!= observed_navigation_generation
|
||||
|| !currentPoseTask_(post_missing_context)
|
||||
|| post_missing_context.navigation_generation
|
||||
!= pose_context.navigation_generation
|
||||
|| post_missing_context.task_id != pose_context.task_id) {
|
||||
continue;
|
||||
}
|
||||
if (!missing_task_fault.empty()) {
|
||||
std::string attribution;
|
||||
if (missing_task_fault_control_attempt != 0
|
||||
&& missing_task_fault_control_attempt
|
||||
!= pose_context.control_attempt_sequence_at_start) {
|
||||
attribution =
|
||||
"controller_fault_attribution=ambiguous because the "
|
||||
"fault was observed after another control command "
|
||||
"attempt had begun, ";
|
||||
}
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit tracked free-navigation task disappeared from "
|
||||
"1110 task_status_package while a new controller fault "
|
||||
"was observed: " + task_status.detail + ", "
|
||||
+ attribution + missing_task_fault;
|
||||
clearPoseTask_(pose_context.navigation_generation);
|
||||
return status;
|
||||
}
|
||||
if (const std::string unavailable =
|
||||
fault_monitoring_unavailable();
|
||||
!unavailable.empty()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit tracked free-navigation status is unsafe to "
|
||||
"accept because controller fault monitoring is "
|
||||
"unavailable: " + unavailable
|
||||
+ "; query the controller and cancel or stop before "
|
||||
"another motion command";
|
||||
return status;
|
||||
}
|
||||
missing_pose_task_detail = task_status.detail;
|
||||
clearPoseTask_(pose_context.navigation_generation);
|
||||
break;
|
||||
}
|
||||
if (task_status.type_present && task_status.type != 1) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.type = toTaskType(task_status.type);
|
||||
status.message =
|
||||
"SEER Robokit returned an unexpected task type for the tracked "
|
||||
"free-navigation task: " + task_status.detail;
|
||||
clearPoseTask_(pose_context.navigation_generation);
|
||||
return status;
|
||||
}
|
||||
status.state = toTaskState(task_status.state);
|
||||
status.progress = task_status.progress;
|
||||
status.message = task_status.detail;
|
||||
const auto controller_reported_state = status.state;
|
||||
const bool controller_state_terminal =
|
||||
controller_reported_state == AgvTaskState::Completed
|
||||
|| controller_reported_state == AgvTaskState::Failed
|
||||
|| controller_reported_state == AgvTaskState::Canceled;
|
||||
std::uint64_t fault_control_attempt = 0;
|
||||
const std::string fault = cachedControllerFaultDetail_(
|
||||
pose_context.controller_fault_sequence_at_start,
|
||||
controller_reported_state == AgvTaskState::Completed
|
||||
|| controller_reported_state == AgvTaskState::Failed
|
||||
? controllerFaultCaptureGraceMs_()
|
||||
: 0,
|
||||
&fault_control_attempt);
|
||||
PoseTaskContext post_fault_context;
|
||||
if (navigation_generation_.load(std::memory_order_relaxed)
|
||||
!= observed_navigation_generation
|
||||
|| !currentPoseTask_(post_fault_context)
|
||||
|| post_fault_context.navigation_generation
|
||||
!= pose_context.navigation_generation
|
||||
|| post_fault_context.task_id != pose_context.task_id) {
|
||||
continue;
|
||||
}
|
||||
const std::string unavailable =
|
||||
fault_monitoring_unavailable();
|
||||
if (!fault.empty()) {
|
||||
std::string attribution;
|
||||
if (fault_control_attempt != 0
|
||||
&& fault_control_attempt
|
||||
!= pose_context.control_attempt_sequence_at_start) {
|
||||
attribution =
|
||||
"controller_fault_attribution=ambiguous because the "
|
||||
"fault was observed after another control command "
|
||||
"attempt had begun, ";
|
||||
}
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit reported a new controller fault while the tracked "
|
||||
"free-navigation task had controller_task_state="
|
||||
+ std::to_string(task_status.state) + ": "
|
||||
+ task_status.detail + ", " + attribution + fault;
|
||||
if (!unavailable.empty()) {
|
||||
status.message +=
|
||||
", controller_fault_monitoring_unavailable="
|
||||
+ unavailable;
|
||||
}
|
||||
} else if (!unavailable.empty()) {
|
||||
if (controller_reported_state == AgvTaskState::Failed
|
||||
|| controller_reported_state == AgvTaskState::Canceled) {
|
||||
status.message +=
|
||||
", controller_fault_monitoring_unavailable="
|
||||
+ unavailable;
|
||||
} else {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit tracked free-navigation state is unsafe to accept "
|
||||
"because controller fault monitoring became unavailable: "
|
||||
+ unavailable
|
||||
+ "; query the controller and cancel or stop before "
|
||||
"another motion command";
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if (status.state == AgvTaskState::Completed) {
|
||||
std::string pose_detail;
|
||||
const bool target_reached =
|
||||
poseTargetReached_(pose_context, pose_detail);
|
||||
PoseTaskContext post_pose_context;
|
||||
if (navigation_generation_.load(std::memory_order_relaxed)
|
||||
!= observed_navigation_generation
|
||||
|| !currentPoseTask_(post_pose_context)
|
||||
|| post_pose_context.navigation_generation
|
||||
!= pose_context.navigation_generation
|
||||
|| post_pose_context.task_id != pose_context.task_id) {
|
||||
continue;
|
||||
}
|
||||
std::uint64_t post_pose_fault_control_attempt = 0;
|
||||
const std::string post_pose_fault =
|
||||
cachedControllerFaultDetail_(
|
||||
pose_context.controller_fault_sequence_at_start,
|
||||
0,
|
||||
&post_pose_fault_control_attempt);
|
||||
if (!post_pose_fault.empty()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
std::string attribution;
|
||||
if (post_pose_fault_control_attempt != 0
|
||||
&& post_pose_fault_control_attempt
|
||||
!= pose_context
|
||||
.control_attempt_sequence_at_start) {
|
||||
attribution =
|
||||
"controller_fault_attribution=ambiguous because the "
|
||||
"fault was observed after another control command "
|
||||
"attempt had begun, ";
|
||||
}
|
||||
status.message =
|
||||
"SEER Robokit reported the tracked free-navigation task "
|
||||
"Completed, but a new controller fault was observed during "
|
||||
"target verification: " + task_status.detail + ", "
|
||||
+ attribution + post_pose_fault;
|
||||
} else if (const std::string post_pose_unavailable =
|
||||
fault_monitoring_unavailable();
|
||||
!post_pose_unavailable.empty()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit tracked free-navigation completion is unsafe to "
|
||||
"accept because controller fault monitoring became "
|
||||
"unavailable: " + post_pose_unavailable
|
||||
+ "; query the controller and cancel or stop before "
|
||||
"another motion command";
|
||||
return status;
|
||||
} else if (!target_reached) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit reported the tracked free-navigation task "
|
||||
"Completed, but the requested target was not reached: "
|
||||
+ task_status.detail + ", " + pose_detail;
|
||||
} else {
|
||||
status.message += ", target_verified: " + pose_detail;
|
||||
}
|
||||
}
|
||||
if (controller_state_terminal) {
|
||||
clearPoseTask_(pose_context.navigation_generation);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
PoseTaskContext changed_context;
|
||||
if (currentPoseTask_(changed_context)) {
|
||||
status.state = AgvTaskState::Waiting;
|
||||
status.type = AgvTaskType::NavigateToPose;
|
||||
status.message =
|
||||
"SEER Robokit free-navigation task changed while its status was being "
|
||||
"queried; query navigation status again";
|
||||
return status;
|
||||
}
|
||||
|
||||
Json::Value payload(Json::objectValue);
|
||||
jsonMember(payload, "simple") = false;
|
||||
|
||||
Json::Value response;
|
||||
const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response);
|
||||
if (!result.ok()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message = missing_pose_task_detail.empty()
|
||||
? result.message
|
||||
: missing_pose_task_detail + "; 1020 status query failed: "
|
||||
+ result.message;
|
||||
return status;
|
||||
}
|
||||
const auto controller_result = resultFromResponse_(response);
|
||||
if (!controller_result.ok()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message = missing_pose_task_detail.empty()
|
||||
? controller_result.message
|
||||
: missing_pose_task_detail + "; 1020 status query failed: "
|
||||
+ controller_result.message;
|
||||
return status;
|
||||
}
|
||||
|
||||
status.state = toTaskState(jsonGet(response, "task_status", 0).asInt());
|
||||
status.type = toTaskType(jsonGet(response, "task_type", 0).asInt());
|
||||
status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString();
|
||||
if (!missing_pose_task_detail.empty()) {
|
||||
status.message = missing_pose_task_detail
|
||||
+ "; fallback_1020_status=" + std::to_string(
|
||||
jsonGet(response, "task_status", 0).asInt())
|
||||
+ ", fallback_1020_type=" + std::to_string(
|
||||
jsonGet(response, "task_type", 0).asInt())
|
||||
+ (status.message.empty() ? std::string{} : ", " + status.message);
|
||||
}
|
||||
if (const auto* task_status_package = jsonFind(response, "task_status_package")) {
|
||||
status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble();
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::configurePush_()
|
||||
{
|
||||
if (config_.state_push_included_fields_size() > 0 && config_.state_push_excluded_fields_size() > 0) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::InvalidArgument,
|
||||
"SEER Robokit push included_fields and excluded_fields cannot both be set");
|
||||
}
|
||||
|
||||
Json::Value payload(Json::objectValue);
|
||||
if (config_.state_push_interval_ms() > 0) {
|
||||
jsonMember(payload, "interval") = config_.state_push_interval_ms();
|
||||
}
|
||||
appendStringArray(payload, "included_fields", config_.state_push_included_fields());
|
||||
appendStringArray(payload, "excluded_fields", config_.state_push_excluded_fields());
|
||||
|
||||
if (payload.empty()) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
const std::string payload_text = toJsonString_(payload);
|
||||
const auto frame = buildFrame_(kRobotPushConfigReq, payload_text);
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock_push_ < 0) {
|
||||
return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit push socket not connected");
|
||||
}
|
||||
if (::send(sock_push_, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast<ssize_t>(frame.size())) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit send push config failed: " + systemError());
|
||||
}
|
||||
|
||||
while (true) {
|
||||
std::uint16_t command = 0;
|
||||
std::string response_payload;
|
||||
const auto result = receiveFrame_(sock_push_, command, response_payload);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
|
||||
Json::Value response;
|
||||
std::string error;
|
||||
if (!response_payload.empty() && !parseJson_(response_payload, response, error)) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, error);
|
||||
}
|
||||
|
||||
if (command == kRobotPushConfigRes) {
|
||||
return resultFromResponse_(response);
|
||||
}
|
||||
if (command == kRobotPush && response.isObject()) {
|
||||
updateCachedRuntimeState_(response);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::startPushThread_()
|
||||
{
|
||||
if (!state_push_enabled_) {
|
||||
return;
|
||||
}
|
||||
if (push_running_.exchange(true)) {
|
||||
return;
|
||||
}
|
||||
if (sock_push_ < 0) {
|
||||
push_running_ = false;
|
||||
return;
|
||||
}
|
||||
push_thread_ = std::thread(&SeerRobokitAgv::pushLoop_, this);
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::stopPushThread_()
|
||||
{
|
||||
const bool was_running = push_running_.exchange(false);
|
||||
if (was_running) {
|
||||
int sock = -1;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
sock = sock_push_;
|
||||
}
|
||||
if (sock >= 0) {
|
||||
::shutdown(sock, SHUT_RDWR);
|
||||
}
|
||||
}
|
||||
if (push_thread_.joinable()) {
|
||||
push_thread_.join();
|
||||
}
|
||||
invalidateControllerFaultState_();
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::pushLoop_()
|
||||
{
|
||||
while (push_running_) {
|
||||
int sock = -1;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
sock = sock_push_;
|
||||
}
|
||||
if (sock < 0) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
continue;
|
||||
}
|
||||
|
||||
std::uint16_t command = 0;
|
||||
std::string payload;
|
||||
const auto result = receiveFrame_(sock, command, payload);
|
||||
if (!push_running_) {
|
||||
break;
|
||||
}
|
||||
if (!result.ok()) {
|
||||
if (result.code != AgvErrorCode::Timeout) {
|
||||
invalidateControllerFaultState_();
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
last_error_ = result.message;
|
||||
closeSocket_(sock_push_);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if (command != kRobotPush || payload.empty()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Json::Value parsed;
|
||||
std::string error;
|
||||
if (!parseJson_(payload, parsed, error)) {
|
||||
invalidateControllerFaultState_();
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
last_error_ = error;
|
||||
continue;
|
||||
}
|
||||
updateCachedRuntimeState_(parsed);
|
||||
}
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::invalidateControllerFaultState_()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
|
||||
controller_fault_channel_epoch_.fetch_add(
|
||||
1,
|
||||
std::memory_order_relaxed);
|
||||
controller_fault_state_observed_ = false;
|
||||
controller_fault_state_observed_at_ = {};
|
||||
active_controller_fault_detail_.clear();
|
||||
runtime_state_cv_.notify_all();
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::updateCachedRuntimeState_(const Json::Value& payload)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
|
||||
auto state = cached_runtime_state_valid_ ? cached_runtime_state_ : AgvRuntimeState{};
|
||||
state.timestamp = nowSeconds();
|
||||
state.connected = true;
|
||||
|
||||
if (jsonHas(payload, "x")) state.pose.x = jsonGet(payload, "x", state.pose.x).asDouble();
|
||||
if (jsonHas(payload, "y")) state.pose.y = jsonGet(payload, "y", state.pose.y).asDouble();
|
||||
if (jsonHas(payload, "angle")) state.pose.theta = jsonGet(payload, "angle", state.pose.theta).asDouble();
|
||||
if (jsonHas(payload, "vx")) state.velocity.vx = jsonGet(payload, "vx", state.velocity.vx).asDouble();
|
||||
if (jsonHas(payload, "vy")) state.velocity.vy = jsonGet(payload, "vy", state.velocity.vy).asDouble();
|
||||
if (jsonHas(payload, "w")) state.velocity.wz = jsonGet(payload, "w", state.velocity.wz).asDouble();
|
||||
if (jsonHas(payload, "battery_level")) {
|
||||
state.battery.percentage = jsonGet(payload, "battery_level", state.battery.percentage).asDouble();
|
||||
}
|
||||
if (jsonHas(payload, "battery_temp")) {
|
||||
state.battery.temperature = jsonGet(payload, "battery_temp", state.battery.temperature).asDouble();
|
||||
}
|
||||
if (jsonHas(payload, "charging")) {
|
||||
state.battery.charging = jsonGet(payload, "charging", state.battery.charging).asBool();
|
||||
}
|
||||
if (jsonHas(payload, "voltage")) {
|
||||
state.battery.voltage = jsonGet(payload, "voltage", state.battery.voltage).asDouble();
|
||||
}
|
||||
if (jsonHas(payload, "current")) {
|
||||
state.battery.current = jsonGet(payload, "current", state.battery.current).asDouble();
|
||||
}
|
||||
if (jsonHas(payload, "current_map")) {
|
||||
state.current_map = jsonGet(payload, "current_map", state.current_map).asString();
|
||||
}
|
||||
if (jsonHas(payload, "current_station")) {
|
||||
state.current_station = jsonGet(payload, "current_station", state.current_station).asString();
|
||||
}
|
||||
if (jsonHas(payload, "confidence")) {
|
||||
state.localized = jsonGet(payload, "confidence", 0.0).asDouble() > 0.0;
|
||||
}
|
||||
if (jsonHas(payload, "emergency")) {
|
||||
state.emergency_stopped = jsonGet(payload, "emergency", state.emergency_stopped).asBool();
|
||||
}
|
||||
|
||||
state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4;
|
||||
const bool has_fatals = jsonHas(payload, "fatals");
|
||||
const bool has_errors = jsonHas(payload, "errors");
|
||||
const bool has_fault_fields = has_fatals || has_errors;
|
||||
if (has_fault_fields) {
|
||||
const auto* fatals = jsonFind(payload, "fatals");
|
||||
const auto* errors = jsonFind(payload, "errors");
|
||||
const bool valid_fatals = !has_fatals
|
||||
|| (fatals && fatals->isArray());
|
||||
const bool valid_errors = !has_errors
|
||||
|| (errors && errors->isArray());
|
||||
const bool complete_fault_state =
|
||||
has_fatals && has_errors && valid_fatals && valid_errors;
|
||||
if (complete_fault_state) {
|
||||
controller_fault_state_observed_ = true;
|
||||
controller_fault_state_observed_at_ =
|
||||
std::chrono::steady_clock::now();
|
||||
} else {
|
||||
controller_fault_state_observed_ = false;
|
||||
controller_fault_state_observed_at_ = {};
|
||||
}
|
||||
|
||||
const bool reported_fault =
|
||||
hasFaultArray(payload, "fatals")
|
||||
|| hasFaultArray(payload, "errors");
|
||||
const bool invalid_or_incomplete_fault_state =
|
||||
!complete_fault_state && !reported_fault;
|
||||
state.fault = reported_fault
|
||||
|| invalid_or_incomplete_fault_state;
|
||||
if (state.fault) {
|
||||
std::ostringstream detail;
|
||||
detail << (reported_fault
|
||||
? "SEER Robokit controller fault"
|
||||
: "SEER Robokit controller fault state is incomplete or malformed");
|
||||
if (fatals
|
||||
&& (!fatals->isArray()
|
||||
|| !fatals->empty()
|
||||
|| !complete_fault_state)) {
|
||||
detail << ": fatals="
|
||||
<< (fatals->isNull()
|
||||
? std::string("null")
|
||||
: jsonValueToString(*fatals));
|
||||
}
|
||||
if (errors
|
||||
&& (!errors->isArray()
|
||||
|| !errors->empty()
|
||||
|| !complete_fault_state)) {
|
||||
detail << ": errors="
|
||||
<< (errors->isNull()
|
||||
? std::string("null")
|
||||
: jsonValueToString(*errors));
|
||||
}
|
||||
state.last_error = detail.str();
|
||||
if (state.last_error != active_controller_fault_detail_) {
|
||||
active_controller_fault_detail_ = state.last_error;
|
||||
++controller_fault_sequence_;
|
||||
last_controller_fault_timestamp_ = state.timestamp;
|
||||
last_controller_fault_detail_ = state.last_error;
|
||||
last_controller_fault_control_attempt_ =
|
||||
control_attempt_sequence_.load(
|
||||
std::memory_order_acquire);
|
||||
}
|
||||
} else {
|
||||
state.last_error.clear();
|
||||
active_controller_fault_detail_.clear();
|
||||
}
|
||||
}
|
||||
if (state.emergency_stopped) {
|
||||
state.mode = AgvMode::EmergencyStop;
|
||||
} else if (state.fault) {
|
||||
state.mode = AgvMode::Fault;
|
||||
} else if (state.battery.charging) {
|
||||
state.mode = AgvMode::Charging;
|
||||
} else if (state.moving) {
|
||||
state.mode = AgvMode::Auto;
|
||||
} else {
|
||||
state.mode = AgvMode::Idle;
|
||||
}
|
||||
|
||||
cached_runtime_state_ = state;
|
||||
cached_runtime_state_valid_ = true;
|
||||
runtime_state_cv_.notify_all();
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
362
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp
Normal file
362
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp
Normal file
@ -0,0 +1,362 @@
|
||||
#include "seer_robokit_agv.h"
|
||||
#include "seer_robokit_protocol.h"
|
||||
#include "seer_robokit_utils.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <arpa/inet.h>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/time.h>
|
||||
#include <unistd.h>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
using namespace seer_robokit::protocol;
|
||||
using namespace seer_robokit::detail;
|
||||
|
||||
AgvResult SeerRobokitAgv::connectSocket_(int& sock, const int port)
|
||||
{
|
||||
sock = ::socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sock < 0) {
|
||||
last_error_ = "create socket failed: " + systemError();
|
||||
return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_);
|
||||
}
|
||||
|
||||
sockaddr_in address{};
|
||||
address.sin_family = AF_INET;
|
||||
address.sin_port = htons(static_cast<std::uint16_t>(port));
|
||||
if (::inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) {
|
||||
closeSocket_(sock);
|
||||
last_error_ = "invalid SEER Robokit ip: " + ip_;
|
||||
return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_);
|
||||
}
|
||||
|
||||
if (::connect(sock, reinterpret_cast<sockaddr*>(&address), sizeof(address)) < 0) {
|
||||
closeSocket_(sock);
|
||||
last_error_ = "connect SEER Robokit port " + std::to_string(port) + " failed: " + systemError();
|
||||
return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_);
|
||||
}
|
||||
|
||||
timeval timeout{};
|
||||
timeout.tv_sec = recv_timeout_ms_ / 1000;
|
||||
timeout.tv_usec = (recv_timeout_ms_ % 1000) * 1000;
|
||||
::setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::ensureOtherSocket_()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock_other_ >= 0) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
return connectSocket_(sock_other_, ports_.other);
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::closeSocket_(int& sock) const
|
||||
{
|
||||
if (sock >= 0) {
|
||||
::close(sock);
|
||||
sock = -1;
|
||||
}
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::connected_() const
|
||||
{
|
||||
return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::sendCommand_(
|
||||
const int sock,
|
||||
const std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response,
|
||||
CommandTransmissionState* transmission_state) const
|
||||
{
|
||||
std::string response_payload;
|
||||
auto result = sendCommandRaw_(
|
||||
sock,
|
||||
command,
|
||||
payload,
|
||||
&response_payload,
|
||||
transmission_state);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
if (!response) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
Json::Value parsed;
|
||||
std::string error;
|
||||
if (!parseJson_(response_payload, parsed, error)) {
|
||||
const std::string json_text = extractJson_(response_payload);
|
||||
if (json_text.empty() || !parseJson_(json_text, parsed, error)) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, error);
|
||||
}
|
||||
}
|
||||
|
||||
*response = std::move(parsed);
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::sendCommandRaw_(
|
||||
const int sock,
|
||||
const std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
std::string* response_payload,
|
||||
CommandTransmissionState* transmission_state) const
|
||||
{
|
||||
if (transmission_state) {
|
||||
*transmission_state = CommandTransmissionState::NotSent;
|
||||
}
|
||||
const auto exchange = [&]() {
|
||||
const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload);
|
||||
const auto frame = buildFrame_(command, payload_text);
|
||||
const auto sent = ::send(
|
||||
sock,
|
||||
frame.data(),
|
||||
frame.size(),
|
||||
MSG_NOSIGNAL);
|
||||
if (sent > 0 && transmission_state) {
|
||||
*transmission_state = CommandTransmissionState::PossiblySent;
|
||||
}
|
||||
if (sent != static_cast<ssize_t>(frame.size())) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SEER Robokit send command failed: " + systemError());
|
||||
}
|
||||
|
||||
std::uint16_t response_command = 0;
|
||||
std::string payload_text_response;
|
||||
const auto result = receiveFrame_(sock, response_command, payload_text_response);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
const auto expected_response_command = static_cast<std::uint16_t>(
|
||||
command + 10000U);
|
||||
if (response_command != expected_response_command) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SEER Robokit response command mismatch: expected="
|
||||
+ std::to_string(expected_response_command)
|
||||
+ ", actual=" + std::to_string(response_command));
|
||||
}
|
||||
if (response_payload) {
|
||||
*response_payload = std::move(payload_text_response);
|
||||
}
|
||||
return AgvResult::success();
|
||||
};
|
||||
const auto close_matching_socket_locked = [this, sock]() {
|
||||
if (sock == sock_status_) {
|
||||
closeSocket_(sock_status_);
|
||||
} else if (sock == sock_control_) {
|
||||
closeSocket_(sock_control_);
|
||||
} else if (sock == sock_navigation_) {
|
||||
closeSocket_(sock_navigation_);
|
||||
} else if (sock == sock_config_) {
|
||||
closeSocket_(sock_config_);
|
||||
} else if (sock == sock_other_) {
|
||||
closeSocket_(sock_other_);
|
||||
}
|
||||
};
|
||||
const auto mark_channel_desynchronized = [](AgvResult result) {
|
||||
std::string detail = result.message.empty()
|
||||
? "unknown transport or frame error"
|
||||
: result.message;
|
||||
detail +=
|
||||
"; SEER Robokit channel closed because the response stream may be "
|
||||
"desynchronized; reconnect before sending another command";
|
||||
return AgvResult::failure(result.code, detail);
|
||||
};
|
||||
|
||||
bool is_status_socket = false;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::NotConnected,
|
||||
"SEER Robokit socket not connected");
|
||||
}
|
||||
is_status_socket = sock == sock_status_;
|
||||
}
|
||||
|
||||
if (is_status_socket) {
|
||||
// A slow 1110 status response must never hold the lifecycle/global I/O
|
||||
// mutex needed by cancelNavigation() or emergencyStop(). The dedicated
|
||||
// status lock still serializes requests on port 19204. connect_() and
|
||||
// disconnect_() take this lock before changing the descriptor.
|
||||
std::lock_guard<std::mutex> status_lock(status_io_mutex_);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0 || sock != sock_status_) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::NotConnected,
|
||||
"SEER Robokit status socket is no longer connected");
|
||||
}
|
||||
}
|
||||
auto result = exchange();
|
||||
if (!result.ok()) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
close_matching_socket_locked();
|
||||
return mark_channel_desynchronized(std::move(result));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0
|
||||
|| (sock != sock_control_
|
||||
&& sock != sock_navigation_
|
||||
&& sock != sock_config_
|
||||
&& sock != sock_other_)) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::NotConnected,
|
||||
"SEER Robokit socket is no longer connected");
|
||||
}
|
||||
auto result = exchange();
|
||||
if (!result.ok()) {
|
||||
close_matching_socket_locked();
|
||||
return mark_channel_desynchronized(std::move(result));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::sendCommandNoResponse_(
|
||||
const int sock,
|
||||
const std::uint16_t command,
|
||||
const Json::Value& payload) const
|
||||
{
|
||||
return sendCommand_(sock, command, payload, nullptr);
|
||||
}
|
||||
|
||||
std::vector<std::uint8_t> SeerRobokitAgv::buildFrame_(
|
||||
const std::uint16_t command,
|
||||
const std::string& payload)
|
||||
{
|
||||
std::vector<std::uint8_t> frame(16 + payload.size(), 0);
|
||||
frame[0] = 0x5A;
|
||||
frame[1] = 0x01;
|
||||
frame[2] = 0x00;
|
||||
frame[3] = 0x01;
|
||||
const auto length = static_cast<std::uint32_t>(payload.size());
|
||||
frame[4] = static_cast<std::uint8_t>((length >> 24U) & 0xFFU);
|
||||
frame[5] = static_cast<std::uint8_t>((length >> 16U) & 0xFFU);
|
||||
frame[6] = static_cast<std::uint8_t>((length >> 8U) & 0xFFU);
|
||||
frame[7] = static_cast<std::uint8_t>(length & 0xFFU);
|
||||
frame[8] = static_cast<std::uint8_t>((command >> 8U) & 0xFFU);
|
||||
frame[9] = static_cast<std::uint8_t>(command & 0xFFU);
|
||||
std::copy(payload.begin(), payload.end(), frame.begin() + 16);
|
||||
return frame;
|
||||
}
|
||||
|
||||
std::string SeerRobokitAgv::toJsonString_(const Json::Value& value)
|
||||
{
|
||||
Json::StreamWriterBuilder builder;
|
||||
builder["indentation"] = "";
|
||||
return Json::writeString(builder, value);
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::parseJson_(const std::string& input, Json::Value& output, std::string& error)
|
||||
{
|
||||
Json::CharReaderBuilder builder;
|
||||
std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
|
||||
return reader->parse(input.data(), input.data() + input.size(), &output, &error);
|
||||
}
|
||||
|
||||
std::string SeerRobokitAgv::extractJson_(const std::string& raw)
|
||||
{
|
||||
const auto begin = raw.find('{');
|
||||
const auto end = raw.rfind('}');
|
||||
if (begin == std::string::npos || end == std::string::npos || end < begin) {
|
||||
return {};
|
||||
}
|
||||
return raw.substr(begin, end - begin + 1);
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::receiveFrame_(const int sock, std::uint16_t& command, std::string& payload)
|
||||
{
|
||||
const auto recv_exact = [](const int fd, std::uint8_t* data, const std::size_t size) -> AgvResult {
|
||||
std::size_t offset = 0;
|
||||
while (offset < size) {
|
||||
const ssize_t count = ::recv(fd, data + offset, size - offset, 0);
|
||||
if (count > 0) {
|
||||
offset += static_cast<std::size_t>(count);
|
||||
continue;
|
||||
}
|
||||
if (count == 0) {
|
||||
return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit socket closed");
|
||||
}
|
||||
if (errno == EINTR) {
|
||||
continue;
|
||||
}
|
||||
if (errno == EAGAIN || errno == EWOULDBLOCK) {
|
||||
return AgvResult::failure(AgvErrorCode::Timeout, "SEER Robokit receive timeout");
|
||||
}
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit receive failed: " + systemError());
|
||||
}
|
||||
return AgvResult::success();
|
||||
};
|
||||
|
||||
std::uint8_t header[16]{};
|
||||
auto result = recv_exact(sock, header, sizeof(header));
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
if (header[0] != 0x5A) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame header is invalid");
|
||||
}
|
||||
|
||||
const auto length = (static_cast<std::uint32_t>(header[4]) << 24U)
|
||||
| (static_cast<std::uint32_t>(header[5]) << 16U)
|
||||
| (static_cast<std::uint32_t>(header[6]) << 8U)
|
||||
| static_cast<std::uint32_t>(header[7]);
|
||||
command = static_cast<std::uint16_t>((static_cast<std::uint16_t>(header[8]) << 8U) | header[9]);
|
||||
payload.clear();
|
||||
if (length == 0) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
if (length > kMaxFramePayloadBytes) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame payload is too large");
|
||||
}
|
||||
|
||||
std::vector<std::uint8_t> buffer(length);
|
||||
result = recv_exact(sock, buffer.data(), buffer.size());
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
payload.assign(reinterpret_cast<const char*>(buffer.data()), buffer.size());
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
|
||||
AgvResult SeerRobokitAgv::resultFromResponse_(const Json::Value& response)
|
||||
{
|
||||
if (!hasNumericControllerRetCode(response)) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SEER Robokit controller response is missing a numeric ret_code");
|
||||
}
|
||||
const auto* ret_code_value = jsonFind(response, "ret_code");
|
||||
const bool success = ret_code_value->isUInt() || ret_code_value->isUInt64()
|
||||
? ret_code_value->asUInt64() == 0
|
||||
: ret_code_value->asInt64() == 0;
|
||||
const std::string ret_code = jsonValueToString(*ret_code_value);
|
||||
const std::string message = jsonGet(response, "err_msg", "").asString();
|
||||
if (success) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
std::string detail = "SEER Robokit command failed: ret_code=" + ret_code;
|
||||
if (!message.empty()) {
|
||||
detail += ", err_msg=" + message;
|
||||
}
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, detail);
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
139
cmvr-es/devices/arm/aubo_arm/README.md
Normal file
139
cmvr-es/devices/arm/aubo_arm/README.md
Normal file
@ -0,0 +1,139 @@
|
||||
# AUBO RobotArm 与控制柜 IO
|
||||
|
||||
`AuboArm` 是 AUBO SDK v0.27.1 的 `RobotArm` 后端。控制柜 Standard 数字 IO
|
||||
通过设备通用的 `executeJsonCommand` 接口访问，远程调用复用
|
||||
`cmvr.api.ArmService/ExecuteJsonCommand`，不经过 `SystemService` 或
|
||||
`MotorService`。该 RPC 只路由到 `RobotArm`，不会把 JSON 命令转发给其他设备类型。
|
||||
旧的 `cmvr.api.SystemService/ExecuteJsonCommand` 不再注册，调用方必须更新服务路径；
|
||||
请求和响应消息结构保持不变。
|
||||
|
||||
返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。
|
||||
|
||||
## 代码与配置
|
||||
|
||||
- 实现：[`aubo_arm.h`](aubo_arm.h)、[`aubo_arm.cpp`](aubo_arm.cpp)
|
||||
- 设备配置：[`../../../config/devices/arm/aubo_arm.pb.txt`](../../../config/devices/arm/aubo_arm.pb.txt)
|
||||
- DeviceManager 配置：
|
||||
[`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt)
|
||||
- ArmService 实现：
|
||||
[`../../../service/grpc/server/src/grpc_arm_service.cpp`](../../../service/grpc/server/src/grpc_arm_service.cpp)
|
||||
- Proto：[`../../../../protos/cmvr/api/arm_service.proto`](../../../../protos/cmvr/api/arm_service.proto)
|
||||
|
||||
仓库配置使用 SDK RPC 端口 `30004`。现场部署必须填写真实控制器地址和凭据，
|
||||
不要把生产密码提交到默认配置。
|
||||
|
||||
## 控制柜 Standard 数字 IO
|
||||
|
||||
当前支持：
|
||||
|
||||
| `operation` | 说明 | 必填字段 |
|
||||
| --- | --- | --- |
|
||||
| `get_di` | 读取控制柜数字输入 | `index` |
|
||||
| `get_do` | 读取控制柜数字输出及其 runstate | `index` |
|
||||
| `set_do` | 设置控制柜数字输出 | `index`、`value` |
|
||||
|
||||
JSON 命令：
|
||||
|
||||
```json
|
||||
{"command":"cabinet_io","operation":"get_di","index":0}
|
||||
{"command":"cabinet_io","operation":"get_do","index":0}
|
||||
{"command":"cabinet_io","operation":"set_do","index":0,"value":true}
|
||||
```
|
||||
|
||||
`index` 从 `0` 开始，运行时根据控制器返回的 IO 数量检查范围。
|
||||
`set_do.value` 必须是 JSON 布尔值 `true` 或 `false`，不接受 `0/1` 或字符串。
|
||||
`set_do` 成功响应中的 `requested_value` 只表示 SDK 已接受请求；确认实际输出时
|
||||
必须再调用 `get_do`。
|
||||
|
||||
读取成功响应示例：
|
||||
|
||||
```json
|
||||
{
|
||||
"success": true,
|
||||
"command": "cabinet_io",
|
||||
"operation": "get_di",
|
||||
"index": 0,
|
||||
"count": 16,
|
||||
"value": false
|
||||
}
|
||||
```
|
||||
|
||||
## 通过 gRPC 调用
|
||||
|
||||
默认 gRPC 端口为 `50052`。读取 DI0：
|
||||
|
||||
```shell
|
||||
grpcurl -plaintext \
|
||||
-d '{
|
||||
"header":{"deviceId":"aubo_arm"},
|
||||
"requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"get_di\",\"index\":0}"
|
||||
}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.ArmService/ExecuteJsonCommand
|
||||
```
|
||||
|
||||
设置 DO0 为高电平：
|
||||
|
||||
```shell
|
||||
grpcurl -plaintext \
|
||||
-d '{
|
||||
"header":{"deviceId":"aubo_arm"},
|
||||
"requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"set_do\",\"index\":0,\"value\":true}"
|
||||
}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.ArmService/ExecuteJsonCommand
|
||||
```
|
||||
|
||||
使用源码默认配置时：
|
||||
|
||||
1. 在 `cmvr-es/config/devices/arm/aubo_arm.pb.txt` 填写正确地址和登录信息；
|
||||
2. 在 `cmvr-es/config/manager/device_manager.pb.txt` 将 `aubo_arm.enable`
|
||||
改为 `true`；
|
||||
3. 重新安装配置并启动安装产物。
|
||||
|
||||
```shell
|
||||
cmake --install build
|
||||
./output/bin/cmvr_es
|
||||
```
|
||||
|
||||
`output/bin/cmvr_es` 默认读取 `output/bin/config/`。使用 `--config` 时，应修改
|
||||
对应外部配置根。设备未启用或初始化失败时，gRPC 返回
|
||||
`Device not found: aubo_arm`。
|
||||
|
||||
## 安全与语义边界
|
||||
|
||||
- 后端使用独立 SDK RPC 会话持续读取控制器的 `SafetyModeType`、
|
||||
`RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时，
|
||||
所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝；
|
||||
- 硬件急停会立即使当前运动 generation 失效，并在急停输入有效期间保持锁存。
|
||||
AUBO SDK 将示教器/控制柜急停报告为 `RobotEmergencyStop`，将控制器系统急停
|
||||
（外部系统急停输入）报告为 `SystemEmergencyStop`；两者在本后端都属于硬件急停。
|
||||
检测到任一硬件急停输入消失且控制器重新报告 `Normal`/`ReducedMode` 后，后端自动
|
||||
执行 `poweron()` 和 `startup()`，恢复到 `Running` 后再完成安全确认并开放新的 gRPC
|
||||
控制指令；防护停机和 Safety Fault/Violation 仍保持显式恢复语义；
|
||||
- `emergencyStop()` 使用独立的 `SoftwareEmergencyStop` 锁存。即使软件急停在真实
|
||||
硬件急停有效期间触发，后续硬件采样也不能覆盖该锁存，释放硬件急停开关不会
|
||||
自动清除软件急停；它只能通过显式安全恢复流程解除；
|
||||
- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。硬件急停
|
||||
自动恢复先上电到 `Idle`，在刹车释放前清理 runtime、servo 和轨迹队列，再执行
|
||||
`startup()`；到达 `Running` 后还会再次确认 `ExecId == -1`、普通队列和轨迹队列
|
||||
均为空、运行时已停止且机械臂稳定，全部成立后才能解除锁存；
|
||||
- 当前 AUBO 配置通过 `auto_power_on_after_hardware_estop_release: true` 显式启用自动
|
||||
上电。自动确认失败时继续保持 fail-closed，并允许通过 `torqueOn`/`clearFault`/
|
||||
`unlockProtectiveStop` 显式重试；本轮释放期间收到 `stopMotion()` 或 `torqueOff()`
|
||||
会取消自动上电，显式停止始终优先；
|
||||
- 恢复流程只调用 `poweron()` 和 `startup()`，不会调用 `resume`、`arbitraryResume`、
|
||||
`startMove`，也不会重新提交急停前的目标、速度、servo 指令或程序；
|
||||
- AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放
|
||||
急停开关后的控制器恢复时序。因此自动恢复必须在释放后再次清队列并完成上述
|
||||
安全确认；无法确认时不得解除锁存。“释放开关后零位移”的最终保证仍需真机
|
||||
验证及控制器侧安全配置配合；
|
||||
- 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO；
|
||||
- `set_do` 不修改输出 runstate；
|
||||
- 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回
|
||||
`output_managed_by_runstate`；
|
||||
- 普通访问不会调用会重置全部输出配置的
|
||||
`setDigitalOutputRunstateDefault()`；
|
||||
- 模拟量 IO 涉及 domain、单位和量程，当前 JSON 接口不开放；
|
||||
- gRPC/JSON 返回成功不代表目标 IO 具备功能安全等级；
|
||||
- 真实写测试前应确认通道用途、负载、电气隔离、默认电平和控制器程序所有权。
|
||||
File diff suppressed because it is too large
Load Diff
260
cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h
Normal file
260
cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h
Normal file
@ -0,0 +1,260 @@
|
||||
#ifndef CMVR_ES_AUBO_SAFETY_STATE_H
|
||||
#define CMVR_ES_AUBO_SAFETY_STATE_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
|
||||
namespace cmvr::device::aubo_internal {
|
||||
|
||||
// This is deliberately richer than RobotArm::SafetyMode. Recovery and
|
||||
// Violation have no lossless public mapping, but both must remain fail-closed.
|
||||
enum class SafetyCondition {
|
||||
Unknown,
|
||||
Normal,
|
||||
Reduced,
|
||||
Recovery,
|
||||
Violation,
|
||||
ProtectiveStop,
|
||||
SafeguardStop,
|
||||
SystemEmergencyStop,
|
||||
RobotEmergencyStop,
|
||||
SoftwareEmergencyStop,
|
||||
Fault,
|
||||
};
|
||||
|
||||
inline bool isMotionSafe(const SafetyCondition condition) noexcept
|
||||
{
|
||||
return condition == SafetyCondition::Normal ||
|
||||
condition == SafetyCondition::Reduced;
|
||||
}
|
||||
|
||||
inline bool isHardwareEmergencyStop(
|
||||
const SafetyCondition condition) noexcept
|
||||
{
|
||||
return condition == SafetyCondition::SystemEmergencyStop ||
|
||||
condition == SafetyCondition::RobotEmergencyStop;
|
||||
}
|
||||
|
||||
inline SafetyCondition effectiveSafetyCondition(
|
||||
const SafetyCondition reported_condition,
|
||||
const int robot_emergency_stop_source) noexcept
|
||||
{
|
||||
// The source bitmask describes robot-side inputs such as the control box
|
||||
// and teach pendant. Keep the SDK's SystemEmergencyStop classification for
|
||||
// the separate external system-emergency input.
|
||||
if (reported_condition == SafetyCondition::SystemEmergencyStop) {
|
||||
return SafetyCondition::SystemEmergencyStop;
|
||||
}
|
||||
if (robot_emergency_stop_source < 0) {
|
||||
return SafetyCondition::Unknown;
|
||||
}
|
||||
if (robot_emergency_stop_source != 0) {
|
||||
return SafetyCondition::RobotEmergencyStop;
|
||||
}
|
||||
return reported_condition;
|
||||
}
|
||||
|
||||
inline bool needsProtectiveUnlock(
|
||||
const SafetyCondition condition) noexcept
|
||||
{
|
||||
return condition == SafetyCondition::ProtectiveStop ||
|
||||
condition == SafetyCondition::Violation;
|
||||
}
|
||||
|
||||
inline bool needsInterfaceBoardRestart(
|
||||
const SafetyCondition condition) noexcept
|
||||
{
|
||||
return condition == SafetyCondition::SystemEmergencyStop ||
|
||||
condition == SafetyCondition::RobotEmergencyStop ||
|
||||
condition == SafetyCondition::Fault;
|
||||
}
|
||||
|
||||
struct SafetyPermit {
|
||||
std::uint64_t epoch{0};
|
||||
|
||||
bool valid() const noexcept { return epoch != 0; }
|
||||
};
|
||||
|
||||
struct RecoveryToken {
|
||||
std::uint64_t epoch{0};
|
||||
|
||||
bool valid() const noexcept { return epoch != 0; }
|
||||
};
|
||||
|
||||
struct SafetySnapshot {
|
||||
SafetyCondition observed{SafetyCondition::Unknown};
|
||||
SafetyCondition latched_reason{SafetyCondition::Unknown};
|
||||
std::uint64_t epoch{0};
|
||||
bool latched{false};
|
||||
bool recovery_in_progress{false};
|
||||
bool software_emergency_stop_latched{false};
|
||||
};
|
||||
|
||||
inline bool shouldAutoRecoverHardwareEmergencyStop(
|
||||
const SafetySnapshot& snapshot,
|
||||
const bool hardware_emergency_stop_was_observed,
|
||||
const int current_emergency_stop_source,
|
||||
const bool auto_power_on_enabled,
|
||||
const bool automatic_recovery_suppressed) noexcept
|
||||
{
|
||||
return auto_power_on_enabled && !automatic_recovery_suppressed &&
|
||||
hardware_emergency_stop_was_observed && snapshot.latched &&
|
||||
!snapshot.recovery_in_progress &&
|
||||
!snapshot.software_emergency_stop_latched &&
|
||||
isHardwareEmergencyStop(snapshot.latched_reason) &&
|
||||
isMotionSafe(snapshot.observed) &&
|
||||
current_emergency_stop_source == 0;
|
||||
}
|
||||
|
||||
// Hardware safety is an event, not a level. Once an unsafe state has been
|
||||
// observed, returning to Normal only changes the observed level. A separate,
|
||||
// explicit recovery must prove that the old controller operation has been
|
||||
// cancelled before new motion permits can be issued.
|
||||
class SafetyState final {
|
||||
public:
|
||||
SafetyState() = default;
|
||||
|
||||
void observe(const SafetyCondition condition)
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
const bool changed = observed_ != condition;
|
||||
observed_ = condition;
|
||||
if (condition == SafetyCondition::SoftwareEmergencyStop) {
|
||||
software_emergency_stop_latched_ = true;
|
||||
}
|
||||
if (isMotionSafe(condition)) {
|
||||
return;
|
||||
}
|
||||
|
||||
const bool preserve_hardware_estop_latch =
|
||||
condition == SafetyCondition::Unknown && latched_ &&
|
||||
isHardwareEmergencyStop(latched_reason_) &&
|
||||
!software_emergency_stop_latched_;
|
||||
if (!latched_ || recovery_in_progress_ ||
|
||||
(changed && !preserve_hardware_estop_latch)) {
|
||||
++epoch_;
|
||||
}
|
||||
latched_ = true;
|
||||
recovery_in_progress_ = false;
|
||||
// A physical E-stop sample can continue arriving after a software
|
||||
// E-stop request. Keep the software stop independently latched so a
|
||||
// later physical-input release can never clear it automatically.
|
||||
if (software_emergency_stop_latched_) {
|
||||
latched_reason_ = SafetyCondition::SoftwareEmergencyStop;
|
||||
} else if (!preserve_hardware_estop_latch) {
|
||||
latched_reason_ = condition;
|
||||
}
|
||||
}
|
||||
|
||||
std::optional<SafetyPermit> tryPermit() const
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
if (latched_ || !isMotionSafe(observed_)) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return SafetyPermit{epoch_};
|
||||
}
|
||||
|
||||
bool validate(const SafetyPermit permit) const
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
return permit.valid() && permit.epoch == epoch_ && !latched_ &&
|
||||
isMotionSafe(observed_);
|
||||
}
|
||||
|
||||
std::optional<RecoveryToken> beginRecovery(
|
||||
const std::uint64_t expected_epoch)
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ ||
|
||||
recovery_in_progress_ ||
|
||||
!isMotionSafe(observed_)) {
|
||||
return std::nullopt;
|
||||
}
|
||||
recovery_in_progress_ = true;
|
||||
return RecoveryToken{epoch_};
|
||||
}
|
||||
|
||||
bool completeRecovery(
|
||||
const RecoveryToken token,
|
||||
const bool robot_running,
|
||||
const bool controller_idle,
|
||||
const bool cancellation_confirmed)
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
if (!token.valid() || token.epoch != epoch_ || !latched_ ||
|
||||
!recovery_in_progress_ || !isMotionSafe(observed_) ||
|
||||
!robot_running || !controller_idle ||
|
||||
!cancellation_confirmed) {
|
||||
return false;
|
||||
}
|
||||
|
||||
latched_ = false;
|
||||
recovery_in_progress_ = false;
|
||||
latched_reason_ = SafetyCondition::Unknown;
|
||||
software_emergency_stop_latched_ = false;
|
||||
++epoch_;
|
||||
return true;
|
||||
}
|
||||
|
||||
// The caller may clear the physical E-stop latch only after it has powered
|
||||
// the controller, released the brakes, and then re-confirmed an empty,
|
||||
// steady controller in Running mode. This never authorizes replaying the
|
||||
// old target, runtime program, or servo session.
|
||||
bool completeHardwareEmergencyStopRecovery(
|
||||
const RecoveryToken token,
|
||||
const bool robot_running,
|
||||
const bool controller_idle,
|
||||
const bool cancellation_confirmed)
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
if (!token.valid() || token.epoch != epoch_ || !latched_ ||
|
||||
!recovery_in_progress_ ||
|
||||
software_emergency_stop_latched_ ||
|
||||
!isHardwareEmergencyStop(latched_reason_) ||
|
||||
!isMotionSafe(observed_) || !robot_running || !controller_idle ||
|
||||
!cancellation_confirmed) {
|
||||
return false;
|
||||
}
|
||||
|
||||
latched_ = false;
|
||||
recovery_in_progress_ = false;
|
||||
latched_reason_ = SafetyCondition::Unknown;
|
||||
++epoch_;
|
||||
return true;
|
||||
}
|
||||
|
||||
void failRecovery(const RecoveryToken token)
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
if (token.valid() && token.epoch == epoch_) {
|
||||
recovery_in_progress_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
SafetySnapshot snapshot() const
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
return {
|
||||
observed_,
|
||||
latched_reason_,
|
||||
epoch_,
|
||||
latched_,
|
||||
recovery_in_progress_,
|
||||
software_emergency_stop_latched_};
|
||||
}
|
||||
|
||||
private:
|
||||
mutable std::mutex mutex_;
|
||||
SafetyCondition observed_{SafetyCondition::Unknown};
|
||||
SafetyCondition latched_reason_{SafetyCondition::Unknown};
|
||||
std::uint64_t epoch_{1};
|
||||
bool latched_{false};
|
||||
bool recovery_in_progress_{false};
|
||||
bool software_emergency_stop_latched_{false};
|
||||
};
|
||||
|
||||
} // namespace cmvr::device::aubo_internal
|
||||
|
||||
#endif // CMVR_ES_AUBO_SAFETY_STATE_H
|
||||
@ -1,5 +1,7 @@
|
||||
add_library(huayan_arm SHARED huayan_arm.cpp)
|
||||
|
||||
find_package(Threads REQUIRED)
|
||||
|
||||
set(HUAYAN_ARM_SDK_DIR ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/huayan_arm/v1.0)
|
||||
|
||||
target_include_directories(huayan_arm
|
||||
@ -20,6 +22,7 @@ target_link_libraries(huayan_arm
|
||||
PRIVATE
|
||||
HR_Pro
|
||||
glog
|
||||
Threads::Threads
|
||||
)
|
||||
|
||||
add_library(cmvr_es::device::huayan_arm ALIAS huayan_arm)
|
||||
|
||||
23
cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt
Normal file
23
cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt
Normal file
@ -0,0 +1,23 @@
|
||||
add_library(ume_robot_arm SHARED
|
||||
src/damiao_mit_codec.cpp
|
||||
src/damiao_can_fd_chain.cpp
|
||||
src/ume_robot_arm.cpp
|
||||
)
|
||||
|
||||
target_include_directories(ume_robot_arm PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
|
||||
target_link_libraries(ume_robot_arm
|
||||
PUBLIC
|
||||
cmvr_es::device::canbus
|
||||
cmvr_es::ik_solver
|
||||
cmvr_es::common
|
||||
PRIVATE
|
||||
cmvr_es::proto
|
||||
cmvr_es::logging
|
||||
pthread
|
||||
)
|
||||
|
||||
add_library(cmvr_es::device::ume_robot_arm ALIAS ume_robot_arm)
|
||||
install(TARGETS ume_robot_arm LIBRARY DESTINATION lib)
|
||||
@ -1,205 +0,0 @@
|
||||
#include "common/base/logging/logger.h"
|
||||
#include "pcan_client.h"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
#include "cmvr/msgs/error_code.pb.h"
|
||||
#include "cstdio"
|
||||
|
||||
namespace cmvr {
|
||||
namespace device {
|
||||
#define CAN_ID_MASK 0x1FFFF800U // can_filter mask
|
||||
#define CAN_STANDARD_MAX_ID 0x7FFU
|
||||
|
||||
using cmvr::msgs::ErrorCode;
|
||||
using cmvr::msgs::CANCardParameter;
|
||||
PcanClient::PcanClient(const config::SocketCanConfig &cfg) {
|
||||
port_ = static_cast<CANCardParameter::CANChannelId>(cfg.channel_id());
|
||||
interface_ = CANCardParameter::NATIVE;
|
||||
enable_can_err_check_ = false;
|
||||
}
|
||||
|
||||
PcanClient::~PcanClient() {
|
||||
stop();
|
||||
}
|
||||
|
||||
bool PcanClient::init() {
|
||||
|
||||
|
||||
#define LPCSTR const char*
|
||||
/// <summary>
|
||||
/// Sets a TPCANDevice value. The input can be numeric, in hexadecimal or decimal format, or as string denoting
|
||||
/// a TPCANDevice value name.
|
||||
/// </summary>
|
||||
LPCSTR DeviceType = "PCAN_PCI";
|
||||
/// <summary>
|
||||
/// Sets value in range of a double. The input can be hexadecimal or decimal format.
|
||||
/// </summary>
|
||||
LPCSTR DeviceID = "";
|
||||
/// <summary>
|
||||
/// Sets a zero-based index value in range of a double. The input can be hexadecimal or decimal format.
|
||||
/// </summary>
|
||||
LPCSTR ControllerNumber = "";
|
||||
/// <summary>
|
||||
/// Sets a valid Internet Protocol address
|
||||
/// </summary>
|
||||
LPCSTR IPAddress = "";
|
||||
/// <summary>
|
||||
/// Sets a valid GUID for a PCAN device
|
||||
/// </summary>
|
||||
LPCSTR DeviceGUID = "";
|
||||
|
||||
|
||||
char sParameters[260];
|
||||
if (DeviceType != "")
|
||||
snprintf(sParameters, sizeof(sParameters), "%s=%s", LOOKUP_DEVICE_TYPE, DeviceType);
|
||||
if (DeviceID != "")
|
||||
{
|
||||
if (sParameters != "")
|
||||
snprintf(sParameters, sizeof(sParameters), "%s, ", sParameters);
|
||||
snprintf(sParameters, sizeof(sParameters), "%s%s=%s", sParameters, LOOKUP_DEVICE_ID, DeviceID);
|
||||
}
|
||||
if (ControllerNumber != "")
|
||||
{
|
||||
if (sParameters != "")
|
||||
snprintf(sParameters, sizeof(sParameters), "%s, ", sParameters);
|
||||
snprintf(sParameters, sizeof(sParameters), "%s%s=%s", sParameters, LOOKUP_CONTROLLER_NUMBER, ControllerNumber);
|
||||
}
|
||||
if (IPAddress != "")
|
||||
{
|
||||
if (sParameters != "")
|
||||
snprintf(sParameters, sizeof(sParameters), "%s, ", sParameters);
|
||||
snprintf(sParameters, sizeof(sParameters), "%s%s=%s", sParameters, LOOKUP_IP_ADDRESS, IPAddress);
|
||||
}
|
||||
|
||||
if (DeviceGUID != "")
|
||||
{
|
||||
if (sParameters != "")
|
||||
snprintf(sParameters, sizeof(sParameters), "%s, ", sParameters);
|
||||
snprintf(sParameters, sizeof(sParameters), "%s%s=%s", sParameters, LOOKUP_DEVICE_GUID, DeviceGUID);
|
||||
}
|
||||
|
||||
TPCANHandle handle;
|
||||
TPCANStatus stsResult = CAN_LookUpChannel((LPSTR)sParameters, &handle);
|
||||
TPCANStatus ret = CAN_Initialize(PcanHandle, Bitrate);
|
||||
if (ret != PCAN_ERROR_OK) {
|
||||
CMVR_LOG(ERROR) << "send message failed, error code: " << ret;
|
||||
return false;
|
||||
}
|
||||
|
||||
TPCANStatus state = CAN_GetStatus(PcanHandle);
|
||||
if (state & PCAN_ERROR_BUSOFF) {
|
||||
// 进入 bus-off，尝试复位总线
|
||||
CMVR_LOG(ERROR) << "send message failed, error code: " << ret;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PcanClient::start() {
|
||||
is_started_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PcanClient::stop() {
|
||||
if (is_started_) {
|
||||
is_started_ = false;
|
||||
CAN_Uninitialize(dev_handler_);
|
||||
CMVR_LOG(INFO) << "close socket can ok. port:" << port_;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
cmvr::msgs::ErrorCode PcanClient::send(const std::vector<CanFrame> &frames, int32_t *const frame_num) {
|
||||
if (frame_num == nullptr) {
|
||||
CMVR_LOG(FATAL) << "frame_num is null";
|
||||
}
|
||||
if (frames.size() != static_cast<size_t>(*frame_num)) {
|
||||
CMVR_LOG(FATAL) << "frames size does not match frame_num";
|
||||
}
|
||||
|
||||
if (!is_started_) {
|
||||
CMVR_LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!";
|
||||
return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
|
||||
}
|
||||
for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) {
|
||||
if (frames[i].len > CANBUS_MESSAGE_LENGTH || frames[i].len < 0) {
|
||||
CMVR_LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len
|
||||
<< ", which is not equal to can message data length ("
|
||||
<< CANBUS_MESSAGE_LENGTH << ").";
|
||||
return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
|
||||
}
|
||||
if (frames[i].id > CAN_STANDARD_MAX_ID) {
|
||||
send_frames_[i].ID = (frames[i].id & CAN_EFF_MASK) | CAN_EFF_FLAG;
|
||||
} else {
|
||||
send_frames_[i].ID = (frames[i].id & CAN_SFF_MASK);
|
||||
}
|
||||
// CMVR_LOG(INFO) << "send can id is " << send_frames_[i].can_id;
|
||||
send_frames_[i].LEN = frames[i].len;
|
||||
std::memcpy(send_frames_[i].DATA, frames[i].data, frames[i].len);
|
||||
send_frames_[i].MSGTYPE = PCAN_MESSAGE_EXTENDED;
|
||||
// Synchronous transmission of CAN messages
|
||||
TPCANStatus ret = CAN_Write(PcanHandle, &send_frames_[i]);
|
||||
if (ret != PCAN_ERROR_OK) {
|
||||
CMVR_LOG(ERROR) << "send message failed, error code: " << ret;
|
||||
return ErrorCode::CAN_CLIENT_ERROR_BASE;
|
||||
}
|
||||
}
|
||||
|
||||
return ErrorCode::OK;
|
||||
}
|
||||
|
||||
cmvr::msgs::ErrorCode PcanClient::receive(std::vector<CanFrame> *const frames, int32_t *const frame_num) {
|
||||
if (!is_started_) {
|
||||
CMVR_LOG(ERROR) << "Nvidia can client is not init! Please init first!";
|
||||
return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
|
||||
}
|
||||
|
||||
if (*frame_num > MAX_CAN_RECV_FRAME_LEN || *frame_num < 0) {
|
||||
CMVR_LOG(ERROR) << "recv can frame num not in range[0, " << MAX_CAN_RECV_FRAME_LEN
|
||||
<< "], frame_num:" << *frame_num;
|
||||
// TODO(Authors): check the difference of returning frame_num/error_code
|
||||
return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM;
|
||||
}
|
||||
TPCANTimestamp CANTimeStamp;
|
||||
for (int32_t i = 0; i < *frame_num && i < MAX_CAN_RECV_FRAME_LEN; ++i) {
|
||||
CanFrame cf;
|
||||
|
||||
// We execute the "Read" function of the PCANBasic
|
||||
TPCANStatus ret = CAN_Read(PcanHandle, &recv_frames_[i], &CANTimeStamp);
|
||||
if (ret != PCAN_ERROR_OK) {
|
||||
|
||||
CMVR_LOG(ERROR) << "receive message failed, error code: " << ret;
|
||||
return ErrorCode::CAN_CLIENT_ERROR_BASE;
|
||||
}
|
||||
if (recv_frames_[i].LEN > CANBUS_MESSAGE_LENGTH ||
|
||||
recv_frames_[i].LEN < 0) {
|
||||
CMVR_LOG(ERROR) << "recv_frames_[" << i
|
||||
<< "].can_dlc = " << recv_frames_[i].LEN
|
||||
<< ", which is not equal to can message data length ("
|
||||
<< CANBUS_MESSAGE_LENGTH << ").";
|
||||
return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
|
||||
}
|
||||
if (recv_frames_[i].ID > CAN_STANDARD_MAX_ID) {
|
||||
cf.id = enable_can_err_check_
|
||||
? recv_frames_[i].ID & CAN_EFF_MASK | CAN_ERR_FLAG
|
||||
: recv_frames_[i].ID & CAN_EFF_MASK;
|
||||
} else {
|
||||
cf.id = (recv_frames_[i].ID & CAN_SFF_MASK);
|
||||
}
|
||||
CMVR_LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].ID;
|
||||
cf.len = recv_frames_[i].LEN;
|
||||
std::memcpy(cf.data, recv_frames_[i].DATA, recv_frames_[i].LEN);
|
||||
frames->push_back(cf);
|
||||
}
|
||||
return ErrorCode::OK;
|
||||
}
|
||||
|
||||
std::string PcanClient::getErrorString(const int32_t status) {
|
||||
return "";
|
||||
}
|
||||
} // namespace device
|
||||
} // namespace cmvr
|
||||
@ -1,86 +0,0 @@
|
||||
//
|
||||
// Created by lgv on 2025/8/7.
|
||||
//
|
||||
|
||||
/**
|
||||
* @file
|
||||
* @brief Defines the SocketCanClientRaw class which inherits AbstractCanbus.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <unistd.h>
|
||||
|
||||
#include <net/if.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cmvr/msgs/error_code.pb.h"
|
||||
#include "cmvr/msgs/can_card_parameter.pb.h"
|
||||
#include "cmvr/config/motor_config/motor_config.pb.h"
|
||||
|
||||
#include "gflags/gflags.h"
|
||||
#include "../../abstract_canbus.h"
|
||||
#include "canbus/common/canbus_consts.h"
|
||||
|
||||
#include <PCANBasic.h>
|
||||
|
||||
namespace cmvr {
|
||||
namespace device {
|
||||
|
||||
class PcanClient final : public AbstractCanbus {
|
||||
public:
|
||||
explicit PcanClient(const config::SocketCanConfig &cfg);
|
||||
~PcanClient();
|
||||
std::string typeName() const override { return "PcanClient"; }
|
||||
bool init() override;
|
||||
bool start() override;
|
||||
bool stop() override;
|
||||
|
||||
/**
|
||||
* @brief Send messages
|
||||
* @param frames The messages to send.
|
||||
* @param frame_num The amount of messages to send.
|
||||
* @return The status of the sending action
|
||||
*/
|
||||
cmvr::msgs::ErrorCode send(const std::vector<CanFrame> &frames,
|
||||
int32_t *const frame_num) override;
|
||||
|
||||
/**
|
||||
* @brief Receive messages
|
||||
* @param frames The messages to receive.
|
||||
* @param frame_num The amount of messages to receive.
|
||||
* @return The status of the receiving action
|
||||
*/
|
||||
cmvr::msgs::ErrorCode receive(std::vector<CanFrame> *const frames,
|
||||
int32_t *const frame_num) override;
|
||||
|
||||
/**
|
||||
* @brief Get the error string.
|
||||
* @param status The status to get the error string.
|
||||
*/
|
||||
std::string getErrorString(const int32_t status) override;
|
||||
private:
|
||||
int dev_handler_ = 0;
|
||||
cmvr::msgs::CANCardParameter::CANChannelId port_;
|
||||
cmvr::msgs::CANCardParameter::CANInterface interface_;
|
||||
TPCANMsg send_frames_[MAX_CAN_SEND_FRAME_LEN];
|
||||
TPCANMsg recv_frames_[MAX_CAN_RECV_FRAME_LEN];
|
||||
|
||||
|
||||
//
|
||||
bool enable_can_err_check_{false};
|
||||
const TPCANHandle PcanHandle = PCAN_USBBUS1;
|
||||
const TPCANBaudrate Bitrate = PCAN_BAUD_1M;
|
||||
};
|
||||
}
|
||||
}
|
||||
@ -1,46 +0,0 @@
|
||||
#include "common/base/logging/logger.h"
|
||||
//
|
||||
// Created by lgv on 2025/8/7.
|
||||
//
|
||||
#include "cmvr/msgs/error_code.pb.h"
|
||||
#include "cmvr/msgs/can_card_parameter.pb.h"
|
||||
#include "canbus/can_client/pcan/pcan_client.h"
|
||||
#include "gtest/gtest.h"
|
||||
namespace cmvr {
|
||||
namespace device {
|
||||
using cmvr::msgs::ErrorCode;
|
||||
using cmvr::msgs::CANCardParameter;
|
||||
|
||||
TEST(PcanClienTest, simple_test) {
|
||||
CANCardParameter param;
|
||||
param.set_brand(CANCardParameter::SOCKET_CAN_RAW);
|
||||
param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO);
|
||||
|
||||
config::SocketCanConfig cfg;
|
||||
cfg.set_channel_id(0);
|
||||
PcanClient socket_can_client(cfg);
|
||||
|
||||
socket_can_client.init();
|
||||
|
||||
// EXPECT_EQ(socket_can_client.start(), ErrorCode::CAN_CLIENT_ERROR_BASE);
|
||||
socket_can_client.start();
|
||||
std::vector<CanFrame> frames;
|
||||
// int32_t num = 0;
|
||||
// EXPECT_EQ(socket_can_client.send(frames, &num),
|
||||
// ErrorCode::OK);
|
||||
// ++num;
|
||||
// EXPECT_EQ(socket_can_client.receive(&frames, &num),
|
||||
// ErrorCode::OK);
|
||||
// CMVR_LOG(INFO) << frames.at(0).CanFrameString();
|
||||
CanFrame can_frame;
|
||||
can_frame.id = 0x123;
|
||||
can_frame.len = 8;
|
||||
memset(can_frame.data, 0xA3, sizeof(can_frame.data));
|
||||
frames.clear();
|
||||
frames.push_back(can_frame);
|
||||
EXPECT_EQ(socket_can_client.sendSingleFrame(frames),
|
||||
ErrorCode::OK);
|
||||
socket_can_client.stop();
|
||||
}
|
||||
}
|
||||
}
|
||||
36
cmvr-es/devices/motor/README.md
Normal file
36
cmvr-es/devices/motor/README.md
Normal file
@ -0,0 +1,36 @@
|
||||
# Motor 设备模块
|
||||
|
||||
`devices/motor/` 提供电机管理、协议适配、总线 runtime 和厂商驱动。Service、
|
||||
RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口，不应
|
||||
直接访问 CAN、EtherCAT、MuJoCo 或厂商 SDK。
|
||||
|
||||
返回 [Devices 模块指南](../README.md) 或 [项目总览](../../../README.md)。
|
||||
|
||||
## 目录职责
|
||||
|
||||
| 目录 | 职责 |
|
||||
| --- | --- |
|
||||
| `manager/` | 创建 MotorGroup，按 `motor_id`/`joint_name` 暴露 `AbstractMotor` |
|
||||
| `bus_runtime/` | 连接、收发和总线生命周期 |
|
||||
| `drivers/` | CANopen、EtherCAT 和 MuJoCo 等具体后端 |
|
||||
|
||||
## 当前后端
|
||||
|
||||
- CAN + TI5 CANopen；
|
||||
- EtherCAT + EYOU CiA 402；
|
||||
- MuJoCo 仿真电机。
|
||||
|
||||
配置示例：
|
||||
|
||||
- [`ti5_motors.pb.txt`](../../config/devices/motor/ti5_motors.pb.txt)
|
||||
- [`ethercat_motors.pb.txt`](../../config/devices/motor/ethercat_motors.pb.txt)
|
||||
- [`mujoco_motors.pb.txt`](../../config/devices/motor/mujoco_motors.pb.txt)
|
||||
|
||||
对外接口与控制权语义见 [MotorService 文档](../../service/README.md#motorservice)。
|
||||
|
||||
## 安全边界
|
||||
|
||||
- MotorService 是单轴 API，不提供多轴同扫描周期的原子 commit；
|
||||
- 软件 `emergencyStop` 和 Quick Stop 不具备功能安全等级；
|
||||
- 真实设备必须具有经风险评估确定的硬接线急停、安全继电器和驱动器安全链；
|
||||
- 新硬件配置保持 `enable: false`，完成方向、限位和故障注入验证后才能启用。
|
||||
@ -6,13 +6,20 @@
|
||||
|
||||
## 当前管理器
|
||||
|
||||
管理模块目录统一使用 `*_manager` 后缀，主管理类使用 `*Manager` 后缀。工厂、适配器、
|
||||
账本、快照和结果结构体属于管理器内部的支撑类型，保留其职责名称，不强行改成
|
||||
`*Manager`。
|
||||
|
||||
| 目录 | CMake target | 职责 |
|
||||
| --- | --- | --- |
|
||||
| [`control_authority_manager/`](control_authority_manager/) | `cmvr_es::control_authority_manager` | 控制权租约、代际、dispatch fence 和 quarantine |
|
||||
| [`device_manager/`](device_manager/) | `cmvr_es::device_manager` | 按配置创建、初始化、查询和批量启停设备 |
|
||||
| [`safety_manager/`](safety_manager/) | `cmvr_es::safety_manager` | Sensor/Control 安全准入、StopAll、恢复和命令账本 |
|
||||
| [`task_manager/`](task_manager/) | `cmvr_es::task_manager` | 创建任务、校验运行模式、统一启停和调度周期任务 |
|
||||
| [`media_source_hub/`](media_source_hub/) | `cmvr_es::media_source_hub`、`cmvr_es::device_media_source_adapter` | 实时媒体源注册、按需启停和多消费者分发 |
|
||||
| [`media_source_manager/`](media_source_manager/) | `cmvr_es::media_source_manager`、`cmvr_es::device_media_source_adapter` | 实时媒体源注册、按需启停和多消费者分发 |
|
||||
|
||||
`manager/` 当前没有聚合 `CMakeLists.txt`，三个子目录由 [`../CMakeLists.txt`](../CMakeLists.txt) 分别加入。新增 manager 时必须显式更新该文件。
|
||||
`manager/` 当前没有聚合 `CMakeLists.txt`，所有模块由 [`../CMakeLists.txt`](../CMakeLists.txt)
|
||||
按依赖顺序加入。新增 manager 时必须同时更新目录、target、依赖顺序和本 README。
|
||||
|
||||
## 进程生命周期
|
||||
|
||||
@ -30,7 +37,8 @@
|
||||
|
||||
- DeviceManager 构造不会自动调用全部设备的 `start()`；
|
||||
- 当前主退出路径没有调用 `DeviceManager::stop()`；
|
||||
- `SystemService/StopAll` 会调用 DeviceManager stop；
|
||||
- `SystemService/StopAll` 只停止当前运动、控制和媒体活动，不调用
|
||||
`DeviceManager::stop()`，成功返回后可继续接受新命令；
|
||||
- `DeviceManager::destroyInstance()` 不调用设备 stop，销毁前必须先显式停止；
|
||||
- `TaskManager::destroyInstance()` 会调用 `stopRunTask()`，但 manager 未处于 running 状态时该调用会直接返回；
|
||||
- DeviceManager 和 TaskManager 都是首次配置生效的单例，不支持热加载。
|
||||
@ -135,12 +143,12 @@
|
||||
- 有顺序依赖的工作应放入同一协调任务或显式建模；
|
||||
- task 返回后，其内部状态并发安全由具体实现负责。
|
||||
|
||||
## MediaSourceHub
|
||||
## MediaSourceManager
|
||||
|
||||
关键文件：
|
||||
|
||||
- [`media_source_hub/include/media_source_hub.h`](media_source_hub/include/media_source_hub.h)
|
||||
- [`media_source_hub/src/device_media_source_adapter.cpp`](media_source_hub/src/device_media_source_adapter.cpp)
|
||||
- [`media_source_manager/include/media_source_manager.h`](media_source_manager/include/media_source_manager.h)
|
||||
- [`media_source_manager/src/device_media_source_adapter.cpp`](media_source_manager/src/device_media_source_adapter.cpp)
|
||||
- [`../common/media/media_frame.h`](../common/media/media_frame.h)
|
||||
- [`../common/base/ring_buffer.h`](../common/base/ring_buffer.h)
|
||||
|
||||
@ -151,7 +159,8 @@
|
||||
| 摄像头彩色流 | `<device_id>/video/color` | 64 |
|
||||
| 麦克风主流 | `<device_id>/audio/main` | 256 |
|
||||
|
||||
当前 gRPC RGB/麦克风流和 QUIC 彩色/麦克风轨道使用 Hub；gRPC Depth/RGBD 仍直接读取设备帧。
|
||||
当前 gRPC RGB/麦克风流和 QUIC 彩色/麦克风轨道使用 MediaSourceManager；gRPC Depth/RGBD
|
||||
仍直接读取设备帧。
|
||||
|
||||
### 注册新媒体源
|
||||
|
||||
@ -210,7 +219,7 @@ ring generation 不等于 `TrackDescriptor::generation`，ring 的 `ReadResult.s
|
||||
|
||||
## 新增第四种 Manager
|
||||
|
||||
1. 先确认能力不是 DeviceManager、TaskManager 或 MediaSourceHub 的子职责；
|
||||
1. 先确认能力不是 DeviceManager、TaskManager 或 MediaSourceManager 的子职责；
|
||||
2. 定义所有权、初始化、start/stop 和线程模型；
|
||||
3. 避免新增无必要的全局单例；
|
||||
4. 新建独立目录、头文件、实现和 CMake target；
|
||||
@ -220,16 +229,6 @@ ring generation 不等于 `TrackDescriptor::generation`，ring 的 `ReadResult.s
|
||||
|
||||
## 测试
|
||||
|
||||
MediaSourceHub：
|
||||
|
||||
```bash
|
||||
cmake --build build --target media_source_hub_test
|
||||
ctest \
|
||||
--test-dir build \
|
||||
-R '^media_source_hub_test$' \
|
||||
--output-on-failure
|
||||
```
|
||||
|
||||
DeviceManager 已有 `device_manager_snapshot_test`，覆盖全量状态表、生命周期失败、
|
||||
异常限长、排序和值快照并发读取。TaskManager 仍缺少独立 CTest。修改其行为时
|
||||
至少补充：
|
||||
|
||||
14
cmvr-es/manager/control_authority_manager/CMakeLists.txt
Normal file
14
cmvr-es/manager/control_authority_manager/CMakeLists.txt
Normal file
@ -0,0 +1,14 @@
|
||||
add_library(control_authority_manager STATIC
|
||||
src/control_authority_manager.cpp
|
||||
)
|
||||
target_compile_features(control_authority_manager PUBLIC cxx_std_17)
|
||||
target_include_directories(control_authority_manager
|
||||
PUBLIC
|
||||
${PROJECT_SOURCE_DIR}/cmvr-es
|
||||
)
|
||||
|
||||
add_library(
|
||||
cmvr_es::control_authority_manager
|
||||
ALIAS control_authority_manager
|
||||
)
|
||||
install(TARGETS control_authority_manager ARCHIVE DESTINATION lib)
|
||||
@ -0,0 +1,170 @@
|
||||
#ifndef CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
|
||||
#define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
|
||||
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
|
||||
namespace cmvr::control {
|
||||
|
||||
struct ControlLeaseToken {
|
||||
std::string resource_id;
|
||||
std::string owner_id;
|
||||
std::uint64_t generation{0};
|
||||
|
||||
bool valid() const noexcept
|
||||
{
|
||||
return !resource_id.empty() &&
|
||||
!owner_id.empty() &&
|
||||
generation != 0U;
|
||||
}
|
||||
};
|
||||
|
||||
struct ControlAcquireResult {
|
||||
bool acquired{false};
|
||||
ControlLeaseToken token;
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
class ControlDispatchGuard;
|
||||
|
||||
// Process-wide, transport-independent control ownership. The generation in a
|
||||
// token prevents a delayed release from an old network session from releasing
|
||||
// a newer lease on the same arm.
|
||||
class ControlAuthorityManager {
|
||||
public:
|
||||
using Duration = std::chrono::milliseconds;
|
||||
|
||||
static ControlAuthorityManager& instance();
|
||||
|
||||
ControlAcquireResult tryAcquire(
|
||||
const std::string& resource_id,
|
||||
const std::string& owner_id,
|
||||
Duration ttl);
|
||||
|
||||
// Atomically invalidates a normal control lease and joins a safety
|
||||
// barrier. Each safety caller receives an independent token; normal
|
||||
// control remains blocked until the last safety token is released.
|
||||
ControlAcquireResult preemptAcquire(
|
||||
const std::string& resource_id,
|
||||
const std::string& owner_id,
|
||||
Duration ttl);
|
||||
|
||||
// Converts the expected normal lease into a safety barrier only while it
|
||||
// is still the current lease. A stale token never preempts a successor or
|
||||
// joins an existing safety barrier.
|
||||
ControlAcquireResult preemptAcquireIfCurrent(
|
||||
const ControlLeaseToken& expected_token,
|
||||
const std::string& owner_id,
|
||||
Duration ttl);
|
||||
|
||||
// Waits for normal lease handlers displaced by the current safety barrier
|
||||
// to release their tokens and for their in-flight dispatches to finish.
|
||||
// Returns false on timeout or when safety_token is no longer a holder of
|
||||
// the current entry.
|
||||
bool waitForPreemptedRelease(
|
||||
const ControlLeaseToken& safety_token,
|
||||
Duration timeout);
|
||||
|
||||
// Permanently blocks the resource only if the expected normal lease is
|
||||
// still current. A later safety holder may clear this fail-closed state
|
||||
// only after it has independently confirmed the preempted handler exited.
|
||||
bool quarantineIfCurrent(
|
||||
const ControlLeaseToken& expected_token) noexcept;
|
||||
|
||||
// Abandons a safety token while retaining its barrier. A retired token can
|
||||
// no longer be validated, released, or used as a recovery authority. This
|
||||
// lets a failed stop path discard local token ownership without silently
|
||||
// reopening the resource.
|
||||
bool retireSafetyHolder(
|
||||
const ControlLeaseToken& safety_token) noexcept;
|
||||
|
||||
// Clears retired safety holders and a tokenless quarantine after a newer,
|
||||
// active safety holder has confirmed every preempted normal handler has
|
||||
// exited. Other active safety holders are deliberately preserved.
|
||||
bool recoverRetiredSafetyHolders(
|
||||
const ControlLeaseToken& recovery_token) noexcept;
|
||||
|
||||
bool renew(const ControlLeaseToken& token, Duration ttl);
|
||||
bool validate(const ControlLeaseToken& token);
|
||||
|
||||
// Use only around a bounded device-command submission. Never retain this
|
||||
// guard while waiting for physical motion or another long-running task.
|
||||
ControlDispatchGuard tryBeginDispatch(
|
||||
const ControlLeaseToken& token);
|
||||
void release(const ControlLeaseToken& token) noexcept;
|
||||
|
||||
// Safety/control paths which do not possess a lease use this query to
|
||||
// reject mutating commands. Read-only state and stop/torque-off commands
|
||||
// are intentionally allowed by their callers.
|
||||
bool isLeased(const std::string& resource_id);
|
||||
void revoke(const std::string& resource_id) noexcept;
|
||||
|
||||
// Test/process teardown hook. Runtime code should release/revoke exact
|
||||
// resources instead of clearing unrelated ownership.
|
||||
void clear() noexcept;
|
||||
|
||||
private:
|
||||
friend class ControlDispatchGuard;
|
||||
|
||||
struct SafetyHolder {
|
||||
std::string owner_id;
|
||||
bool retired{false};
|
||||
};
|
||||
|
||||
struct Entry;
|
||||
|
||||
static void quarantine_(Entry& entry) noexcept;
|
||||
bool expired_(const Entry& entry) const noexcept;
|
||||
static bool isSafetyHolder_(
|
||||
const Entry& entry,
|
||||
const ControlLeaseToken& token) noexcept;
|
||||
static bool isActiveSafetyHolder_(
|
||||
const Entry& entry,
|
||||
const ControlLeaseToken& token) noexcept;
|
||||
static bool canErase_(const Entry& entry) noexcept;
|
||||
static void invalidateToDispatchFence_(Entry& entry) noexcept;
|
||||
void endDispatch_(const std::shared_ptr<Entry>& entry) noexcept;
|
||||
|
||||
std::mutex mutex_;
|
||||
std::condition_variable release_cv_;
|
||||
std::unordered_map<std::string, std::shared_ptr<Entry>> entries_;
|
||||
std::uint64_t next_generation_{0};
|
||||
};
|
||||
|
||||
// Tracks one bounded backend dispatch without retaining the process-wide
|
||||
// authority lock. Safety preemption invalidates the lease immediately, while
|
||||
// waitForPreemptedRelease() joins both the displaced handler and its in-flight
|
||||
// dispatches before the safety operation reaches the device.
|
||||
class ControlDispatchGuard final {
|
||||
public:
|
||||
ControlDispatchGuard() noexcept = default;
|
||||
~ControlDispatchGuard() noexcept;
|
||||
|
||||
ControlDispatchGuard(ControlDispatchGuard&& other) noexcept;
|
||||
ControlDispatchGuard& operator=(ControlDispatchGuard&& other) noexcept;
|
||||
|
||||
ControlDispatchGuard(const ControlDispatchGuard&) = delete;
|
||||
ControlDispatchGuard& operator=(const ControlDispatchGuard&) = delete;
|
||||
|
||||
bool acquired() const noexcept { return entry_ != nullptr; }
|
||||
|
||||
private:
|
||||
friend class ControlAuthorityManager;
|
||||
|
||||
ControlDispatchGuard(
|
||||
ControlAuthorityManager* manager,
|
||||
std::shared_ptr<ControlAuthorityManager::Entry> entry) noexcept;
|
||||
void reset_() noexcept;
|
||||
|
||||
ControlAuthorityManager* manager_{nullptr};
|
||||
std::shared_ptr<ControlAuthorityManager::Entry> entry_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::control
|
||||
|
||||
#endif // CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
|
||||
@ -0,0 +1,682 @@
|
||||
#include "manager/control_authority_manager/include/control_authority_manager.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
namespace cmvr::control {
|
||||
|
||||
struct ControlAuthorityManager::Entry {
|
||||
std::string resource_id;
|
||||
std::string owner_id;
|
||||
std::uint64_t generation{0};
|
||||
std::chrono::steady_clock::time_point deadline;
|
||||
bool preemptible{true};
|
||||
bool quarantined{false};
|
||||
bool quarantined_normal_pending{false};
|
||||
bool dispatch_fence_only{false};
|
||||
std::uint64_t in_flight_dispatches{0};
|
||||
std::unordered_map<std::uint64_t, SafetyHolder> safety_holders;
|
||||
std::unordered_map<std::uint64_t, std::string>
|
||||
preempted_normal_holders;
|
||||
};
|
||||
|
||||
ControlDispatchGuard::ControlDispatchGuard(
|
||||
ControlAuthorityManager* const manager,
|
||||
std::shared_ptr<ControlAuthorityManager::Entry> entry) noexcept
|
||||
: manager_(manager),
|
||||
entry_(std::move(entry))
|
||||
{
|
||||
}
|
||||
|
||||
ControlDispatchGuard::~ControlDispatchGuard() noexcept
|
||||
{
|
||||
reset_();
|
||||
}
|
||||
|
||||
ControlDispatchGuard::ControlDispatchGuard(
|
||||
ControlDispatchGuard&& other) noexcept
|
||||
: manager_(std::exchange(other.manager_, nullptr)),
|
||||
entry_(std::move(other.entry_))
|
||||
{
|
||||
}
|
||||
|
||||
ControlDispatchGuard& ControlDispatchGuard::operator=(
|
||||
ControlDispatchGuard&& other) noexcept
|
||||
{
|
||||
if (this != &other) {
|
||||
reset_();
|
||||
manager_ = std::exchange(other.manager_, nullptr);
|
||||
entry_ = std::move(other.entry_);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
void ControlDispatchGuard::reset_() noexcept
|
||||
{
|
||||
if (entry_ == nullptr) {
|
||||
return;
|
||||
}
|
||||
auto entry = std::move(entry_);
|
||||
auto* const manager = std::exchange(manager_, nullptr);
|
||||
manager->endDispatch_(entry);
|
||||
}
|
||||
|
||||
ControlAuthorityManager& ControlAuthorityManager::instance()
|
||||
{
|
||||
static ControlAuthorityManager manager;
|
||||
return manager;
|
||||
}
|
||||
|
||||
ControlAcquireResult ControlAuthorityManager::tryAcquire(
|
||||
const std::string& resource_id,
|
||||
const std::string& owner_id,
|
||||
const Duration ttl)
|
||||
{
|
||||
if (resource_id.empty() || owner_id.empty() ||
|
||||
ttl <= Duration::zero()) {
|
||||
return {false, {}, "invalid control lease request"};
|
||||
}
|
||||
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto existing = entries_.find(resource_id);
|
||||
if (existing != entries_.end()) {
|
||||
auto& entry = *existing->second;
|
||||
if (!expired_(entry)) {
|
||||
if (entry.dispatch_fence_only) {
|
||||
return {
|
||||
false,
|
||||
{},
|
||||
"control resource still has an in-flight dispatch"};
|
||||
}
|
||||
return {
|
||||
false,
|
||||
{},
|
||||
"control resource is already leased by " +
|
||||
entry.owner_id};
|
||||
}
|
||||
if (entry.in_flight_dispatches != 0U) {
|
||||
invalidateToDispatchFence_(entry);
|
||||
return {
|
||||
false,
|
||||
{},
|
||||
"control resource still has an in-flight dispatch"};
|
||||
}
|
||||
entries_.erase(existing);
|
||||
}
|
||||
|
||||
ControlLeaseToken token;
|
||||
token.resource_id = resource_id;
|
||||
token.owner_id = owner_id;
|
||||
token.generation = ++next_generation_;
|
||||
|
||||
auto entry = std::make_shared<Entry>();
|
||||
entry->resource_id = resource_id;
|
||||
entry->owner_id = owner_id;
|
||||
entry->generation = token.generation;
|
||||
entry->deadline = std::chrono::steady_clock::now() + ttl;
|
||||
entries_.emplace(resource_id, std::move(entry));
|
||||
return {true, std::move(token), {}};
|
||||
}
|
||||
|
||||
ControlAcquireResult ControlAuthorityManager::preemptAcquire(
|
||||
const std::string& resource_id,
|
||||
const std::string& owner_id,
|
||||
const Duration ttl)
|
||||
{
|
||||
if (resource_id.empty() || owner_id.empty() ||
|
||||
ttl <= Duration::zero()) {
|
||||
return {false, {}, "invalid control barrier request"};
|
||||
}
|
||||
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto existing = entries_.find(resource_id);
|
||||
if (existing != entries_.end() &&
|
||||
!existing->second->preemptible &&
|
||||
!existing->second->dispatch_fence_only) {
|
||||
ControlLeaseToken token;
|
||||
token.resource_id = resource_id;
|
||||
token.owner_id = owner_id;
|
||||
token.generation = ++next_generation_;
|
||||
existing->second->safety_holders.emplace(
|
||||
token.generation,
|
||||
SafetyHolder{token.owner_id, false});
|
||||
return {true, std::move(token), {}};
|
||||
}
|
||||
|
||||
const bool replacing_normal =
|
||||
existing != entries_.end() && existing->second->preemptible;
|
||||
try {
|
||||
ControlLeaseToken token;
|
||||
token.resource_id = resource_id;
|
||||
token.owner_id = owner_id;
|
||||
token.generation = ++next_generation_;
|
||||
|
||||
std::unordered_map<std::uint64_t, SafetyHolder> safety_holders;
|
||||
safety_holders.emplace(
|
||||
token.generation,
|
||||
SafetyHolder{owner_id, false});
|
||||
std::string safety_owner = owner_id;
|
||||
std::unordered_map<std::uint64_t, std::string>
|
||||
preempted_normal_holders;
|
||||
if (replacing_normal) {
|
||||
preempted_normal_holders.emplace(
|
||||
existing->second->generation,
|
||||
existing->second->owner_id);
|
||||
}
|
||||
|
||||
std::shared_ptr<Entry> entry;
|
||||
if (existing == entries_.end()) {
|
||||
entry = std::make_shared<Entry>();
|
||||
entry->resource_id = resource_id;
|
||||
entry->owner_id.swap(safety_owner);
|
||||
entry->generation = token.generation;
|
||||
entry->deadline =
|
||||
std::chrono::steady_clock::time_point::max();
|
||||
entry->preemptible = false;
|
||||
entry->safety_holders.swap(safety_holders);
|
||||
entry->preempted_normal_holders.swap(
|
||||
preempted_normal_holders);
|
||||
entries_.emplace(resource_id, entry);
|
||||
} else {
|
||||
entry = existing->second;
|
||||
entry->owner_id.swap(safety_owner);
|
||||
entry->generation = token.generation;
|
||||
entry->deadline =
|
||||
std::chrono::steady_clock::time_point::max();
|
||||
entry->preemptible = false;
|
||||
entry->quarantined = false;
|
||||
entry->quarantined_normal_pending = false;
|
||||
entry->dispatch_fence_only = false;
|
||||
entry->safety_holders.swap(safety_holders);
|
||||
entry->preempted_normal_holders.swap(
|
||||
preempted_normal_holders);
|
||||
}
|
||||
return {true, std::move(token), {}};
|
||||
} catch (...) {
|
||||
if (replacing_normal && existing->second->preemptible) {
|
||||
quarantine_(*existing->second);
|
||||
}
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent(
|
||||
const ControlLeaseToken& expected_token,
|
||||
const std::string& owner_id,
|
||||
const Duration ttl)
|
||||
{
|
||||
if (!expected_token.valid() || owner_id.empty() ||
|
||||
ttl <= Duration::zero()) {
|
||||
return {false, {}, "invalid conditional control barrier request"};
|
||||
}
|
||||
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto existing = entries_.find(expected_token.resource_id);
|
||||
if (existing == entries_.end() ||
|
||||
expired_(*existing->second) ||
|
||||
!existing->second->preemptible ||
|
||||
existing->second->owner_id != expected_token.owner_id ||
|
||||
existing->second->generation != expected_token.generation) {
|
||||
return {
|
||||
false,
|
||||
{},
|
||||
"expected control lease is no longer current"};
|
||||
}
|
||||
|
||||
try {
|
||||
ControlLeaseToken token;
|
||||
token.resource_id = expected_token.resource_id;
|
||||
token.owner_id = owner_id;
|
||||
token.generation = ++next_generation_;
|
||||
|
||||
std::unordered_map<std::uint64_t, SafetyHolder> safety_holders;
|
||||
safety_holders.emplace(
|
||||
token.generation,
|
||||
SafetyHolder{owner_id, false});
|
||||
std::string safety_owner = owner_id;
|
||||
std::unordered_map<std::uint64_t, std::string>
|
||||
preempted_normal_holders;
|
||||
preempted_normal_holders.emplace(
|
||||
existing->second->generation,
|
||||
existing->second->owner_id);
|
||||
|
||||
auto& entry = *existing->second;
|
||||
entry.owner_id.swap(safety_owner);
|
||||
entry.generation = token.generation;
|
||||
entry.deadline = std::chrono::steady_clock::time_point::max();
|
||||
entry.preemptible = false;
|
||||
entry.quarantined = false;
|
||||
entry.quarantined_normal_pending = false;
|
||||
entry.dispatch_fence_only = false;
|
||||
entry.safety_holders.swap(safety_holders);
|
||||
entry.preempted_normal_holders.swap(
|
||||
preempted_normal_holders);
|
||||
return {true, std::move(token), {}};
|
||||
} catch (...) {
|
||||
if (existing->second->preemptible) {
|
||||
quarantine_(*existing->second);
|
||||
}
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::waitForPreemptedRelease(
|
||||
const ControlLeaseToken& safety_token,
|
||||
const Duration timeout)
|
||||
{
|
||||
if (!safety_token.valid() || timeout < Duration::zero()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::unique_lock lock(mutex_);
|
||||
const auto currentState = [this, &safety_token]() {
|
||||
const auto found = entries_.find(safety_token.resource_id);
|
||||
if (found == entries_.end() ||
|
||||
!isActiveSafetyHolder_(*found->second, safety_token)) {
|
||||
return -1;
|
||||
}
|
||||
return !found->second->quarantined_normal_pending &&
|
||||
found->second->preempted_normal_holders.empty() &&
|
||||
found->second->in_flight_dispatches == 0U
|
||||
? 1
|
||||
: 0;
|
||||
};
|
||||
|
||||
if (currentState() < 0) {
|
||||
return false;
|
||||
}
|
||||
release_cv_.wait_for(
|
||||
lock,
|
||||
timeout,
|
||||
[¤tState]() { return currentState() != 0; });
|
||||
return currentState() == 1;
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::quarantineIfCurrent(
|
||||
const ControlLeaseToken& expected_token) noexcept
|
||||
{
|
||||
if (!expected_token.valid()) {
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto existing = entries_.find(expected_token.resource_id);
|
||||
if (existing == entries_.end() ||
|
||||
expired_(*existing->second) ||
|
||||
!existing->second->preemptible ||
|
||||
existing->second->owner_id != expected_token.owner_id ||
|
||||
existing->second->generation != expected_token.generation) {
|
||||
return false;
|
||||
}
|
||||
quarantine_(*existing->second);
|
||||
return true;
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::retireSafetyHolder(
|
||||
const ControlLeaseToken& safety_token) noexcept
|
||||
{
|
||||
if (!safety_token.valid()) {
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto existing = entries_.find(safety_token.resource_id);
|
||||
if (existing == entries_.end() ||
|
||||
existing->second->preemptible ||
|
||||
existing->second->dispatch_fence_only) {
|
||||
return false;
|
||||
}
|
||||
const auto holder = existing->second->safety_holders.find(
|
||||
safety_token.generation);
|
||||
if (holder == existing->second->safety_holders.end() ||
|
||||
holder->second.owner_id != safety_token.owner_id) {
|
||||
return false;
|
||||
}
|
||||
holder->second.retired = true;
|
||||
release_cv_.notify_all();
|
||||
return true;
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::recoverRetiredSafetyHolders(
|
||||
const ControlLeaseToken& recovery_token) noexcept
|
||||
{
|
||||
if (!recovery_token.valid()) {
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto existing = entries_.find(recovery_token.resource_id);
|
||||
if (existing == entries_.end() ||
|
||||
!isActiveSafetyHolder_(*existing->second, recovery_token) ||
|
||||
existing->second->quarantined_normal_pending ||
|
||||
!existing->second->preempted_normal_holders.empty() ||
|
||||
existing->second->in_flight_dispatches != 0U) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (auto holder = existing->second->safety_holders.begin();
|
||||
holder != existing->second->safety_holders.end();) {
|
||||
if (holder->second.retired) {
|
||||
holder = existing->second->safety_holders.erase(holder);
|
||||
} else {
|
||||
++holder;
|
||||
}
|
||||
}
|
||||
existing->second->quarantined = false;
|
||||
release_cv_.notify_all();
|
||||
return true;
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::renew(
|
||||
const ControlLeaseToken& token,
|
||||
const Duration ttl)
|
||||
{
|
||||
if (!token.valid() || ttl <= Duration::zero()) {
|
||||
return false;
|
||||
}
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto found = entries_.find(token.resource_id);
|
||||
if (found == entries_.end()) {
|
||||
return false;
|
||||
}
|
||||
if (expired_(*found->second)) {
|
||||
if (found->second->in_flight_dispatches != 0U) {
|
||||
invalidateToDispatchFence_(*found->second);
|
||||
} else {
|
||||
entries_.erase(found);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (!found->second->preemptible) {
|
||||
const auto holder = found->second->safety_holders.find(
|
||||
token.generation);
|
||||
return holder != found->second->safety_holders.end() &&
|
||||
holder->second.owner_id == token.owner_id &&
|
||||
!holder->second.retired;
|
||||
}
|
||||
if (found->second->owner_id != token.owner_id ||
|
||||
found->second->generation != token.generation) {
|
||||
return false;
|
||||
}
|
||||
found->second->deadline = std::chrono::steady_clock::now() + ttl;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::validate(
|
||||
const ControlLeaseToken& token)
|
||||
{
|
||||
if (!token.valid()) {
|
||||
return false;
|
||||
}
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto found = entries_.find(token.resource_id);
|
||||
if (found == entries_.end()) {
|
||||
return false;
|
||||
}
|
||||
if (expired_(*found->second)) {
|
||||
if (found->second->in_flight_dispatches != 0U) {
|
||||
invalidateToDispatchFence_(*found->second);
|
||||
} else {
|
||||
entries_.erase(found);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (!found->second->preemptible) {
|
||||
const auto holder = found->second->safety_holders.find(
|
||||
token.generation);
|
||||
return holder != found->second->safety_holders.end() &&
|
||||
holder->second.owner_id == token.owner_id &&
|
||||
!holder->second.retired;
|
||||
}
|
||||
return found->second->owner_id == token.owner_id &&
|
||||
found->second->generation == token.generation;
|
||||
}
|
||||
|
||||
ControlDispatchGuard ControlAuthorityManager::tryBeginDispatch(
|
||||
const ControlLeaseToken& token)
|
||||
{
|
||||
if (!token.valid()) {
|
||||
return {};
|
||||
}
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto found = entries_.find(token.resource_id);
|
||||
if (found == entries_.end()) {
|
||||
return {};
|
||||
}
|
||||
if (expired_(*found->second)) {
|
||||
if (found->second->in_flight_dispatches != 0U) {
|
||||
invalidateToDispatchFence_(*found->second);
|
||||
} else {
|
||||
entries_.erase(found);
|
||||
}
|
||||
return {};
|
||||
}
|
||||
if (!found->second->preemptible ||
|
||||
found->second->owner_id != token.owner_id ||
|
||||
found->second->generation != token.generation) {
|
||||
return {};
|
||||
}
|
||||
++found->second->in_flight_dispatches;
|
||||
return ControlDispatchGuard(this, found->second);
|
||||
}
|
||||
|
||||
void ControlAuthorityManager::release(
|
||||
const ControlLeaseToken& token) noexcept
|
||||
{
|
||||
if (!token.valid()) {
|
||||
return;
|
||||
}
|
||||
try {
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto found = entries_.find(token.resource_id);
|
||||
if (found == entries_.end()) {
|
||||
return;
|
||||
}
|
||||
auto& entry = *found->second;
|
||||
if (!entry.preemptible) {
|
||||
if (entry.dispatch_fence_only) {
|
||||
return;
|
||||
}
|
||||
const auto holder = entry.safety_holders.find(token.generation);
|
||||
if (holder != entry.safety_holders.end() &&
|
||||
holder->second.owner_id == token.owner_id) {
|
||||
if (holder->second.retired) {
|
||||
return;
|
||||
}
|
||||
entry.safety_holders.erase(holder);
|
||||
if (canErase_(entry)) {
|
||||
entries_.erase(found);
|
||||
}
|
||||
release_cv_.notify_all();
|
||||
return;
|
||||
}
|
||||
|
||||
const auto preempted = entry.preempted_normal_holders.find(
|
||||
token.generation);
|
||||
if (preempted != entry.preempted_normal_holders.end() &&
|
||||
preempted->second == token.owner_id) {
|
||||
entry.preempted_normal_holders.erase(preempted);
|
||||
if (canErase_(entry)) {
|
||||
entries_.erase(found);
|
||||
}
|
||||
release_cv_.notify_all();
|
||||
return;
|
||||
}
|
||||
|
||||
if (entry.quarantined_normal_pending &&
|
||||
entry.owner_id == token.owner_id &&
|
||||
entry.generation == token.generation) {
|
||||
entry.quarantined_normal_pending = false;
|
||||
if (canErase_(entry)) {
|
||||
entries_.erase(found);
|
||||
}
|
||||
release_cv_.notify_all();
|
||||
}
|
||||
} else if (entry.owner_id == token.owner_id &&
|
||||
entry.generation == token.generation) {
|
||||
if (entry.in_flight_dispatches != 0U) {
|
||||
invalidateToDispatchFence_(entry);
|
||||
} else {
|
||||
entries_.erase(found);
|
||||
}
|
||||
release_cv_.notify_all();
|
||||
}
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::isLeased(
|
||||
const std::string& resource_id)
|
||||
{
|
||||
if (resource_id.empty()) {
|
||||
return false;
|
||||
}
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto found = entries_.find(resource_id);
|
||||
if (found == entries_.end()) {
|
||||
return false;
|
||||
}
|
||||
if (expired_(*found->second)) {
|
||||
if (found->second->in_flight_dispatches != 0U) {
|
||||
invalidateToDispatchFence_(*found->second);
|
||||
return true;
|
||||
}
|
||||
entries_.erase(found);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void ControlAuthorityManager::revoke(
|
||||
const std::string& resource_id) noexcept
|
||||
{
|
||||
try {
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto found = entries_.find(resource_id);
|
||||
if (found == entries_.end()) {
|
||||
return;
|
||||
}
|
||||
if (found->second->in_flight_dispatches != 0U) {
|
||||
invalidateToDispatchFence_(*found->second);
|
||||
} else {
|
||||
entries_.erase(found);
|
||||
}
|
||||
release_cv_.notify_all();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
void ControlAuthorityManager::clear() noexcept
|
||||
{
|
||||
try {
|
||||
std::lock_guard lock(mutex_);
|
||||
for (auto entry = entries_.begin(); entry != entries_.end();) {
|
||||
if (entry->second->in_flight_dispatches != 0U) {
|
||||
invalidateToDispatchFence_(*entry->second);
|
||||
++entry;
|
||||
} else {
|
||||
entry = entries_.erase(entry);
|
||||
}
|
||||
}
|
||||
release_cv_.notify_all();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::expired_(const Entry& entry) const noexcept
|
||||
{
|
||||
return !entry.quarantined &&
|
||||
std::chrono::steady_clock::now() >= entry.deadline;
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::isSafetyHolder_(
|
||||
const Entry& entry,
|
||||
const ControlLeaseToken& token) noexcept
|
||||
{
|
||||
if (entry.preemptible || entry.dispatch_fence_only) {
|
||||
return false;
|
||||
}
|
||||
const auto holder = entry.safety_holders.find(token.generation);
|
||||
return holder != entry.safety_holders.end() &&
|
||||
holder->second.owner_id == token.owner_id;
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::isActiveSafetyHolder_(
|
||||
const Entry& entry,
|
||||
const ControlLeaseToken& token) noexcept
|
||||
{
|
||||
if (entry.preemptible || entry.dispatch_fence_only) {
|
||||
return false;
|
||||
}
|
||||
const auto holder = entry.safety_holders.find(token.generation);
|
||||
return holder != entry.safety_holders.end() &&
|
||||
holder->second.owner_id == token.owner_id &&
|
||||
!holder->second.retired;
|
||||
}
|
||||
|
||||
bool ControlAuthorityManager::canErase_(const Entry& entry) noexcept
|
||||
{
|
||||
if (entry.in_flight_dispatches != 0U) {
|
||||
return false;
|
||||
}
|
||||
if (entry.dispatch_fence_only) {
|
||||
return true;
|
||||
}
|
||||
return !entry.preemptible &&
|
||||
entry.safety_holders.empty() &&
|
||||
entry.preempted_normal_holders.empty() &&
|
||||
!entry.quarantined_normal_pending &&
|
||||
!entry.quarantined;
|
||||
}
|
||||
|
||||
void ControlAuthorityManager::invalidateToDispatchFence_(
|
||||
Entry& entry) noexcept
|
||||
{
|
||||
entry.owner_id.clear();
|
||||
entry.generation = 0U;
|
||||
entry.deadline = std::chrono::steady_clock::time_point::max();
|
||||
entry.preemptible = false;
|
||||
entry.quarantined = false;
|
||||
entry.quarantined_normal_pending = false;
|
||||
entry.dispatch_fence_only = true;
|
||||
entry.safety_holders.clear();
|
||||
entry.preempted_normal_holders.clear();
|
||||
}
|
||||
|
||||
void ControlAuthorityManager::endDispatch_(
|
||||
const std::shared_ptr<Entry>& entry) noexcept
|
||||
{
|
||||
try {
|
||||
std::lock_guard lock(mutex_);
|
||||
if (entry->in_flight_dispatches == 0U) {
|
||||
return;
|
||||
}
|
||||
--entry->in_flight_dispatches;
|
||||
const auto found = entries_.find(entry->resource_id);
|
||||
if (found != entries_.end() && found->second == entry &&
|
||||
canErase_(*entry)) {
|
||||
entries_.erase(found);
|
||||
}
|
||||
release_cv_.notify_all();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
void ControlAuthorityManager::quarantine_(Entry& entry) noexcept
|
||||
{
|
||||
entry.quarantined_normal_pending = entry.preemptible;
|
||||
entry.deadline = std::chrono::steady_clock::time_point::max();
|
||||
entry.preemptible = false;
|
||||
entry.quarantined = true;
|
||||
entry.dispatch_fence_only = false;
|
||||
}
|
||||
|
||||
} // namespace cmvr::control
|
||||
@ -1,5 +1,6 @@
|
||||
add_library(device_manager STATIC
|
||||
src/device_factory.cpp
|
||||
src/device_safety_adapters.cpp
|
||||
src/device_manager.cpp
|
||||
)
|
||||
|
||||
@ -7,6 +8,7 @@ target_include_directories(device_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
target_link_libraries(device_manager PRIVATE
|
||||
cmvr_es::proto
|
||||
cmvr_es::safety_manager
|
||||
cmvr_es::device::camera
|
||||
cmvr_es::device::agv
|
||||
cmvr_es::device::speaker
|
||||
@ -42,9 +44,16 @@ if(BUILD_TESTING)
|
||||
"${CMAKE_BINARY_DIR}/cmvr_compiler_runtime")
|
||||
list(JOIN _device_manager_test_library_dirs ":"
|
||||
_device_manager_test_library_path)
|
||||
set(_device_manager_snapshot_test_environment
|
||||
"LD_LIBRARY_PATH=${_device_manager_test_library_path}")
|
||||
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
|
||||
list(APPEND _device_manager_snapshot_test_environment
|
||||
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
|
||||
endif()
|
||||
set_tests_properties(device_manager_snapshot_test PROPERTIES
|
||||
ENVIRONMENT
|
||||
"LD_LIBRARY_PATH=${_device_manager_test_library_path}"
|
||||
"${_device_manager_snapshot_test_environment}"
|
||||
)
|
||||
endif()
|
||||
|
||||
endif()
|
||||
|
||||
1129
cmvr-es/manager/device_manager/src/device_safety_adapters.cpp
Normal file
1129
cmvr-es/manager/device_manager/src/device_safety_adapters.cpp
Normal file
File diff suppressed because it is too large
Load Diff
45
cmvr-es/manager/media_source_manager/CMakeLists.txt
Normal file
45
cmvr-es/manager/media_source_manager/CMakeLists.txt
Normal file
@ -0,0 +1,45 @@
|
||||
if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
|
||||
cmake_minimum_required(VERSION 3.22)
|
||||
project(cmvr_media_source_manager LANGUAGES CXX)
|
||||
add_subdirectory(
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../../service/grpc/stop_all
|
||||
${CMAKE_CURRENT_BINARY_DIR}/stop_all
|
||||
)
|
||||
endif()
|
||||
|
||||
add_library(media_source_manager STATIC
|
||||
src/media_source_manager.cpp
|
||||
)
|
||||
|
||||
target_compile_features(media_source_manager PUBLIC cxx_std_17)
|
||||
target_include_directories(media_source_manager
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../..
|
||||
)
|
||||
target_link_libraries(media_source_manager
|
||||
PUBLIC
|
||||
cmvr_es::stop_all_admission_gate
|
||||
)
|
||||
|
||||
add_library(cmvr_es::media_source_manager ALIAS media_source_manager)
|
||||
|
||||
if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
|
||||
add_library(device_media_source_adapter STATIC
|
||||
src/device_media_source_adapter.cpp
|
||||
)
|
||||
target_compile_features(device_media_source_adapter PUBLIC cxx_std_17)
|
||||
target_include_directories(device_media_source_adapter
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../..
|
||||
)
|
||||
target_link_libraries(device_media_source_adapter
|
||||
PUBLIC
|
||||
cmvr_es::media_source_manager
|
||||
cmvr_es::common
|
||||
cmvr_es::proto
|
||||
cmvr_es::logging
|
||||
cmvr_es::safety_manager
|
||||
)
|
||||
add_library(cmvr_es::device_media_source_adapter ALIAS device_media_source_adapter)
|
||||
|
||||
endif()
|
||||
@ -0,0 +1,46 @@
|
||||
#ifndef CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H
|
||||
#define CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "devices/camera/abstract_camera.h"
|
||||
#include "devices/microphone/abstract_microphone.h"
|
||||
#include "manager/media_source_manager/include/media_source_manager.h"
|
||||
#include "manager/safety_manager/include/safety_manager.h"
|
||||
|
||||
namespace cmvr::media {
|
||||
|
||||
// Process-wide protocol-neutral media hub shared by gRPC and QUIC services.
|
||||
MediaSourceManager& globalMediaSourceManager();
|
||||
|
||||
std::string cameraColorTrackId(const std::string& device_id);
|
||||
std::string microphoneTrackId(const std::string& device_id);
|
||||
|
||||
// Acquires the Coordinator's Sensor/StartActivity lane and performs the
|
||||
// device endpoint's final hardware check. Keep the returned guard alive until
|
||||
// the operation which can start the physical media producer has returned.
|
||||
safety::DispatchGuard beginMediaSourceStartDispatch(
|
||||
safety::SafetyManager& coordinator,
|
||||
const std::string& device_id);
|
||||
|
||||
// Registration is idempotent for an already registered track. The adapter owns a
|
||||
// short-lived pump thread and one startStreaming()/stopStreaming() lease only while
|
||||
// at least one Hub subscription is active. It ensures start() succeeds but deliberately
|
||||
// does not call stop(), because the base device lifecycle can also be owned by control RPCs.
|
||||
bool ensureCameraMediaSource(
|
||||
MediaSourceManager& hub,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
size_t ring_capacity = 64);
|
||||
|
||||
bool ensureMicrophoneMediaSource(
|
||||
MediaSourceManager& hub,
|
||||
const std::shared_ptr<device::AbstractMicrophone>& microphone,
|
||||
size_t ring_capacity = 256);
|
||||
|
||||
} // namespace cmvr::media
|
||||
|
||||
#endif // CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H
|
||||
@ -0,0 +1,162 @@
|
||||
#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H
|
||||
#define CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "common/base/ring_buffer.h"
|
||||
#include "common/media/media_frame.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
class StopAllAdmissionGate;
|
||||
}
|
||||
|
||||
namespace cmvr::media {
|
||||
|
||||
// MediaSourceManager owns no protocol-specific state. A device or capture adapter registers
|
||||
// start/stop callbacks and receives a sink callback when the first consumer subscribes.
|
||||
class MediaSourceManager final {
|
||||
public:
|
||||
using FrameRing = BroadcastFrameRing<MediaFrame>;
|
||||
using FrameReadResult = FrameRing::ReadResult;
|
||||
using StartPosition = FrameRing::StartPosition;
|
||||
using FrameSink = std::function<void(MediaFramePtr)>;
|
||||
// Cancellation checks run while MediaSourceManager protects source lifecycle
|
||||
// state. Predicates must therefore be fast, non-blocking and must not call
|
||||
// back into the same hub.
|
||||
using CancelPredicate = std::function<bool()>;
|
||||
|
||||
struct SourceCallbacks {
|
||||
// start() may run asynchronously. It must observe cancelled during any
|
||||
// potentially blocking startup work and return false promptly once set.
|
||||
// MediaSourceManager retains the callback state until a non-cooperative start
|
||||
// eventually returns, so late completion cannot access destroyed state.
|
||||
std::function<bool(
|
||||
const FrameSink& sink,
|
||||
const CancelPredicate& cancelled)> start;
|
||||
// stop() is the synchronous publication barrier for the last lease and
|
||||
// must unblock and join the source producer before returning.
|
||||
std::function<void()> stop;
|
||||
// Optional confirmed variant used by operational StopAll. Returning
|
||||
// false keeps the source quarantined so a later StopAll can retry it.
|
||||
// When omitted, a non-throwing stop() call is treated as confirmation.
|
||||
std::function<bool()> stop_confirmed;
|
||||
std::function<bool()> request_key_frame;
|
||||
};
|
||||
|
||||
private:
|
||||
struct SourceState;
|
||||
|
||||
public:
|
||||
class Subscription final {
|
||||
public:
|
||||
Subscription() = default;
|
||||
~Subscription();
|
||||
|
||||
Subscription(const Subscription&) = delete;
|
||||
Subscription& operator=(const Subscription&) = delete;
|
||||
Subscription(Subscription&& other) noexcept;
|
||||
Subscription& operator=(Subscription&& other) noexcept;
|
||||
|
||||
// A Subscription owns one reader cursor and is single-consumer. Moving,
|
||||
// resetting, or reading the same object concurrently is unsupported; use
|
||||
// one independent subscription per consumer thread.
|
||||
bool valid() const;
|
||||
explicit operator bool() const { return valid(); }
|
||||
|
||||
// Returns the most recently observed immutable descriptor. A callback source may
|
||||
// replace the initially registered UNKNOWN codec/config descriptor with the first
|
||||
// real frame descriptor without invalidating existing subscriptions.
|
||||
TrackDescriptorPtr descriptor() const;
|
||||
|
||||
std::optional<FrameReadResult> tryRead();
|
||||
std::optional<FrameReadResult> waitRead(std::chrono::milliseconds timeout);
|
||||
uint64_t discardPendingIfExceeds(size_t maximum_pending_frames);
|
||||
uint64_t droppedCount() const noexcept;
|
||||
void reset();
|
||||
|
||||
private:
|
||||
friend class MediaSourceManager;
|
||||
Subscription(std::shared_ptr<SourceState> source, FrameRing::Cursor cursor);
|
||||
|
||||
std::shared_ptr<SourceState> source_;
|
||||
FrameRing::Cursor cursor_;
|
||||
bool active_{false};
|
||||
};
|
||||
|
||||
// Pass the process-wide StopAll gate for a hub whose sources are part of
|
||||
// whole-machine operational stopping. Test/private hubs may remain local.
|
||||
explicit MediaSourceManager(
|
||||
service::StopAllAdmissionGate* admission_gate = nullptr);
|
||||
~MediaSourceManager();
|
||||
|
||||
MediaSourceManager(const MediaSourceManager&) = delete;
|
||||
MediaSourceManager& operator=(const MediaSourceManager&) = delete;
|
||||
|
||||
bool registerSource(
|
||||
TrackDescriptorPtr initial_descriptor,
|
||||
SourceCallbacks callbacks,
|
||||
size_t ring_capacity = 64);
|
||||
|
||||
// Active sources cannot be unregistered. Destroy/reset their subscriptions first.
|
||||
bool unregisterSource(const std::string& track_id);
|
||||
|
||||
bool hasSource(const std::string& track_id) const;
|
||||
std::vector<TrackDescriptorPtr> listTracks() const;
|
||||
// Returns a stable, sorted snapshot of physical source IDs. The snapshot
|
||||
// includes sources temporarily removed from the public track map while a
|
||||
// stop callback is in progress, so StopAll can discover orphaned activity
|
||||
// without consulting DeviceManager.
|
||||
std::vector<std::string> trackedSourceIds() const;
|
||||
size_t subscriberCount(const std::string& track_id) const;
|
||||
|
||||
// Protocol adapters can request an IDR after a discontinuity without knowing the
|
||||
// concrete camera implementation. Returns false when unsupported or not running.
|
||||
bool requestKeyFrame(const std::string& track_id) const;
|
||||
|
||||
Subscription subscribe(
|
||||
const std::string& track_id,
|
||||
StartPosition start_position = StartPosition::NEXT_PUBLISHED,
|
||||
CancelPredicate cancelled = {});
|
||||
|
||||
// Stops and unregisters every source whose registered descriptor belongs
|
||||
// to source_id. Sources for other physical devices remain registered and
|
||||
// keep running. A failed source is restored for a later retry.
|
||||
bool stopSourcesForDevice(
|
||||
const std::string& source_id,
|
||||
std::vector<std::string>* failures = nullptr);
|
||||
|
||||
// Stops and unregisters every source that was registered before this call's
|
||||
// stop phase began. Outstanding subscriptions are invalidated and blocked
|
||||
// waitRead calls are awakened. Registrations concurrent with the stop wait
|
||||
// for that phase to finish and are retained, so sources can be ensured and
|
||||
// subscribed again after this method returns.
|
||||
bool stopAllSources(std::vector<std::string>* failures = nullptr);
|
||||
|
||||
// Stops all registered sources and invalidates outstanding subscriptions. The
|
||||
// subscriptions remain destructible and their waitRead calls are awakened.
|
||||
// A cooperative in-progress start is cancelled; a callback that violates the
|
||||
// cancellation contract is quarantined with retained state rather than blocking
|
||||
// shutdown or risking a use-after-free. Equivalent to stopAllSources().
|
||||
void shutdown();
|
||||
|
||||
private:
|
||||
bool stopSources(
|
||||
const std::optional<std::string>& source_id,
|
||||
std::vector<std::string>* failures);
|
||||
|
||||
struct Impl;
|
||||
std::shared_ptr<Impl> impl_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::media
|
||||
|
||||
#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H
|
||||
@ -0,0 +1,735 @@
|
||||
#include "manager/media_source_manager/include/device_media_source_adapter.h"
|
||||
|
||||
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cctype>
|
||||
#include <cstdint>
|
||||
#include <exception>
|
||||
#include <iterator>
|
||||
#include <limits>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "common/base/logging/logger.h"
|
||||
|
||||
namespace cmvr::media {
|
||||
namespace {
|
||||
|
||||
std::atomic<std::uint64_t> media_start_sequence{0};
|
||||
|
||||
std::string normalizedCodec(std::string codec) {
|
||||
codec.erase(
|
||||
std::remove_if(codec.begin(), codec.end(), [](const unsigned char c) {
|
||||
return !std::isalnum(c);
|
||||
}),
|
||||
codec.end());
|
||||
std::transform(codec.begin(), codec.end(), codec.begin(), [](const unsigned char c) {
|
||||
return static_cast<char>(std::tolower(c));
|
||||
});
|
||||
return codec;
|
||||
}
|
||||
|
||||
Codec videoCodec(const std::string& value) {
|
||||
const std::string codec = normalizedCodec(value);
|
||||
if (codec == "h264" || codec == "avc" || codec == "avc1" || codec == "libx264") {
|
||||
return Codec::H264;
|
||||
}
|
||||
if (codec == "h265" || codec == "hevc" || codec == "hvc1" || codec == "libx265") {
|
||||
return Codec::H265;
|
||||
}
|
||||
return Codec::UNKNOWN;
|
||||
}
|
||||
|
||||
PayloadFormat videoPayloadFormat(
|
||||
const Codec codec,
|
||||
const std::vector<uint8_t>& payload) noexcept {
|
||||
if (codec != Codec::H264 && codec != Codec::H265) {
|
||||
return PayloadFormat::UNKNOWN;
|
||||
}
|
||||
const bool three_byte_start_code = payload.size() >= 3 &&
|
||||
payload[0] == 0U && payload[1] == 0U && payload[2] == 1U;
|
||||
const bool four_byte_start_code = payload.size() >= 4 &&
|
||||
payload[0] == 0U && payload[1] == 0U && payload[2] == 0U && payload[3] == 1U;
|
||||
return three_byte_start_code || four_byte_start_code
|
||||
? PayloadFormat::ANNEX_B
|
||||
: PayloadFormat::UNKNOWN;
|
||||
}
|
||||
|
||||
Codec audioCodec(const std::string& value) {
|
||||
const std::string codec = normalizedCodec(value);
|
||||
if (codec == "opus") return Codec::OPUS;
|
||||
if (codec == "aac") return Codec::AAC;
|
||||
if (codec == "pcms16le") return Codec::PCM_S16LE;
|
||||
return Codec::UNKNOWN;
|
||||
}
|
||||
|
||||
Codec audioCodec(const device::AudioStreamFrameData& source) {
|
||||
const Codec codec = audioCodec(source.codec);
|
||||
if (codec != Codec::UNKNOWN || !normalizedCodec(source.codec).empty()) {
|
||||
return codec;
|
||||
}
|
||||
switch (source.format) {
|
||||
case device::AudioStreamFormat::PCM:
|
||||
return Codec::PCM_S16LE;
|
||||
case device::AudioStreamFormat::AAC:
|
||||
return Codec::AAC;
|
||||
case device::AudioStreamFormat::OPUS:
|
||||
return Codec::OPUS;
|
||||
default:
|
||||
return Codec::UNKNOWN;
|
||||
}
|
||||
}
|
||||
|
||||
PayloadFormat audioPayloadFormat(
|
||||
const Codec codec,
|
||||
const std::vector<uint8_t>& payload) noexcept {
|
||||
switch (codec) {
|
||||
case Codec::OPUS:
|
||||
return PayloadFormat::OPUS_PACKET;
|
||||
case Codec::PCM_S16LE:
|
||||
return PayloadFormat::RAW;
|
||||
case Codec::AAC: {
|
||||
// FFmpeg encoders commonly expose raw AAC access units plus AudioSpecificConfig;
|
||||
// only advertise ADTS when the sync word and layer bits are actually present.
|
||||
const bool has_adts_header = payload.size() >= 2 && payload[0] == 0xFFU &&
|
||||
(payload[1] & 0xF6U) == 0xF0U;
|
||||
return has_adts_header ? PayloadFormat::AAC_ADTS : PayloadFormat::RAW;
|
||||
}
|
||||
default:
|
||||
return PayloadFormat::UNKNOWN;
|
||||
}
|
||||
}
|
||||
|
||||
Rational sanitizedTimeBase(
|
||||
const int32_t numerator,
|
||||
const int32_t denominator,
|
||||
const int32_t fallback_denominator) noexcept {
|
||||
return Rational{
|
||||
numerator > 0 ? numerator : 1,
|
||||
denominator > 0 ? denominator : std::max(1, fallback_denominator)};
|
||||
}
|
||||
|
||||
uint64_t descriptorGeneration(
|
||||
const uint64_t stream_epoch,
|
||||
const uint32_t codec_generation) {
|
||||
const uint64_t generation = (stream_epoch << 32U) | codec_generation;
|
||||
return generation == 0 ? 1 : generation;
|
||||
}
|
||||
|
||||
template<typename DeviceT>
|
||||
struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
|
||||
explicit PumpState(std::shared_ptr<DeviceT> device_ptr)
|
||||
: device(std::move(device_ptr)) {}
|
||||
|
||||
virtual ~PumpState() {
|
||||
(void)stop();
|
||||
}
|
||||
|
||||
bool begin(
|
||||
const MediaSourceManager::FrameSink& frame_sink,
|
||||
const MediaSourceManager::CancelPredicate& cancelled) {
|
||||
if (!frame_sink || !device) {
|
||||
return false;
|
||||
}
|
||||
const auto cancellation_requested = [&cancelled] {
|
||||
if (!cancelled) return false;
|
||||
try {
|
||||
return cancelled();
|
||||
} catch (...) {
|
||||
return true;
|
||||
}
|
||||
};
|
||||
if (cancellation_requested()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::unique_lock<std::mutex> lock(mutex);
|
||||
if (running.load(std::memory_order_acquire)) {
|
||||
return true;
|
||||
}
|
||||
if (worker.joinable()) {
|
||||
// A previous worker must always be collected before a new capture lease starts.
|
||||
std::thread stale_worker = std::move(worker);
|
||||
lock.unlock();
|
||||
if (!collectThread(std::move(stale_worker))) {
|
||||
return false;
|
||||
}
|
||||
lock.lock();
|
||||
}
|
||||
|
||||
sink = frame_sink;
|
||||
bool streaming_attempted = false;
|
||||
try {
|
||||
if (!device->start()) {
|
||||
sink = {};
|
||||
return false;
|
||||
}
|
||||
if (cancellation_requested()) {
|
||||
sink = {};
|
||||
return false;
|
||||
}
|
||||
streaming_attempted = true;
|
||||
if (!device->startStreaming()) {
|
||||
sink = {};
|
||||
lock.unlock();
|
||||
stopDeviceStreaming();
|
||||
return false;
|
||||
}
|
||||
streaming_started = true;
|
||||
if (cancellation_requested()) {
|
||||
streaming_started = false;
|
||||
sink = {};
|
||||
lock.unlock();
|
||||
stopDeviceStreaming();
|
||||
return false;
|
||||
}
|
||||
running.store(true, std::memory_order_release);
|
||||
try {
|
||||
// The worker owns the pump while run() is active. This also
|
||||
// makes the defensive self-stop/detach path lifetime-safe.
|
||||
const auto self = this->shared_from_this();
|
||||
worker = std::thread([self] { self->run(); });
|
||||
} catch (...) {
|
||||
running.store(false, std::memory_order_release);
|
||||
streaming_started = false;
|
||||
sink = {};
|
||||
lock.unlock();
|
||||
stopDeviceStreaming();
|
||||
return false;
|
||||
}
|
||||
} catch (const std::exception& error) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to start media source: "
|
||||
<< error.what();
|
||||
sink = {};
|
||||
lock.unlock();
|
||||
if (streaming_attempted) stopDeviceStreaming();
|
||||
return false;
|
||||
} catch (...) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to start media source";
|
||||
sink = {};
|
||||
lock.unlock();
|
||||
if (streaming_attempted) stopDeviceStreaming();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool stop() noexcept {
|
||||
std::thread thread;
|
||||
bool stop_streaming = false;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex);
|
||||
running.store(false, std::memory_order_release);
|
||||
stop_streaming = streaming_started;
|
||||
streaming_started = false;
|
||||
sink = {};
|
||||
thread = std::move(worker);
|
||||
}
|
||||
bool stopped = true;
|
||||
if (stop_streaming) {
|
||||
stopped = stopDeviceStreaming();
|
||||
}
|
||||
if (thread.joinable()) {
|
||||
stopped = collectThread(std::move(thread)) && stopped;
|
||||
}
|
||||
return stopped;
|
||||
}
|
||||
|
||||
static bool collectThread(std::thread thread) noexcept {
|
||||
if (!thread.joinable()) {
|
||||
return true;
|
||||
}
|
||||
try {
|
||||
if (thread.get_id() == std::this_thread::get_id()) {
|
||||
thread.detach();
|
||||
return false;
|
||||
} else {
|
||||
thread.join();
|
||||
}
|
||||
return true;
|
||||
} catch (const std::exception& error) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to collect media pump: "
|
||||
<< error.what();
|
||||
if (thread.joinable()) {
|
||||
try {
|
||||
thread.detach();
|
||||
} catch (...) {
|
||||
// std::thread's destructor would terminate if this extremely rare
|
||||
// platform error occurred; there is no recoverable ownership path.
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void run() = 0;
|
||||
|
||||
bool stopDeviceStreaming() noexcept {
|
||||
try {
|
||||
if (device) {
|
||||
device->stopStreaming();
|
||||
}
|
||||
return true;
|
||||
} catch (const std::exception& error) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source: "
|
||||
<< error.what();
|
||||
} catch (...) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source";
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
std::shared_ptr<DeviceT> device;
|
||||
std::atomic<bool> running{false};
|
||||
std::mutex mutex;
|
||||
std::thread worker;
|
||||
MediaSourceManager::FrameSink sink;
|
||||
bool streaming_started{false};
|
||||
};
|
||||
|
||||
struct CameraPump final : PumpState<device::AbstractCamera> {
|
||||
CameraPump(std::shared_ptr<device::AbstractCamera> camera, std::string id)
|
||||
: PumpState(std::move(camera)), track_id(std::move(id)) {}
|
||||
~CameraPump() override { (void)stop(); }
|
||||
|
||||
void run() override {
|
||||
size_t cursor = 0;
|
||||
uint64_t last_epoch = 0;
|
||||
uint64_t last_sequence = 0;
|
||||
uint64_t cached_config_generation = 0;
|
||||
std::vector<uint8_t> cached_codec_config;
|
||||
TrackDescriptorPtr last_descriptor;
|
||||
bool have_previous = false;
|
||||
bool have_cached_config_generation = false;
|
||||
bool waiting_for_key_frame = true;
|
||||
bool pending_discontinuity = true;
|
||||
bool requested_key_frame = false;
|
||||
std::chrono::steady_clock::time_point last_key_frame_request;
|
||||
|
||||
const auto request_key_frame = [&] {
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
if (requested_key_frame &&
|
||||
now - last_key_frame_request < std::chrono::milliseconds(250)) {
|
||||
return;
|
||||
}
|
||||
requested_key_frame = true;
|
||||
last_key_frame_request = now;
|
||||
try {
|
||||
device->requestKeyFrame();
|
||||
} catch (...) {
|
||||
// Unsupported/failed key-frame requests fall back to the encoder's GOP.
|
||||
}
|
||||
};
|
||||
request_key_frame();
|
||||
|
||||
while (running.load(std::memory_order_acquire)) {
|
||||
device::StreamFrameData source;
|
||||
try {
|
||||
if (!device->waitEncodedFrame(source, cursor, std::chrono::milliseconds(50))) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
continue;
|
||||
}
|
||||
} catch (const std::exception& error) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Camera frame read failed for "
|
||||
<< track_id << ": " << error.what();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
continue;
|
||||
} catch (...) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Camera frame read failed for "
|
||||
<< track_id;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
continue;
|
||||
}
|
||||
if (source.rgbFrame.empty()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const uint64_t descriptor_generation = descriptorGeneration(
|
||||
source.stream_epoch, source.codec_config_generation);
|
||||
if (!have_cached_config_generation ||
|
||||
cached_config_generation != descriptor_generation) {
|
||||
cached_codec_config.clear();
|
||||
cached_config_generation = descriptor_generation;
|
||||
have_cached_config_generation = true;
|
||||
}
|
||||
if (!source.codec_config.empty()) {
|
||||
cached_codec_config = source.codec_config;
|
||||
}
|
||||
|
||||
TrackDescriptor::Config track;
|
||||
track.id = track_id;
|
||||
track.source_id = device->id();
|
||||
track.kind = MediaKind::VIDEO;
|
||||
track.codec = videoCodec(source.codec);
|
||||
track.payload_format = videoPayloadFormat(track.codec, source.rgbFrame);
|
||||
track.time_base = sanitizedTimeBase(
|
||||
source.time_base_num,
|
||||
source.time_base_den,
|
||||
source.fps);
|
||||
track.width = static_cast<uint32_t>(std::max(0, source.width));
|
||||
track.height = static_cast<uint32_t>(std::max(0, source.height));
|
||||
track.nominal_rate = static_cast<uint32_t>(std::max(0, source.fps));
|
||||
track.fx = source.intrinsics.fx;
|
||||
track.fy = source.intrinsics.fy;
|
||||
track.cx = source.intrinsics.cx;
|
||||
track.cy = source.intrinsics.cy;
|
||||
track.distortion.assign(
|
||||
std::begin(source.intrinsics.coeffs),
|
||||
std::end(source.intrinsics.coeffs));
|
||||
track.generation = descriptor_generation;
|
||||
track.codec_config = cached_codec_config;
|
||||
|
||||
const bool epoch_changed = have_previous && source.stream_epoch != last_epoch;
|
||||
const bool sequence_wrapped = have_previous &&
|
||||
last_sequence == std::numeric_limits<uint64_t>::max() && source.sequence != 0;
|
||||
const bool sequence_gap = have_previous && !epoch_changed &&
|
||||
(sequence_wrapped ||
|
||||
(last_sequence != std::numeric_limits<uint64_t>::max() &&
|
||||
source.sequence != last_sequence + 1));
|
||||
|
||||
TrackDescriptorPtr descriptor;
|
||||
try {
|
||||
descriptor = makeTrackDescriptor(std::move(track));
|
||||
} catch (const std::exception& error) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid camera descriptor for "
|
||||
<< track_id << ": " << error.what();
|
||||
pending_discontinuity = true;
|
||||
last_epoch = source.stream_epoch;
|
||||
last_sequence = source.sequence;
|
||||
have_previous = true;
|
||||
continue;
|
||||
}
|
||||
const bool descriptor_changed = last_descriptor &&
|
||||
!equivalentTrackDescriptor(*last_descriptor, *descriptor);
|
||||
const bool discontinuity = !have_previous || source.discontinuity || epoch_changed ||
|
||||
sequence_gap || descriptor_changed;
|
||||
const bool inter_frame_codec = descriptor->codec == Codec::H264 ||
|
||||
descriptor->codec == Codec::H265;
|
||||
|
||||
if (discontinuity) {
|
||||
pending_discontinuity = true;
|
||||
if (inter_frame_codec) {
|
||||
waiting_for_key_frame = true;
|
||||
request_key_frame();
|
||||
} else {
|
||||
waiting_for_key_frame = false;
|
||||
}
|
||||
}
|
||||
|
||||
last_epoch = source.stream_epoch;
|
||||
last_sequence = source.sequence;
|
||||
last_descriptor = descriptor;
|
||||
have_previous = true;
|
||||
|
||||
if (waiting_for_key_frame && !source.bKey) {
|
||||
request_key_frame();
|
||||
continue;
|
||||
}
|
||||
waiting_for_key_frame = false;
|
||||
|
||||
MediaFrame::Config frame;
|
||||
frame.descriptor = std::move(descriptor);
|
||||
frame.payload = std::move(source.rgbFrame);
|
||||
frame.sequence = source.sequence;
|
||||
frame.source_timestamp = source.source_timestamp;
|
||||
frame.source_frame_number = source.source_frame_number;
|
||||
frame.pts = source.pts;
|
||||
frame.dts = source.dts;
|
||||
frame.duration = source.duration;
|
||||
frame.capture_time_ns = source.capture_monotonic_ns > 0
|
||||
? static_cast<uint64_t>(source.capture_monotonic_ns)
|
||||
: 0;
|
||||
frame.capture_utc_ns = source.capture_utc_ns;
|
||||
frame.key_frame = source.bKey;
|
||||
frame.discontinuity = pending_discontinuity;
|
||||
|
||||
MediaSourceManager::FrameSink current_sink;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex);
|
||||
current_sink = sink;
|
||||
}
|
||||
if (current_sink) {
|
||||
try {
|
||||
current_sink(makeMediaFrame(std::move(frame)));
|
||||
pending_discontinuity = false;
|
||||
} catch (const std::exception& error) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid camera frame for "
|
||||
<< track_id << ": " << error.what();
|
||||
pending_discontinuity = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::string track_id;
|
||||
};
|
||||
|
||||
struct MicrophonePump final : PumpState<device::AbstractMicrophone> {
|
||||
MicrophonePump(std::shared_ptr<device::AbstractMicrophone> microphone, std::string id)
|
||||
: PumpState(std::move(microphone)), track_id(std::move(id)) {}
|
||||
~MicrophonePump() override { (void)stop(); }
|
||||
|
||||
void run() override {
|
||||
size_t cursor = 0;
|
||||
uint64_t last_epoch = 0;
|
||||
uint64_t last_sequence = 0;
|
||||
uint64_t cached_config_generation = 0;
|
||||
std::vector<uint8_t> cached_codec_config;
|
||||
TrackDescriptorPtr last_descriptor;
|
||||
bool have_previous = false;
|
||||
bool have_cached_config_generation = false;
|
||||
bool pending_discontinuity = true;
|
||||
while (running.load(std::memory_order_acquire)) {
|
||||
device::AudioStreamFrameData source;
|
||||
try {
|
||||
if (!device->waitEncodedFrame(source, cursor, std::chrono::milliseconds(50))) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
continue;
|
||||
}
|
||||
} catch (const std::exception& error) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Microphone frame read failed for "
|
||||
<< track_id << ": " << error.what();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
continue;
|
||||
} catch (...) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Microphone frame read failed for "
|
||||
<< track_id;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
continue;
|
||||
}
|
||||
if (source.data.empty()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const uint64_t descriptor_generation = descriptorGeneration(
|
||||
source.stream_epoch, source.codec_config_generation);
|
||||
if (!have_cached_config_generation ||
|
||||
cached_config_generation != descriptor_generation) {
|
||||
cached_codec_config.clear();
|
||||
cached_config_generation = descriptor_generation;
|
||||
have_cached_config_generation = true;
|
||||
}
|
||||
if (!source.codec_config.empty()) {
|
||||
cached_codec_config = source.codec_config;
|
||||
}
|
||||
|
||||
TrackDescriptor::Config track;
|
||||
track.id = track_id;
|
||||
track.source_id = device->id();
|
||||
track.kind = MediaKind::AUDIO;
|
||||
track.codec = audioCodec(source);
|
||||
track.payload_format = audioPayloadFormat(track.codec, source.data);
|
||||
track.time_base = sanitizedTimeBase(
|
||||
source.time_base_num,
|
||||
source.time_base_den,
|
||||
source.sample_rate);
|
||||
track.sample_rate = static_cast<uint32_t>(std::max(0, source.sample_rate));
|
||||
track.channels = static_cast<uint32_t>(std::max(0, source.channels));
|
||||
// Packet sample counts belong to MediaFrame::duration, not immutable track metadata.
|
||||
track.nominal_rate = 0;
|
||||
track.generation = descriptor_generation;
|
||||
track.codec_config = cached_codec_config;
|
||||
|
||||
const bool epoch_changed = have_previous && source.stream_epoch != last_epoch;
|
||||
const bool sequence_wrapped = have_previous &&
|
||||
last_sequence == std::numeric_limits<uint64_t>::max() && source.sequence != 0;
|
||||
const bool sequence_gap = have_previous && !epoch_changed &&
|
||||
(sequence_wrapped ||
|
||||
(last_sequence != std::numeric_limits<uint64_t>::max() &&
|
||||
source.sequence != last_sequence + 1));
|
||||
|
||||
TrackDescriptorPtr descriptor;
|
||||
try {
|
||||
descriptor = makeTrackDescriptor(std::move(track));
|
||||
} catch (const std::exception& error) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid microphone descriptor for "
|
||||
<< track_id << ": " << error.what();
|
||||
pending_discontinuity = true;
|
||||
last_epoch = source.stream_epoch;
|
||||
last_sequence = source.sequence;
|
||||
have_previous = true;
|
||||
continue;
|
||||
}
|
||||
const bool descriptor_changed = last_descriptor &&
|
||||
!equivalentTrackDescriptor(*last_descriptor, *descriptor);
|
||||
if (!have_previous || source.discontinuity || epoch_changed || sequence_gap ||
|
||||
descriptor_changed) {
|
||||
pending_discontinuity = true;
|
||||
}
|
||||
|
||||
last_epoch = source.stream_epoch;
|
||||
last_sequence = source.sequence;
|
||||
last_descriptor = descriptor;
|
||||
have_previous = true;
|
||||
|
||||
MediaFrame::Config frame;
|
||||
frame.descriptor = std::move(descriptor);
|
||||
frame.payload = std::move(source.data);
|
||||
frame.sequence = source.sequence;
|
||||
frame.pts = source.pts;
|
||||
frame.dts = source.dts;
|
||||
frame.duration = source.duration > 0 ? source.duration : source.nb_samples;
|
||||
frame.capture_time_ns = source.capture_monotonic_ns > 0
|
||||
? static_cast<uint64_t>(source.capture_monotonic_ns)
|
||||
: 0;
|
||||
frame.capture_utc_ns = source.capture_utc_ns;
|
||||
frame.key_frame = true;
|
||||
frame.discontinuity = pending_discontinuity;
|
||||
|
||||
MediaSourceManager::FrameSink current_sink;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex);
|
||||
current_sink = sink;
|
||||
}
|
||||
if (current_sink) {
|
||||
try {
|
||||
current_sink(makeMediaFrame(std::move(frame)));
|
||||
pending_discontinuity = false;
|
||||
} catch (const std::exception& error) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid microphone frame for "
|
||||
<< track_id << ": " << error.what();
|
||||
pending_discontinuity = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::string track_id;
|
||||
};
|
||||
|
||||
TrackDescriptorPtr initialTrack(
|
||||
std::string track_id,
|
||||
std::string source_id,
|
||||
const MediaKind kind) {
|
||||
TrackDescriptor::Config config;
|
||||
config.id = std::move(track_id);
|
||||
config.source_id = std::move(source_id);
|
||||
config.kind = kind;
|
||||
// A placeholder descriptor is never emitted as a media sample, but it
|
||||
// still carries a mathematically valid neutral time base.
|
||||
config.time_base = Rational{1, 1};
|
||||
config.generation = 1;
|
||||
return makeTrackDescriptor(std::move(config));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
MediaSourceManager& globalMediaSourceManager() {
|
||||
static MediaSourceManager hub(&service::globalStopAllAdmissionGate());
|
||||
return hub;
|
||||
}
|
||||
|
||||
std::string cameraColorTrackId(const std::string& device_id) {
|
||||
return device_id + "/video/color";
|
||||
}
|
||||
|
||||
std::string microphoneTrackId(const std::string& device_id) {
|
||||
return device_id + "/audio/main";
|
||||
}
|
||||
|
||||
safety::DispatchGuard beginMediaSourceStartDispatch(
|
||||
safety::SafetyManager& coordinator,
|
||||
const std::string& device_id)
|
||||
{
|
||||
safety::AdmissionRequest request;
|
||||
request.command = {
|
||||
"cmvr.internal.MediaSourceManager/StartSource",
|
||||
safety::CommandIntent::StartActivity,
|
||||
safety::SafetyPolicyFamily::Sensor,
|
||||
true,
|
||||
false};
|
||||
request.actor.principal_id = "internal:media-source-hub";
|
||||
request.actor.authenticated = true;
|
||||
request.command_id = "media-source-start:" + device_id + ':' +
|
||||
std::to_string(
|
||||
media_start_sequence.fetch_add(1, std::memory_order_relaxed) + 1U);
|
||||
request.device_id = device_id;
|
||||
|
||||
auto admission = coordinator.admit(request);
|
||||
if (!admission.permit.has_value()) {
|
||||
CMVR_LOG(WARNING)
|
||||
<< "[DeviceMediaSourceAdapter] Media source admission rejected for "
|
||||
<< device_id << ": " << safety::toString(admission.decision.reason)
|
||||
<< " (" << admission.decision.detail << ')';
|
||||
return {};
|
||||
}
|
||||
auto dispatch = coordinator.beginDispatch(*admission.permit);
|
||||
if (!dispatch.acquired()) {
|
||||
CMVR_LOG(WARNING)
|
||||
<< "[DeviceMediaSourceAdapter] Media source final check rejected for "
|
||||
<< device_id << ": "
|
||||
<< safety::toString(dispatch.hardwareCheck().reason) << " ("
|
||||
<< dispatch.hardwareCheck().detail << ')';
|
||||
}
|
||||
return dispatch;
|
||||
}
|
||||
|
||||
bool ensureCameraMediaSource(
|
||||
MediaSourceManager& hub,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
const size_t ring_capacity) {
|
||||
if (!camera || camera->id().empty()) {
|
||||
return false;
|
||||
}
|
||||
const std::string track_id = cameraColorTrackId(camera->id());
|
||||
if (hub.hasSource(track_id)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const auto pump = std::make_shared<CameraPump>(camera, track_id);
|
||||
MediaSourceManager::SourceCallbacks callbacks;
|
||||
callbacks.start = [pump](
|
||||
const MediaSourceManager::FrameSink& sink,
|
||||
const MediaSourceManager::CancelPredicate& cancelled) {
|
||||
return pump->begin(sink, cancelled);
|
||||
};
|
||||
callbacks.stop_confirmed = [pump] { return pump->stop(); };
|
||||
callbacks.request_key_frame = [camera] { return camera->requestKeyFrame(); };
|
||||
const bool registered = hub.registerSource(
|
||||
initialTrack(track_id, camera->id(), MediaKind::VIDEO),
|
||||
std::move(callbacks),
|
||||
ring_capacity);
|
||||
if (!registered && !hub.hasSource(track_id)) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to register camera track: " << track_id;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ensureMicrophoneMediaSource(
|
||||
MediaSourceManager& hub,
|
||||
const std::shared_ptr<device::AbstractMicrophone>& microphone,
|
||||
const size_t ring_capacity) {
|
||||
if (!microphone || microphone->id().empty()) {
|
||||
return false;
|
||||
}
|
||||
const std::string track_id = microphoneTrackId(microphone->id());
|
||||
if (hub.hasSource(track_id)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const auto pump = std::make_shared<MicrophonePump>(microphone, track_id);
|
||||
MediaSourceManager::SourceCallbacks callbacks;
|
||||
callbacks.start = [pump](
|
||||
const MediaSourceManager::FrameSink& sink,
|
||||
const MediaSourceManager::CancelPredicate& cancelled) {
|
||||
return pump->begin(sink, cancelled);
|
||||
};
|
||||
callbacks.stop_confirmed = [pump] { return pump->stop(); };
|
||||
const bool registered = hub.registerSource(
|
||||
initialTrack(track_id, microphone->id(), MediaKind::AUDIO),
|
||||
std::move(callbacks),
|
||||
ring_capacity);
|
||||
if (!registered && !hub.hasSource(track_id)) {
|
||||
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to register microphone track: " << track_id;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace cmvr::media
|
||||
@ -0,0 +1,803 @@
|
||||
#include "manager/media_source_manager/include/media_source_manager.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
#include <utility>
|
||||
|
||||
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
|
||||
|
||||
namespace cmvr::media {
|
||||
|
||||
struct MediaSourceManager::SourceState final : public std::enable_shared_from_this<SourceState> {
|
||||
enum class Lifecycle {
|
||||
STOPPED,
|
||||
STARTING,
|
||||
RUNNING,
|
||||
STOPPING
|
||||
};
|
||||
|
||||
struct StartAttempt {
|
||||
size_t waiters{0};
|
||||
bool completed{false};
|
||||
bool succeeded{false};
|
||||
std::atomic<bool> cancel_requested{false};
|
||||
};
|
||||
|
||||
SourceState(
|
||||
TrackDescriptorPtr initial_descriptor,
|
||||
SourceCallbacks source_callbacks,
|
||||
const size_t ring_capacity,
|
||||
service::StopAllAdmissionGate* source_admission_gate)
|
||||
: track_id(initial_descriptor->id),
|
||||
source_id(initial_descriptor->source_id),
|
||||
descriptor(std::move(initial_descriptor)),
|
||||
callbacks(std::move(source_callbacks)),
|
||||
ring(ring_capacity),
|
||||
admission_gate(source_admission_gate) {}
|
||||
|
||||
FrameSink makeSink() {
|
||||
const std::weak_ptr<SourceState> weak_source = shared_from_this();
|
||||
return [weak_source](MediaFramePtr frame) {
|
||||
if (const auto source = weak_source.lock()) {
|
||||
source->acceptFrame(std::move(frame));
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
void acceptFrame(MediaFramePtr frame) {
|
||||
if (!frame || !frame->descriptor || frame->descriptor->id != track_id) {
|
||||
return;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||
if (!registered ||
|
||||
(lifecycle != Lifecycle::STARTING && lifecycle != Lifecycle::RUNNING)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
const TrackDescriptorPtr current = std::atomic_load(&descriptor);
|
||||
if (!current || !equivalentTrackDescriptor(*current, *frame->descriptor)) {
|
||||
// C++17 atomic shared_ptr free functions provide an atomic descriptor snapshot to
|
||||
// all subscriptions while frames remain immutable.
|
||||
std::atomic_store(&descriptor, frame->descriptor);
|
||||
}
|
||||
ring.publish(std::move(frame));
|
||||
}
|
||||
|
||||
static bool isCancelled(const CancelPredicate& cancelled) noexcept {
|
||||
if (!cancelled) return false;
|
||||
try {
|
||||
return cancelled();
|
||||
} catch (...) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
bool invokeStart(
|
||||
const FrameSink& sink,
|
||||
const CancelPredicate& cancelled) noexcept {
|
||||
std::lock_guard<std::mutex> callback_lock(callback_mutex);
|
||||
try {
|
||||
return callbacks.start && callbacks.start(sink, cancelled);
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool invokeStop() noexcept {
|
||||
std::lock_guard<std::mutex> callback_lock(callback_mutex);
|
||||
try {
|
||||
if (callbacks.stop_confirmed) {
|
||||
return callbacks.stop_confirmed();
|
||||
}
|
||||
if (callbacks.stop) {
|
||||
callbacks.stop();
|
||||
}
|
||||
return true;
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void completeStart(
|
||||
const std::shared_ptr<StartAttempt>& attempt,
|
||||
const bool started) {
|
||||
bool stop_abandoned_start = false;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||
if (start_attempt != attempt || lifecycle != Lifecycle::STARTING) {
|
||||
return;
|
||||
}
|
||||
attempt->completed = true;
|
||||
attempt->succeeded = started;
|
||||
if (started && registered && attempt->waiters != 0U) {
|
||||
lifecycle = Lifecycle::RUNNING;
|
||||
} else if (started) {
|
||||
lifecycle = Lifecycle::STOPPING;
|
||||
ring.close();
|
||||
stop_abandoned_start = true;
|
||||
} else {
|
||||
lifecycle = Lifecycle::STOPPED;
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
}
|
||||
|
||||
if (stop_abandoned_start) {
|
||||
const bool stopped = invokeStop();
|
||||
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||
stop_unconfirmed = !stopped;
|
||||
if (stopped && lifecycle == Lifecycle::STOPPING) {
|
||||
lifecycle = Lifecycle::STOPPED;
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
}
|
||||
}
|
||||
|
||||
bool acquire(
|
||||
const StartPosition start_position,
|
||||
FrameRing::Cursor& cursor,
|
||||
const CancelPredicate& cancelled) {
|
||||
std::optional<service::StopAllAdmissionGate::AdmissionGuard>
|
||||
admission;
|
||||
std::unique_lock<std::mutex> lock(lifecycle_mutex, std::defer_lock);
|
||||
for (;;) {
|
||||
if (admission_gate) {
|
||||
admission.emplace(admission_gate->lockAdmission());
|
||||
}
|
||||
lock.lock();
|
||||
if (!registered || isCancelled(cancelled) ||
|
||||
(admission && !admission->accepting())) {
|
||||
return false;
|
||||
}
|
||||
if (lifecycle != Lifecycle::STOPPING) {
|
||||
break;
|
||||
}
|
||||
|
||||
// A device stop may block, so never wait for it while retaining
|
||||
// the process-wide admission lock.
|
||||
admission.reset();
|
||||
lifecycle_condition.wait_for(
|
||||
lock, std::chrono::milliseconds(10));
|
||||
lock.unlock();
|
||||
}
|
||||
|
||||
if (lifecycle == Lifecycle::RUNNING) {
|
||||
++subscriber_count;
|
||||
lock.unlock();
|
||||
cursor = ring.makeCursor(start_position);
|
||||
return true;
|
||||
}
|
||||
|
||||
std::shared_ptr<StartAttempt> attempt;
|
||||
if (lifecycle == Lifecycle::STOPPED) {
|
||||
lifecycle = Lifecycle::STARTING;
|
||||
ring.reset();
|
||||
const FrameSink sink = makeSink();
|
||||
attempt = std::make_shared<StartAttempt>();
|
||||
attempt->waiters = 1U;
|
||||
start_attempt = attempt;
|
||||
const auto self = shared_from_this();
|
||||
try {
|
||||
std::thread([self, attempt, sink]() {
|
||||
const CancelPredicate cancelled = [attempt] {
|
||||
return attempt->cancel_requested.load(
|
||||
std::memory_order_acquire);
|
||||
};
|
||||
const bool started = self->invokeStart(sink, cancelled);
|
||||
self->completeStart(attempt, started);
|
||||
}).detach();
|
||||
} catch (...) {
|
||||
start_attempt.reset();
|
||||
lifecycle = Lifecycle::STOPPED;
|
||||
lifecycle_condition.notify_all();
|
||||
return false;
|
||||
}
|
||||
} else if (lifecycle == Lifecycle::STARTING) {
|
||||
attempt = start_attempt;
|
||||
if (!attempt || attempt->cancel_requested.load(std::memory_order_acquire)) {
|
||||
return false;
|
||||
}
|
||||
++attempt->waiters;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Startup is now ordered before beginStopAll(). Do not retain the
|
||||
// process-wide gate while waiting for the device callback to return.
|
||||
admission.reset();
|
||||
|
||||
while (registered && !attempt->completed) {
|
||||
if (isCancelled(cancelled)) {
|
||||
if (attempt->waiters != 0U) --attempt->waiters;
|
||||
if (attempt->waiters == 0U) {
|
||||
attempt->cancel_requested.store(true, std::memory_order_release);
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
return false;
|
||||
}
|
||||
lifecycle_condition.wait_for(lock, std::chrono::milliseconds(10));
|
||||
}
|
||||
|
||||
const bool caller_cancelled = isCancelled(cancelled);
|
||||
const bool acquired = !caller_cancelled && registered && attempt->completed &&
|
||||
attempt->succeeded &&
|
||||
lifecycle == Lifecycle::RUNNING;
|
||||
if (attempt->waiters != 0U) --attempt->waiters;
|
||||
if (!acquired) {
|
||||
if (attempt->waiters == 0U) {
|
||||
attempt->cancel_requested.store(true, std::memory_order_release);
|
||||
}
|
||||
const bool stop_unclaimed_source =
|
||||
start_attempt == attempt && attempt->waiters == 0U &&
|
||||
subscriber_count == 0U &&
|
||||
lifecycle == Lifecycle::RUNNING;
|
||||
if (stop_unclaimed_source) {
|
||||
lifecycle = Lifecycle::STOPPING;
|
||||
ring.close();
|
||||
lock.unlock();
|
||||
const bool stopped = invokeStop();
|
||||
lock.lock();
|
||||
stop_unconfirmed = !stopped;
|
||||
if (stopped && lifecycle == Lifecycle::STOPPING) {
|
||||
lifecycle = Lifecycle::STOPPED;
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
++subscriber_count;
|
||||
lock.unlock();
|
||||
cursor = ring.makeCursor(start_position);
|
||||
return true;
|
||||
}
|
||||
|
||||
void release() {
|
||||
std::unique_lock<std::mutex> lock(lifecycle_mutex);
|
||||
if (subscriber_count == 0) {
|
||||
return;
|
||||
}
|
||||
--subscriber_count;
|
||||
if (subscriber_count != 0 || lifecycle != Lifecycle::RUNNING) {
|
||||
return;
|
||||
}
|
||||
|
||||
lifecycle = Lifecycle::STOPPING;
|
||||
ring.close();
|
||||
lock.unlock();
|
||||
const bool stopped = invokeStop();
|
||||
lock.lock();
|
||||
stop_unconfirmed = !stopped;
|
||||
if (stopped) {
|
||||
lifecycle = Lifecycle::STOPPED;
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
}
|
||||
|
||||
bool deactivateIfUnused() {
|
||||
std::unique_lock<std::mutex> lock(lifecycle_mutex);
|
||||
if (subscriber_count != 0) {
|
||||
return false;
|
||||
}
|
||||
registered = false;
|
||||
ring.close();
|
||||
if (lifecycle == Lifecycle::STARTING) {
|
||||
if (start_attempt) {
|
||||
start_attempt->cancel_requested.store(true, std::memory_order_release);
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
return true;
|
||||
}
|
||||
if (lifecycle == Lifecycle::STOPPING || lifecycle == Lifecycle::STOPPED) {
|
||||
lifecycle_condition.notify_all();
|
||||
return true;
|
||||
}
|
||||
|
||||
lifecycle = Lifecycle::STOPPING;
|
||||
lock.unlock();
|
||||
const bool stopped = invokeStop();
|
||||
lock.lock();
|
||||
stop_unconfirmed = !stopped;
|
||||
if (stopped && lifecycle == Lifecycle::STOPPING) {
|
||||
lifecycle = Lifecycle::STOPPED;
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
return stopped;
|
||||
}
|
||||
|
||||
bool shutdown() {
|
||||
std::unique_lock<std::mutex> lock(lifecycle_mutex);
|
||||
registered = false;
|
||||
ring.close();
|
||||
if (lifecycle == Lifecycle::STARTING) {
|
||||
if (start_attempt) {
|
||||
start_attempt->cancel_requested.store(true, std::memory_order_release);
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
return false;
|
||||
}
|
||||
if (lifecycle == Lifecycle::STOPPING) {
|
||||
if (stop_unconfirmed) {
|
||||
lock.unlock();
|
||||
const bool stopped = invokeStop();
|
||||
lock.lock();
|
||||
stop_unconfirmed = !stopped;
|
||||
if (stopped) {
|
||||
lifecycle = Lifecycle::STOPPED;
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
return stopped;
|
||||
}
|
||||
lifecycle_condition.wait(lock, [this] {
|
||||
return lifecycle != Lifecycle::STOPPING;
|
||||
});
|
||||
lifecycle_condition.notify_all();
|
||||
return lifecycle == Lifecycle::STOPPED && !stop_unconfirmed;
|
||||
}
|
||||
if (lifecycle == Lifecycle::STOPPED) {
|
||||
lifecycle_condition.notify_all();
|
||||
return !stop_unconfirmed;
|
||||
}
|
||||
|
||||
lifecycle = Lifecycle::STOPPING;
|
||||
lock.unlock();
|
||||
const bool stopped = invokeStop();
|
||||
lock.lock();
|
||||
stop_unconfirmed = !stopped;
|
||||
if (stopped && lifecycle == Lifecycle::STOPPING) {
|
||||
lifecycle = Lifecycle::STOPPED;
|
||||
}
|
||||
lifecycle_condition.notify_all();
|
||||
return stopped;
|
||||
}
|
||||
|
||||
bool validForSubscription() const {
|
||||
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||
return registered && lifecycle == Lifecycle::RUNNING;
|
||||
}
|
||||
|
||||
size_t subscriberCount() const {
|
||||
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||
return subscriber_count;
|
||||
}
|
||||
|
||||
bool requestKeyFrame() const {
|
||||
// Serialize with stop first, then re-check lifecycle. A stop that has
|
||||
// already begun rejects the request; a stop that begins afterwards
|
||||
// waits for this callback before invoking the device stop barrier.
|
||||
std::lock_guard<std::mutex> callback_lock(callback_mutex);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||
if (!registered || lifecycle != Lifecycle::RUNNING || !callbacks.request_key_frame) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
try {
|
||||
return callbacks.request_key_frame();
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
TrackDescriptorPtr currentDescriptor() const {
|
||||
return std::atomic_load(&descriptor);
|
||||
}
|
||||
|
||||
const std::string track_id;
|
||||
const std::string source_id;
|
||||
mutable TrackDescriptorPtr descriptor;
|
||||
const SourceCallbacks callbacks;
|
||||
FrameRing ring;
|
||||
service::StopAllAdmissionGate* const admission_gate;
|
||||
|
||||
mutable std::mutex callback_mutex;
|
||||
mutable std::mutex lifecycle_mutex;
|
||||
std::condition_variable lifecycle_condition;
|
||||
Lifecycle lifecycle{Lifecycle::STOPPED};
|
||||
size_t subscriber_count{0};
|
||||
bool registered{true};
|
||||
bool stop_unconfirmed{false};
|
||||
std::shared_ptr<StartAttempt> start_attempt;
|
||||
};
|
||||
|
||||
struct MediaSourceManager::Impl final {
|
||||
explicit Impl(service::StopAllAdmissionGate* source_admission_gate)
|
||||
: admission_gate(source_admission_gate) {}
|
||||
|
||||
mutable std::mutex mutex;
|
||||
std::condition_variable stop_condition;
|
||||
bool stop_all_in_progress{false};
|
||||
std::unordered_set<std::string> device_stops_in_progress;
|
||||
std::unordered_set<std::string> track_stops_in_progress;
|
||||
std::unordered_map<std::string, size_t> tracked_source_counts;
|
||||
std::unordered_map<std::string, std::shared_ptr<SourceState>> sources;
|
||||
service::StopAllAdmissionGate* const admission_gate;
|
||||
};
|
||||
|
||||
MediaSourceManager::Subscription::Subscription(
|
||||
std::shared_ptr<SourceState> source,
|
||||
FrameRing::Cursor cursor)
|
||||
: source_(std::move(source)),
|
||||
cursor_(std::move(cursor)),
|
||||
active_(static_cast<bool>(source_)) {}
|
||||
|
||||
MediaSourceManager::Subscription::~Subscription() {
|
||||
reset();
|
||||
}
|
||||
|
||||
MediaSourceManager::Subscription::Subscription(Subscription&& other) noexcept
|
||||
: source_(std::move(other.source_)),
|
||||
cursor_(other.cursor_),
|
||||
active_(other.active_) {
|
||||
other.active_ = false;
|
||||
}
|
||||
|
||||
MediaSourceManager::Subscription& MediaSourceManager::Subscription::operator=(Subscription&& other) noexcept {
|
||||
if (this == &other) {
|
||||
return *this;
|
||||
}
|
||||
reset();
|
||||
source_ = std::move(other.source_);
|
||||
cursor_ = other.cursor_;
|
||||
active_ = other.active_;
|
||||
other.active_ = false;
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool MediaSourceManager::Subscription::valid() const {
|
||||
return active_ && source_ && source_->validForSubscription();
|
||||
}
|
||||
|
||||
TrackDescriptorPtr MediaSourceManager::Subscription::descriptor() const {
|
||||
return source_ ? source_->currentDescriptor() : nullptr;
|
||||
}
|
||||
|
||||
std::optional<MediaSourceManager::FrameReadResult> MediaSourceManager::Subscription::tryRead() {
|
||||
if (!active_ || !source_) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return source_->ring.tryRead(cursor_);
|
||||
}
|
||||
|
||||
std::optional<MediaSourceManager::FrameReadResult> MediaSourceManager::Subscription::waitRead(
|
||||
const std::chrono::milliseconds timeout) {
|
||||
if (!active_ || !source_) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return source_->ring.waitRead(cursor_, timeout);
|
||||
}
|
||||
|
||||
uint64_t MediaSourceManager::Subscription::discardPendingIfExceeds(
|
||||
const size_t maximum_pending_frames) {
|
||||
if (!active_ || !source_) {
|
||||
return 0;
|
||||
}
|
||||
return source_->ring.discardPendingIfExceeds(cursor_, maximum_pending_frames);
|
||||
}
|
||||
|
||||
uint64_t MediaSourceManager::Subscription::droppedCount() const noexcept {
|
||||
return cursor_.dropped_count;
|
||||
}
|
||||
|
||||
void MediaSourceManager::Subscription::reset() {
|
||||
if (active_ && source_) {
|
||||
source_->release();
|
||||
}
|
||||
active_ = false;
|
||||
source_.reset();
|
||||
}
|
||||
|
||||
MediaSourceManager::MediaSourceManager(
|
||||
service::StopAllAdmissionGate* admission_gate)
|
||||
: impl_(std::make_shared<Impl>(admission_gate)) {}
|
||||
|
||||
MediaSourceManager::~MediaSourceManager() {
|
||||
shutdown();
|
||||
}
|
||||
|
||||
bool MediaSourceManager::registerSource(
|
||||
TrackDescriptorPtr initial_descriptor,
|
||||
SourceCallbacks callbacks,
|
||||
const size_t ring_capacity) {
|
||||
if (!impl_ || !initial_descriptor || initial_descriptor->id.empty() ||
|
||||
!callbacks.start || ring_capacity == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::shared_ptr<SourceState> source;
|
||||
try {
|
||||
source = std::make_shared<SourceState>(
|
||||
std::move(initial_descriptor), std::move(callbacks), ring_capacity,
|
||||
impl_->admission_gate);
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::optional<service::StopAllAdmissionGate::AdmissionGuard> admission;
|
||||
std::unique_lock<std::mutex> lock(impl_->mutex, std::defer_lock);
|
||||
for (;;) {
|
||||
if (impl_->admission_gate) {
|
||||
admission.emplace(impl_->admission_gate->lockAdmission());
|
||||
}
|
||||
lock.lock();
|
||||
if (admission && !admission->accepting()) {
|
||||
return false;
|
||||
}
|
||||
const bool can_register =
|
||||
!impl_->stop_all_in_progress &&
|
||||
impl_->device_stops_in_progress.count(source->source_id) == 0U &&
|
||||
impl_->track_stops_in_progress.count(source->track_id) == 0U;
|
||||
if (can_register) {
|
||||
break;
|
||||
}
|
||||
|
||||
// Hub-local stops may invoke arbitrary device callbacks. Wait for
|
||||
// them without delaying process-wide StopAll admission.
|
||||
admission.reset();
|
||||
impl_->stop_condition.wait(lock, [this, &source] {
|
||||
return !impl_->stop_all_in_progress &&
|
||||
impl_->device_stops_in_progress.count(source->source_id) == 0U &&
|
||||
impl_->track_stops_in_progress.count(source->track_id) == 0U;
|
||||
});
|
||||
lock.unlock();
|
||||
}
|
||||
const std::string source_id = source->source_id;
|
||||
const bool inserted =
|
||||
impl_->sources.emplace(source->track_id, std::move(source)).second;
|
||||
if (inserted) {
|
||||
++impl_->tracked_source_counts[source_id];
|
||||
}
|
||||
return inserted;
|
||||
}
|
||||
|
||||
bool MediaSourceManager::unregisterSource(const std::string& track_id) {
|
||||
if (!impl_ || track_id.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::shared_ptr<SourceState> source;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
const auto it = impl_->sources.find(track_id);
|
||||
if (it == impl_->sources.end()) {
|
||||
return false;
|
||||
}
|
||||
source = it->second;
|
||||
}
|
||||
|
||||
if (!source->deactivateIfUnused()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
const auto it = impl_->sources.find(track_id);
|
||||
if (it != impl_->sources.end() && it->second == source) {
|
||||
const std::string source_id = source->source_id;
|
||||
impl_->sources.erase(it);
|
||||
const auto count_it = impl_->tracked_source_counts.find(source_id);
|
||||
if (count_it != impl_->tracked_source_counts.end() &&
|
||||
--count_it->second == 0U) {
|
||||
impl_->tracked_source_counts.erase(count_it);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MediaSourceManager::hasSource(const std::string& track_id) const {
|
||||
if (!impl_) {
|
||||
return false;
|
||||
}
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
return impl_->sources.find(track_id) != impl_->sources.end();
|
||||
}
|
||||
|
||||
std::vector<TrackDescriptorPtr> MediaSourceManager::listTracks() const {
|
||||
std::vector<std::shared_ptr<SourceState>> sources;
|
||||
if (!impl_) {
|
||||
return {};
|
||||
}
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
sources.reserve(impl_->sources.size());
|
||||
for (const auto& [track_id, source] : impl_->sources) {
|
||||
(void)track_id;
|
||||
sources.push_back(source);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<TrackDescriptorPtr> descriptors;
|
||||
descriptors.reserve(sources.size());
|
||||
for (const auto& source : sources) {
|
||||
descriptors.push_back(source->currentDescriptor());
|
||||
}
|
||||
std::sort(descriptors.begin(), descriptors.end(), [](const auto& lhs, const auto& rhs) {
|
||||
if (!lhs) return static_cast<bool>(rhs);
|
||||
if (!rhs) return false;
|
||||
return lhs->id < rhs->id;
|
||||
});
|
||||
return descriptors;
|
||||
}
|
||||
|
||||
std::vector<std::string> MediaSourceManager::trackedSourceIds() const {
|
||||
if (!impl_) {
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<std::string> source_ids;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
source_ids.reserve(impl_->tracked_source_counts.size());
|
||||
for (const auto& [source_id, count] : impl_->tracked_source_counts) {
|
||||
if (count != 0U) {
|
||||
source_ids.push_back(source_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
std::sort(source_ids.begin(), source_ids.end());
|
||||
return source_ids;
|
||||
}
|
||||
|
||||
size_t MediaSourceManager::subscriberCount(const std::string& track_id) const {
|
||||
if (!impl_) {
|
||||
return 0;
|
||||
}
|
||||
std::shared_ptr<SourceState> source;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
const auto it = impl_->sources.find(track_id);
|
||||
if (it == impl_->sources.end()) {
|
||||
return 0;
|
||||
}
|
||||
source = it->second;
|
||||
}
|
||||
return source->subscriberCount();
|
||||
}
|
||||
|
||||
bool MediaSourceManager::requestKeyFrame(const std::string& track_id) const {
|
||||
if (!impl_) {
|
||||
return false;
|
||||
}
|
||||
std::shared_ptr<SourceState> source;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
const auto it = impl_->sources.find(track_id);
|
||||
if (it == impl_->sources.end()) {
|
||||
return false;
|
||||
}
|
||||
source = it->second;
|
||||
}
|
||||
return source->requestKeyFrame();
|
||||
}
|
||||
|
||||
MediaSourceManager::Subscription MediaSourceManager::subscribe(
|
||||
const std::string& track_id,
|
||||
const StartPosition start_position,
|
||||
CancelPredicate cancelled) {
|
||||
if (!impl_) {
|
||||
return {};
|
||||
}
|
||||
|
||||
std::shared_ptr<SourceState> source;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
const auto it = impl_->sources.find(track_id);
|
||||
if (it == impl_->sources.end()) {
|
||||
return {};
|
||||
}
|
||||
source = it->second;
|
||||
}
|
||||
|
||||
FrameRing::Cursor cursor;
|
||||
if (!source->acquire(start_position, cursor, cancelled)) {
|
||||
return {};
|
||||
}
|
||||
return Subscription(std::move(source), std::move(cursor));
|
||||
}
|
||||
|
||||
bool MediaSourceManager::stopSourcesForDevice(
|
||||
const std::string& source_id,
|
||||
std::vector<std::string>* failures) {
|
||||
if (source_id.empty()) {
|
||||
if (failures) {
|
||||
failures->clear();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return stopSources(source_id, failures);
|
||||
}
|
||||
|
||||
bool MediaSourceManager::stopAllSources(std::vector<std::string>* failures) {
|
||||
return stopSources(std::nullopt, failures);
|
||||
}
|
||||
|
||||
bool MediaSourceManager::stopSources(
|
||||
const std::optional<std::string>& source_id,
|
||||
std::vector<std::string>* failures) {
|
||||
if (failures) {
|
||||
failures->clear();
|
||||
}
|
||||
if (!impl_) {
|
||||
return true;
|
||||
}
|
||||
|
||||
std::unordered_map<std::string, std::shared_ptr<SourceState>> sources;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(impl_->mutex);
|
||||
if (source_id) {
|
||||
impl_->stop_condition.wait(lock, [this, &source_id] {
|
||||
return !impl_->stop_all_in_progress &&
|
||||
impl_->device_stops_in_progress.count(*source_id) == 0U;
|
||||
});
|
||||
impl_->device_stops_in_progress.insert(*source_id);
|
||||
for (auto source_it = impl_->sources.begin();
|
||||
source_it != impl_->sources.end();) {
|
||||
if (source_it->second->source_id != *source_id) {
|
||||
++source_it;
|
||||
continue;
|
||||
}
|
||||
impl_->track_stops_in_progress.insert(source_it->first);
|
||||
sources.emplace(source_it->first, std::move(source_it->second));
|
||||
source_it = impl_->sources.erase(source_it);
|
||||
}
|
||||
} else {
|
||||
impl_->stop_condition.wait(lock, [this] {
|
||||
return !impl_->stop_all_in_progress &&
|
||||
impl_->device_stops_in_progress.empty();
|
||||
});
|
||||
impl_->stop_all_in_progress = true;
|
||||
sources.swap(impl_->sources);
|
||||
}
|
||||
}
|
||||
|
||||
std::unordered_map<std::string, std::shared_ptr<SourceState>> quarantined;
|
||||
for (const auto& [track_id, source] : sources) {
|
||||
if (!source->shutdown()) {
|
||||
quarantined.emplace(track_id, source);
|
||||
if (failures) {
|
||||
failures->push_back(track_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
for (auto& [track_id, source] : quarantined) {
|
||||
impl_->sources.emplace(track_id, std::move(source));
|
||||
}
|
||||
for (const auto& [track_id, source] : sources) {
|
||||
if (quarantined.count(track_id) != 0U) {
|
||||
continue;
|
||||
}
|
||||
const auto count_it =
|
||||
impl_->tracked_source_counts.find(source->source_id);
|
||||
if (count_it != impl_->tracked_source_counts.end() &&
|
||||
--count_it->second == 0U) {
|
||||
impl_->tracked_source_counts.erase(count_it);
|
||||
}
|
||||
}
|
||||
if (source_id) {
|
||||
for (const auto& [track_id, source] : sources) {
|
||||
(void)source;
|
||||
impl_->track_stops_in_progress.erase(track_id);
|
||||
}
|
||||
impl_->device_stops_in_progress.erase(*source_id);
|
||||
} else {
|
||||
impl_->stop_all_in_progress = false;
|
||||
}
|
||||
}
|
||||
impl_->stop_condition.notify_all();
|
||||
return quarantined.empty();
|
||||
}
|
||||
|
||||
void MediaSourceManager::shutdown() {
|
||||
(void)stopAllSources();
|
||||
}
|
||||
|
||||
} // namespace cmvr::media
|
||||
18
cmvr-es/manager/safety_manager/CMakeLists.txt
Normal file
18
cmvr-es/manager/safety_manager/CMakeLists.txt
Normal file
@ -0,0 +1,18 @@
|
||||
add_library(safety_manager STATIC
|
||||
src/command_ledger.cpp
|
||||
src/safety_manager.cpp
|
||||
src/safety_reason.cpp
|
||||
src/safety_snapshot_store.cpp
|
||||
)
|
||||
|
||||
target_include_directories(safety_manager PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${CMAKE_SOURCE_DIR}/cmvr-es
|
||||
)
|
||||
|
||||
target_link_libraries(safety_manager PUBLIC
|
||||
cmvr_es::control_authority_manager
|
||||
)
|
||||
|
||||
add_library(cmvr_es::safety_manager ALIAS safety_manager)
|
||||
install(TARGETS safety_manager LIBRARY DESTINATION lib)
|
||||
130
cmvr-es/manager/safety_manager/include/command_ledger.h
Normal file
130
cmvr-es/manager/safety_manager/include/command_ledger.h
Normal file
@ -0,0 +1,130 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "manager/safety_manager/include/safety_types.h"
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
struct CommandKey {
|
||||
std::string effective_principal_id;
|
||||
std::string command_id;
|
||||
|
||||
bool operator==(const CommandKey& other) const noexcept
|
||||
{
|
||||
return effective_principal_id == other.effective_principal_id &&
|
||||
command_id == other.command_id;
|
||||
}
|
||||
};
|
||||
|
||||
struct CommandOutcome {
|
||||
CommandLifecycle lifecycle{CommandLifecycle::Failed};
|
||||
SafetyReason reason{SafetyReason::InternalError};
|
||||
std::string detail;
|
||||
std::string serialized_response;
|
||||
std::uint64_t safety_epoch{0};
|
||||
std::uint64_t device_generation{0};
|
||||
bool hardware_submission_possible{false};
|
||||
};
|
||||
|
||||
enum class CommandReservationStatus {
|
||||
AcceptedNew,
|
||||
JoinedInFlight,
|
||||
CachedResult,
|
||||
CommandIdConflict,
|
||||
ResultEvicted,
|
||||
LedgerExhausted,
|
||||
Invalid,
|
||||
};
|
||||
|
||||
class CommandLedger final {
|
||||
private:
|
||||
struct State;
|
||||
|
||||
public:
|
||||
struct Config {
|
||||
std::size_t result_capacity{4096};
|
||||
std::size_t total_id_capacity{256U * 1024U};
|
||||
};
|
||||
|
||||
class Ticket final {
|
||||
public:
|
||||
Ticket() = default;
|
||||
bool valid() const noexcept { return state_ != nullptr; }
|
||||
|
||||
private:
|
||||
friend class CommandLedger;
|
||||
explicit Ticket(std::shared_ptr<State> state)
|
||||
: state_(std::move(state))
|
||||
{
|
||||
}
|
||||
|
||||
std::shared_ptr<State> state_;
|
||||
};
|
||||
|
||||
struct Reservation {
|
||||
CommandReservationStatus status{CommandReservationStatus::Invalid};
|
||||
Ticket ticket;
|
||||
std::optional<CommandOutcome> cached_outcome;
|
||||
};
|
||||
|
||||
CommandLedger();
|
||||
explicit CommandLedger(Config config);
|
||||
|
||||
Reservation reserve(CommandKey key, std::string payload_hash);
|
||||
bool setLifecycle(const Ticket& ticket,
|
||||
CommandLifecycle lifecycle,
|
||||
std::uint64_t safety_epoch = 0,
|
||||
std::uint64_t device_generation = 0,
|
||||
bool hardware_submission_possible = false);
|
||||
bool complete(const Ticket& ticket, CommandOutcome outcome);
|
||||
std::optional<CommandOutcome> wait(
|
||||
const Ticket& ticket,
|
||||
SafetyClock::time_point deadline = SafetyClock::time_point::max()) const;
|
||||
std::optional<CommandOutcome> lookup(
|
||||
const CommandKey& key,
|
||||
const std::string& payload_hash) const;
|
||||
|
||||
std::size_t acceptedIdCount() const;
|
||||
std::size_t liveRecordCount() const;
|
||||
std::size_t retiredIdCount() const;
|
||||
|
||||
private:
|
||||
struct KeyHash {
|
||||
std::size_t operator()(const CommandKey& key) const noexcept;
|
||||
};
|
||||
|
||||
struct State {
|
||||
CommandKey key;
|
||||
std::string payload_hash;
|
||||
mutable std::mutex mutex;
|
||||
mutable std::condition_variable condition;
|
||||
CommandLifecycle lifecycle{CommandLifecycle::Reserved};
|
||||
std::optional<CommandOutcome> outcome;
|
||||
std::uint64_t safety_epoch{0};
|
||||
std::uint64_t device_generation{0};
|
||||
bool hardware_submission_possible{false};
|
||||
bool terminal{false};
|
||||
};
|
||||
|
||||
static bool validKey_(const CommandKey& key) noexcept;
|
||||
void trimTerminalResultsLocked_();
|
||||
|
||||
const Config config_;
|
||||
mutable std::mutex mutex_;
|
||||
std::unordered_map<CommandKey, std::shared_ptr<State>, KeyHash> records_;
|
||||
std::unordered_map<CommandKey, std::string, KeyHash> retired_ids_;
|
||||
std::vector<CommandKey> terminal_order_;
|
||||
std::size_t terminal_result_count_{0};
|
||||
};
|
||||
|
||||
const char* toString(CommandReservationStatus status) noexcept;
|
||||
|
||||
} // namespace cmvr::safety
|
||||
@ -0,0 +1,50 @@
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
|
||||
#include "manager/safety_manager/include/safety_types.h"
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
using SafetySnapshotPublisher =
|
||||
std::function<bool(DeviceSafetySnapshot)>;
|
||||
|
||||
class DeviceSafetyEndpoint {
|
||||
public:
|
||||
virtual ~DeviceSafetyEndpoint() = default;
|
||||
|
||||
virtual DeviceSafetyDescriptor descriptor() const = 0;
|
||||
virtual void bindPublisher(SafetySnapshotPublisher publisher) = 0;
|
||||
virtual void requestSafetyRefresh() noexcept = 0;
|
||||
// Called after a backend/session restart. Implementations must publish
|
||||
// subsequent samples with this generation or remain fail-closed.
|
||||
virtual void onDeviceGenerationChanged(
|
||||
std::uint64_t generation) noexcept
|
||||
{
|
||||
(void)generation;
|
||||
}
|
||||
virtual HardwareCheckResult validateBeforeDispatch(
|
||||
const AdmissionPermit& permit) = 0;
|
||||
// Explicit operator-authorized recovery which may power or enable the
|
||||
// device. RecoverSafetyState never calls this method; it is reserved for
|
||||
// RestoreOperationalState transactions.
|
||||
virtual RecoveryCheckResult restoreOperationalState(
|
||||
const RecoveryContext&)
|
||||
{
|
||||
return {
|
||||
false,
|
||||
SafetyReason::UnsupportedCommand,
|
||||
"device does not support operational recovery"};
|
||||
}
|
||||
virtual RecoveryCheckResult reconcileAdmissionState(
|
||||
const RecoveryContext& context) = 0;
|
||||
};
|
||||
|
||||
class DeviceSafetyEndpointProvider {
|
||||
public:
|
||||
virtual ~DeviceSafetyEndpointProvider() = default;
|
||||
virtual std::shared_ptr<DeviceSafetyEndpoint> safetyEndpoint() = 0;
|
||||
};
|
||||
|
||||
} // namespace cmvr::safety
|
||||
222
cmvr-es/manager/safety_manager/include/safety_manager.h
Normal file
222
cmvr-es/manager/safety_manager/include/safety_manager.h
Normal file
@ -0,0 +1,222 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_set>
|
||||
#include <vector>
|
||||
|
||||
#include "manager/safety_manager/include/command_ledger.h"
|
||||
#include "manager/safety_manager/include/device_safety_endpoint.h"
|
||||
#include "manager/safety_manager/include/safety_participant.h"
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
struct SafetyManagerConfig {
|
||||
EnforcementMode enforcement_mode{EnforcementMode::Shadow};
|
||||
std::unordered_set<std::string> enforced_device_ids;
|
||||
std::chrono::milliseconds stop_all_timeout{15000};
|
||||
std::chrono::milliseconds recovery_timeout{10000};
|
||||
CommandLedger::Config command_ledger;
|
||||
std::size_t event_history_capacity{2048};
|
||||
bool fail_startup_on_missing_control_capability{false};
|
||||
};
|
||||
|
||||
struct AdmissionResult {
|
||||
AdmissionDecision decision;
|
||||
std::optional<AdmissionPermit> permit;
|
||||
};
|
||||
|
||||
struct StartupCoverageIssue {
|
||||
std::string target_id;
|
||||
SafetyReason reason{SafetyReason::None};
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
struct StartupCoverageResult {
|
||||
bool ready{false};
|
||||
std::vector<StartupCoverageIssue> issues;
|
||||
};
|
||||
|
||||
struct DeviceSafetyStateView {
|
||||
DeviceSafetyDescriptor descriptor;
|
||||
SafetySnapshotView safety;
|
||||
device::ManagedDeviceState lifecycle{
|
||||
device::ManagedDeviceState::Unknown};
|
||||
device::DeviceHealthSnapshot health;
|
||||
DeviceAdmissionState admission_state{DeviceAdmissionState::Observing};
|
||||
std::vector<SafetyBlocker> blockers;
|
||||
};
|
||||
|
||||
struct ParticipantResultView {
|
||||
bool recorded{false};
|
||||
bool success{false};
|
||||
SafetyReason reason{SafetyReason::None};
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
struct ParticipantSafetyStateView {
|
||||
ParticipantDescriptor descriptor;
|
||||
bool registered{false};
|
||||
bool barrier_active{false};
|
||||
bool barrier_retained{false};
|
||||
std::string operation_id;
|
||||
std::uint64_t safety_epoch{0};
|
||||
ParticipantResultView last_request;
|
||||
ParticipantResultView last_verify;
|
||||
ParticipantResultView last_release;
|
||||
};
|
||||
|
||||
struct SafetyManagerSnapshot {
|
||||
SystemAdmissionState system_state{SystemAdmissionState::Starting};
|
||||
std::uint64_t safety_epoch{0};
|
||||
std::string service_instance_id;
|
||||
EnforcementMode enforcement_mode{EnforcementMode::Shadow};
|
||||
std::string active_operation_id;
|
||||
std::string active_operation_phase;
|
||||
std::vector<DeviceSafetyStateView> devices;
|
||||
std::vector<ParticipantSafetyStateView> participants;
|
||||
std::vector<SafetyEvent> recent_events;
|
||||
};
|
||||
|
||||
struct SafetyTargetResult {
|
||||
std::string target_id;
|
||||
bool success{false};
|
||||
SafetyReason reason{SafetyReason::None};
|
||||
std::string detail;
|
||||
DeviceAdmissionState before_state{DeviceAdmissionState::Observing};
|
||||
DeviceAdmissionState after_state{DeviceAdmissionState::Observing};
|
||||
};
|
||||
|
||||
struct StopAllResult {
|
||||
bool success{false};
|
||||
std::string operation_id;
|
||||
std::uint64_t previous_safety_epoch{0};
|
||||
std::uint64_t current_safety_epoch{0};
|
||||
SystemAdmissionState system_state{SystemAdmissionState::Starting};
|
||||
std::vector<SafetyTargetResult> targets;
|
||||
};
|
||||
|
||||
enum class RecoveryResultCode {
|
||||
Recovered,
|
||||
VerifiedButStillBlocked,
|
||||
BlockerRemains,
|
||||
EpochMismatch,
|
||||
NothingToRecover,
|
||||
TimedOut,
|
||||
Failed,
|
||||
};
|
||||
|
||||
struct RecoveryRequest {
|
||||
std::string recovery_id;
|
||||
std::vector<std::string> device_ids;
|
||||
bool all_devices{false};
|
||||
std::uint64_t expected_safety_epoch{0};
|
||||
bool verify_only{true};
|
||||
bool restore_operational_state{false};
|
||||
std::string reason;
|
||||
SafetyClock::time_point deadline{SafetyClock::time_point::max()};
|
||||
// Authorizes the state-changing part of recovery. Operational restore
|
||||
// calls this before invoking device recovery; software-only recovery calls
|
||||
// it after hardware verification and before releasing admission barriers.
|
||||
// A false result leaves admission latched and prevents device recovery.
|
||||
std::function<bool()> authorize_clear;
|
||||
};
|
||||
|
||||
struct RecoveryResult {
|
||||
RecoveryResultCode result{RecoveryResultCode::Failed};
|
||||
std::string recovery_id;
|
||||
std::uint64_t previous_safety_epoch{0};
|
||||
std::uint64_t current_safety_epoch{0};
|
||||
SystemAdmissionState system_state{SystemAdmissionState::Starting};
|
||||
std::vector<SafetyTargetResult> targets;
|
||||
};
|
||||
|
||||
class SafetyManager;
|
||||
|
||||
class DispatchGuard final {
|
||||
public:
|
||||
DispatchGuard() noexcept = default;
|
||||
~DispatchGuard() noexcept;
|
||||
DispatchGuard(DispatchGuard&& other) noexcept;
|
||||
DispatchGuard& operator=(DispatchGuard&& other) noexcept;
|
||||
DispatchGuard(const DispatchGuard&) = delete;
|
||||
DispatchGuard& operator=(const DispatchGuard&) = delete;
|
||||
|
||||
bool acquired() const noexcept { return coordinator_ != nullptr; }
|
||||
const HardwareCheckResult& hardwareCheck() const noexcept
|
||||
{
|
||||
return hardware_check_;
|
||||
}
|
||||
|
||||
private:
|
||||
friend class SafetyManager;
|
||||
DispatchGuard(SafetyManager* coordinator,
|
||||
std::string device_id,
|
||||
HardwareCheckResult hardware_check) noexcept;
|
||||
void reset_() noexcept;
|
||||
|
||||
SafetyManager* coordinator_{nullptr};
|
||||
std::string device_id_;
|
||||
HardwareCheckResult hardware_check_;
|
||||
};
|
||||
|
||||
class SafetyManager final {
|
||||
public:
|
||||
explicit SafetyManager(SafetyManagerConfig config = {});
|
||||
~SafetyManager();
|
||||
SafetyManager(const SafetyManager&) = delete;
|
||||
SafetyManager& operator=(const SafetyManager&) = delete;
|
||||
|
||||
bool registerDevice(DeviceSafetyRegistration registration);
|
||||
bool registerParticipant(std::shared_ptr<SafetyParticipant> participant);
|
||||
bool unregisterParticipant(const std::string& participant_id);
|
||||
|
||||
void updateDeviceRuntimeState(
|
||||
const std::string& device_id,
|
||||
device::ManagedDeviceState lifecycle,
|
||||
device::DeviceHealthSnapshot health = {});
|
||||
std::optional<std::uint64_t> advanceDeviceGeneration(
|
||||
const std::string& device_id);
|
||||
StartupCoverageResult validateStartupCoverage(
|
||||
SafetyClock::time_point deadline);
|
||||
void markStartupComplete();
|
||||
|
||||
AdmissionResult admit(const AdmissionRequest& request);
|
||||
// Lightweight session check. This validates the coordinator-owned epoch,
|
||||
// generation, freshness, and admission state without calling the device
|
||||
// endpoint or entering the hardware dispatch set.
|
||||
HardwareCheckResult revalidatePermit(
|
||||
const AdmissionPermit& permit) const;
|
||||
DispatchGuard beginDispatch(const AdmissionPermit& permit);
|
||||
void quarantineDevice(const std::string& device_id,
|
||||
SafetyReason reason,
|
||||
std::string operation_id = {});
|
||||
|
||||
StopAllResult stopAll(
|
||||
std::string operation_id,
|
||||
SafetyClock::time_point deadline = SafetyClock::time_point::max());
|
||||
RecoveryResult recover(const RecoveryRequest& request);
|
||||
|
||||
SafetyManagerSnapshot snapshot() const;
|
||||
CommandLedger& commandLedger() noexcept;
|
||||
const std::string& serviceInstanceId() const noexcept;
|
||||
const SafetyManagerConfig& config() const noexcept;
|
||||
|
||||
private:
|
||||
friend class DispatchGuard;
|
||||
struct Impl;
|
||||
bool publishSafetySnapshot(DeviceSafetySnapshot snapshot);
|
||||
void beginShutdown() noexcept;
|
||||
AdmissionDecision evaluate(const AdmissionRequest& request) const;
|
||||
void endDispatch_(const std::string& device_id) noexcept;
|
||||
std::unique_ptr<Impl> impl_;
|
||||
};
|
||||
|
||||
const char* toString(RecoveryResultCode value) noexcept;
|
||||
|
||||
} // namespace cmvr::safety
|
||||
88
cmvr-es/manager/safety_manager/include/safety_participant.h
Normal file
88
cmvr-es/manager/safety_manager/include/safety_participant.h
Normal file
@ -0,0 +1,88 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "manager/safety_manager/include/safety_types.h"
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
class DeviceSafetyEndpoint;
|
||||
|
||||
enum class ParticipantPhase {
|
||||
Ingress,
|
||||
Scheduler,
|
||||
ControlSession,
|
||||
Actuator,
|
||||
PeripheralActivity,
|
||||
Verification,
|
||||
};
|
||||
|
||||
struct ParticipantDescriptor {
|
||||
std::string participant_id;
|
||||
ParticipantPhase phase{ParticipantPhase::Actuator};
|
||||
bool required{true};
|
||||
std::chrono::milliseconds timeout{5000};
|
||||
};
|
||||
|
||||
struct SafetyOperationContext {
|
||||
std::string operation_id;
|
||||
std::uint64_t safety_epoch{0};
|
||||
SafetyClock::time_point deadline{SafetyClock::time_point::max()};
|
||||
};
|
||||
|
||||
struct BarrierToken {
|
||||
std::string participant_id;
|
||||
std::string operation_id;
|
||||
std::uint64_t safety_epoch{0};
|
||||
std::uint64_t generation{0};
|
||||
|
||||
bool valid() const noexcept
|
||||
{
|
||||
return !participant_id.empty() && !operation_id.empty() &&
|
||||
safety_epoch != 0 && generation != 0;
|
||||
}
|
||||
};
|
||||
|
||||
struct ParticipantResult {
|
||||
bool success{false};
|
||||
SafetyReason reason{SafetyReason::StopUnconfirmed};
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
class SafetyParticipant {
|
||||
public:
|
||||
virtual ~SafetyParticipant() = default;
|
||||
virtual ParticipantDescriptor descriptor() const = 0;
|
||||
virtual BarrierToken beginBarrier(
|
||||
const SafetyOperationContext& context) = 0;
|
||||
virtual ParticipantResult requestQuiesce(
|
||||
const BarrierToken& token,
|
||||
const SafetyOperationContext& context) = 0;
|
||||
virtual ParticipantResult verifyQuiescent(
|
||||
const BarrierToken& token,
|
||||
const SafetyOperationContext& context) = 0;
|
||||
virtual RecoveryCheckResult recoverAdmission(
|
||||
const BarrierToken& token,
|
||||
const RecoveryContext& context) = 0;
|
||||
// Commits the participant's admission reopening. A failed commit must
|
||||
// leave that participant fail-closed and be retryable through recovery.
|
||||
virtual ParticipantResult releaseBarrier(
|
||||
const BarrierToken& token) noexcept = 0;
|
||||
};
|
||||
|
||||
class SafetyParticipantProvider {
|
||||
public:
|
||||
virtual ~SafetyParticipantProvider() = default;
|
||||
virtual std::shared_ptr<SafetyParticipant> safetyParticipant() = 0;
|
||||
};
|
||||
|
||||
struct DeviceSafetyRegistration {
|
||||
DeviceSafetyDescriptor descriptor;
|
||||
std::shared_ptr<DeviceSafetyEndpoint> endpoint;
|
||||
std::shared_ptr<SafetyParticipant> participant;
|
||||
};
|
||||
|
||||
} // namespace cmvr::safety
|
||||
48
cmvr-es/manager/safety_manager/include/safety_reason.h
Normal file
48
cmvr-es/manager/safety_manager/include/safety_reason.h
Normal file
@ -0,0 +1,48 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
enum class SafetyReason {
|
||||
None,
|
||||
InvalidArgument,
|
||||
Unauthenticated,
|
||||
PermissionDenied,
|
||||
RecoveryRpcDisabled,
|
||||
DeviceNotFound,
|
||||
DeviceUnavailable,
|
||||
UnsupportedCommand,
|
||||
SystemStarting,
|
||||
SystemStopping,
|
||||
SafetyLatched,
|
||||
SafetyStateMissing,
|
||||
SafetyStateStale,
|
||||
HardwareUnsafe,
|
||||
EmergencyStopActive,
|
||||
ProtectiveStopActive,
|
||||
DeviceDisconnected,
|
||||
DeviceFault,
|
||||
DeviceNotReady,
|
||||
DeviceStillMoving,
|
||||
ControlBusy,
|
||||
GenerationMismatch,
|
||||
CommandIdRequired,
|
||||
CommandIdConflict,
|
||||
ResultEvicted,
|
||||
LedgerExhausted,
|
||||
Backpressure,
|
||||
DeadlineExceededBeforeDispatch,
|
||||
OutcomeUnknown,
|
||||
ParticipantTimeout,
|
||||
StopUnconfirmed,
|
||||
RecoveryEpochMismatch,
|
||||
RecoveryReasonRequired,
|
||||
RecoveryAuditFailed,
|
||||
InternalError,
|
||||
};
|
||||
|
||||
const char* toString(SafetyReason reason) noexcept;
|
||||
bool retryWithSameCommandId(SafetyReason reason) noexcept;
|
||||
|
||||
} // namespace cmvr::safety
|
||||
@ -0,0 +1,54 @@
|
||||
#pragma once
|
||||
|
||||
#include <condition_variable>
|
||||
#include <optional>
|
||||
#include <shared_mutex>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "manager/safety_manager/include/safety_types.h"
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
class SafetySnapshotStore final {
|
||||
public:
|
||||
bool registerDevice(const DeviceSafetyDescriptor& descriptor,
|
||||
std::uint64_t initial_generation = 1);
|
||||
bool unregisterDevice(const std::string& device_id);
|
||||
bool publish(DeviceSafetySnapshot snapshot);
|
||||
bool markUnknown(const std::string& device_id,
|
||||
SafetyReason reason,
|
||||
std::string source_id = {});
|
||||
std::optional<std::uint64_t> bumpGeneration(
|
||||
const std::string& device_id);
|
||||
|
||||
SafetySnapshotView get(
|
||||
const std::string& device_id,
|
||||
SafetyClock::time_point now = SafetyClock::now()) const;
|
||||
std::vector<SafetySnapshotView> snapshot(
|
||||
SafetyClock::time_point now = SafetyClock::now()) const;
|
||||
|
||||
bool waitForNewerSample(
|
||||
const std::string& device_id,
|
||||
std::uint64_t previous_sequence,
|
||||
SafetyClock::time_point deadline,
|
||||
SafetySnapshotView& result) const;
|
||||
|
||||
private:
|
||||
struct Slot {
|
||||
DeviceSafetyDescriptor descriptor;
|
||||
DeviceSafetySnapshot snapshot;
|
||||
bool has_sample{false};
|
||||
};
|
||||
|
||||
static SafetySnapshotView viewOf_(
|
||||
const Slot& slot,
|
||||
SafetyClock::time_point now);
|
||||
|
||||
mutable std::shared_mutex mutex_;
|
||||
mutable std::condition_variable_any changed_;
|
||||
std::unordered_map<std::string, Slot> slots_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::safety
|
||||
235
cmvr-es/manager/safety_manager/include/safety_types.h
Normal file
235
cmvr-es/manager/safety_manager/include/safety_types.h
Normal file
@ -0,0 +1,235 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "devices/device_types.h"
|
||||
#include "manager/safety_manager/include/safety_reason.h"
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
using SafetyClock = std::chrono::steady_clock;
|
||||
|
||||
enum class TriState {
|
||||
Unknown,
|
||||
False,
|
||||
True,
|
||||
};
|
||||
|
||||
enum class SafetyCondition {
|
||||
Nominal,
|
||||
Restricted,
|
||||
Unsafe,
|
||||
Unknown,
|
||||
};
|
||||
|
||||
enum class CommandIntent {
|
||||
Observe,
|
||||
StartActivity,
|
||||
Configure,
|
||||
Actuate,
|
||||
Stop,
|
||||
ResetFault,
|
||||
RecoverAdmission,
|
||||
};
|
||||
|
||||
enum class SafetyPolicyFamily {
|
||||
Sensor,
|
||||
Control,
|
||||
};
|
||||
|
||||
enum class BlockerScope {
|
||||
Device,
|
||||
System,
|
||||
};
|
||||
|
||||
enum class RecoveryRequirement {
|
||||
RefreshOnly,
|
||||
ClearSoftwareLatch,
|
||||
HardwareReleaseRequired,
|
||||
ManualInspectionRequired,
|
||||
};
|
||||
|
||||
enum class SystemAdmissionState {
|
||||
Starting,
|
||||
Open,
|
||||
Stopping,
|
||||
Latched,
|
||||
Recovering,
|
||||
ShuttingDown,
|
||||
};
|
||||
|
||||
enum class DeviceAdmissionState {
|
||||
Observing,
|
||||
Open,
|
||||
Blocked,
|
||||
Quarantined,
|
||||
Recovering,
|
||||
Removed,
|
||||
};
|
||||
|
||||
enum class EnforcementMode {
|
||||
Legacy,
|
||||
Shadow,
|
||||
EnforceSelected,
|
||||
EnforceAll,
|
||||
};
|
||||
|
||||
enum class CommandLifecycle {
|
||||
Received,
|
||||
Reserved,
|
||||
RejectedBeforeDispatch,
|
||||
Admitted,
|
||||
Dispatching,
|
||||
AcceptedByHardware,
|
||||
Completed,
|
||||
Failed,
|
||||
CanceledBeforeDispatch,
|
||||
OutcomeUnknown,
|
||||
};
|
||||
|
||||
struct SafetyBlocker {
|
||||
SafetyReason reason{SafetyReason::None};
|
||||
BlockerScope scope{BlockerScope::Device};
|
||||
RecoveryRequirement recovery_requirement{
|
||||
RecoveryRequirement::RefreshOnly};
|
||||
std::string source_id;
|
||||
std::string operation_id;
|
||||
std::uint64_t first_observed_at_unix_ms{0};
|
||||
std::uint64_t last_observed_at_unix_ms{0};
|
||||
};
|
||||
|
||||
struct DeviceSafetySnapshot {
|
||||
std::string device_id;
|
||||
SafetyCondition condition{SafetyCondition::Unknown};
|
||||
std::uint64_t device_generation{0};
|
||||
std::uint64_t sample_sequence{0};
|
||||
SafetyClock::time_point observed_at{};
|
||||
std::uint64_t observed_at_unix_ms{0};
|
||||
|
||||
TriState connected{TriState::Unknown};
|
||||
TriState operational_ready{TriState::Unknown};
|
||||
TriState quiescent{TriState::Unknown};
|
||||
TriState motion_active{TriState::Unknown};
|
||||
TriState actuator_enabled{TriState::Unknown};
|
||||
TriState emergency_stop_active{TriState::Unknown};
|
||||
TriState protective_stop_active{TriState::Unknown};
|
||||
TriState fault_active{TriState::Unknown};
|
||||
|
||||
std::vector<SafetyBlocker> blockers;
|
||||
};
|
||||
|
||||
struct DeviceSafetyDescriptor {
|
||||
std::string device_id;
|
||||
device::DeviceKind kind{device::DeviceKind::Unknown};
|
||||
SafetyPolicyFamily default_policy{SafetyPolicyFamily::Sensor};
|
||||
std::chrono::milliseconds maximum_snapshot_age{1000};
|
||||
bool requires_safe_stop{false};
|
||||
bool supports_active_refresh{false};
|
||||
bool supports_non_enabling_fault_reset{false};
|
||||
};
|
||||
|
||||
struct SafetySnapshotView {
|
||||
DeviceSafetyDescriptor descriptor;
|
||||
DeviceSafetySnapshot snapshot;
|
||||
bool registered{false};
|
||||
bool has_sample{false};
|
||||
bool fresh{false};
|
||||
std::chrono::milliseconds sample_age{
|
||||
std::chrono::milliseconds::max()};
|
||||
};
|
||||
|
||||
struct CommandActor {
|
||||
std::string principal_id{"anonymous"};
|
||||
bool authenticated{false};
|
||||
std::vector<std::string> roles;
|
||||
};
|
||||
|
||||
struct CommandDescriptor {
|
||||
std::string full_method_name;
|
||||
CommandIntent intent{CommandIntent::Observe};
|
||||
SafetyPolicyFamily policy_family{SafetyPolicyFamily::Sensor};
|
||||
bool mutating{false};
|
||||
bool safety_lane{false};
|
||||
};
|
||||
|
||||
struct AdmissionRequest {
|
||||
CommandDescriptor command;
|
||||
CommandActor actor;
|
||||
std::string command_id;
|
||||
std::string device_id;
|
||||
std::optional<std::uint64_t> expected_device_generation;
|
||||
std::uint64_t authority_generation{0};
|
||||
SafetyClock::time_point deadline{SafetyClock::time_point::max()};
|
||||
};
|
||||
|
||||
struct AdmissionDecision {
|
||||
bool allowed{false};
|
||||
bool policy_allowed{false};
|
||||
bool enforced{false};
|
||||
SafetyReason reason{SafetyReason::None};
|
||||
std::string detail;
|
||||
std::uint64_t safety_epoch{0};
|
||||
std::uint64_t device_generation{0};
|
||||
};
|
||||
|
||||
struct AdmissionPermit {
|
||||
AdmissionPermit() = default;
|
||||
AdmissionPermit(AdmissionPermit&&) noexcept = default;
|
||||
AdmissionPermit& operator=(AdmissionPermit&&) noexcept = default;
|
||||
AdmissionPermit(const AdmissionPermit&) = delete;
|
||||
AdmissionPermit& operator=(const AdmissionPermit&) = delete;
|
||||
|
||||
std::string command_id;
|
||||
std::string device_id;
|
||||
CommandIntent intent{CommandIntent::Observe};
|
||||
std::uint64_t safety_epoch{0};
|
||||
std::uint64_t device_generation{0};
|
||||
std::uint64_t authority_generation{0};
|
||||
SafetyClock::time_point deadline{SafetyClock::time_point::max()};
|
||||
bool policy_allowed{false};
|
||||
bool enforced{false};
|
||||
};
|
||||
|
||||
struct HardwareCheckResult {
|
||||
bool safe{false};
|
||||
SafetyReason reason{SafetyReason::SafetyStateMissing};
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
struct RecoveryContext {
|
||||
std::string recovery_id;
|
||||
std::string reason;
|
||||
std::uint64_t safety_epoch{0};
|
||||
SafetyClock::time_point deadline{SafetyClock::time_point::max()};
|
||||
bool verify_only{true};
|
||||
};
|
||||
|
||||
struct RecoveryCheckResult {
|
||||
bool reconciled{false};
|
||||
SafetyReason reason{SafetyReason::None};
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
struct SafetyEvent {
|
||||
std::uint64_t sequence{0};
|
||||
std::uint64_t safety_epoch{0};
|
||||
std::uint64_t occurred_at_unix_ms{0};
|
||||
std::string source_id;
|
||||
std::string operation_id;
|
||||
SafetyReason reason{SafetyReason::None};
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
const char* toString(TriState value) noexcept;
|
||||
const char* toString(SafetyCondition value) noexcept;
|
||||
const char* toString(CommandIntent value) noexcept;
|
||||
const char* toString(SafetyPolicyFamily value) noexcept;
|
||||
const char* toString(SystemAdmissionState value) noexcept;
|
||||
const char* toString(DeviceAdmissionState value) noexcept;
|
||||
const char* toString(EnforcementMode value) noexcept;
|
||||
|
||||
} // namespace cmvr::safety
|
||||
276
cmvr-es/manager/safety_manager/src/command_ledger.cpp
Normal file
276
cmvr-es/manager/safety_manager/src/command_ledger.cpp
Normal file
@ -0,0 +1,276 @@
|
||||
#include "manager/safety_manager/include/command_ledger.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cctype>
|
||||
#include <functional>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr std::size_t kMaxPrincipalIdLength = 256;
|
||||
constexpr std::size_t kMaxCommandIdLength = 128;
|
||||
|
||||
bool validIdentifier(const std::string& value, const std::size_t maximum)
|
||||
{
|
||||
if (value.empty() || value.size() > maximum) {
|
||||
return false;
|
||||
}
|
||||
return std::all_of(
|
||||
value.begin(), value.end(), [](const unsigned char character) {
|
||||
return std::isalnum(character) || character == '-' ||
|
||||
character == '_' || character == '.' ||
|
||||
character == ':' || character == '/';
|
||||
});
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
CommandLedger::CommandLedger()
|
||||
: CommandLedger(Config{})
|
||||
{
|
||||
}
|
||||
|
||||
CommandLedger::CommandLedger(Config config)
|
||||
: config_(config)
|
||||
{
|
||||
if (config_.total_id_capacity == 0 ||
|
||||
config_.result_capacity > config_.total_id_capacity) {
|
||||
throw std::invalid_argument("invalid CommandLedger capacity");
|
||||
}
|
||||
}
|
||||
|
||||
std::size_t CommandLedger::KeyHash::operator()(
|
||||
const CommandKey& key) const noexcept
|
||||
{
|
||||
const auto first = std::hash<std::string>{}(key.effective_principal_id);
|
||||
const auto second = std::hash<std::string>{}(key.command_id);
|
||||
return first ^ (second + 0x9e3779b9U + (first << 6U) + (first >> 2U));
|
||||
}
|
||||
|
||||
bool CommandLedger::validKey_(const CommandKey& key) noexcept
|
||||
{
|
||||
return validIdentifier(
|
||||
key.effective_principal_id, kMaxPrincipalIdLength) &&
|
||||
validIdentifier(key.command_id, kMaxCommandIdLength);
|
||||
}
|
||||
|
||||
CommandLedger::Reservation CommandLedger::reserve(
|
||||
CommandKey key,
|
||||
std::string payload_hash)
|
||||
{
|
||||
if (!validKey_(key) || payload_hash.empty()) {
|
||||
return {};
|
||||
}
|
||||
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto live = records_.find(key);
|
||||
if (live != records_.end()) {
|
||||
const auto& state = live->second;
|
||||
std::lock_guard state_lock(state->mutex);
|
||||
if (state->payload_hash != payload_hash) {
|
||||
return {CommandReservationStatus::CommandIdConflict, {}, {}};
|
||||
}
|
||||
if (state->terminal && state->outcome.has_value()) {
|
||||
return {
|
||||
CommandReservationStatus::CachedResult,
|
||||
Ticket(state),
|
||||
state->outcome};
|
||||
}
|
||||
return {
|
||||
CommandReservationStatus::JoinedInFlight,
|
||||
Ticket(state),
|
||||
{}};
|
||||
}
|
||||
|
||||
const auto retired = retired_ids_.find(key);
|
||||
if (retired != retired_ids_.end()) {
|
||||
return {
|
||||
retired->second == payload_hash
|
||||
? CommandReservationStatus::ResultEvicted
|
||||
: CommandReservationStatus::CommandIdConflict,
|
||||
{},
|
||||
{}};
|
||||
}
|
||||
|
||||
if (records_.size() + retired_ids_.size() >=
|
||||
config_.total_id_capacity) {
|
||||
return {CommandReservationStatus::LedgerExhausted, {}, {}};
|
||||
}
|
||||
|
||||
auto state = std::make_shared<State>();
|
||||
state->key = std::move(key);
|
||||
state->payload_hash = std::move(payload_hash);
|
||||
const auto inserted = records_.emplace(state->key, state);
|
||||
if (!inserted.second) {
|
||||
throw std::logic_error("CommandLedger duplicate insertion");
|
||||
}
|
||||
return {
|
||||
CommandReservationStatus::AcceptedNew,
|
||||
Ticket(std::move(state)),
|
||||
{}};
|
||||
}
|
||||
|
||||
bool CommandLedger::setLifecycle(
|
||||
const Ticket& ticket,
|
||||
const CommandLifecycle lifecycle,
|
||||
const std::uint64_t safety_epoch,
|
||||
const std::uint64_t device_generation,
|
||||
const bool hardware_submission_possible)
|
||||
{
|
||||
if (!ticket.valid()) {
|
||||
return false;
|
||||
}
|
||||
std::lock_guard ledger_lock(mutex_);
|
||||
const auto found = records_.find(ticket.state_->key);
|
||||
if (found == records_.end() || found->second != ticket.state_) {
|
||||
return false;
|
||||
}
|
||||
std::lock_guard state_lock(ticket.state_->mutex);
|
||||
if (ticket.state_->terminal) {
|
||||
return false;
|
||||
}
|
||||
ticket.state_->lifecycle = lifecycle;
|
||||
ticket.state_->safety_epoch = safety_epoch;
|
||||
ticket.state_->device_generation = device_generation;
|
||||
ticket.state_->hardware_submission_possible =
|
||||
ticket.state_->hardware_submission_possible ||
|
||||
hardware_submission_possible;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CommandLedger::complete(const Ticket& ticket, CommandOutcome outcome)
|
||||
{
|
||||
if (!ticket.valid()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::lock_guard ledger_lock(mutex_);
|
||||
const auto found = records_.find(ticket.state_->key);
|
||||
if (found == records_.end() || found->second != ticket.state_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard state_lock(ticket.state_->mutex);
|
||||
if (ticket.state_->terminal) {
|
||||
return false;
|
||||
}
|
||||
outcome.hardware_submission_possible =
|
||||
outcome.hardware_submission_possible ||
|
||||
ticket.state_->hardware_submission_possible;
|
||||
if (outcome.safety_epoch == 0) {
|
||||
outcome.safety_epoch = ticket.state_->safety_epoch;
|
||||
}
|
||||
if (outcome.device_generation == 0) {
|
||||
outcome.device_generation = ticket.state_->device_generation;
|
||||
}
|
||||
ticket.state_->lifecycle = outcome.lifecycle;
|
||||
ticket.state_->outcome = std::move(outcome);
|
||||
ticket.state_->terminal = true;
|
||||
}
|
||||
ticket.state_->condition.notify_all();
|
||||
terminal_order_.push_back(ticket.state_->key);
|
||||
++terminal_result_count_;
|
||||
trimTerminalResultsLocked_();
|
||||
return true;
|
||||
}
|
||||
|
||||
void CommandLedger::trimTerminalResultsLocked_()
|
||||
{
|
||||
std::size_t consumed = 0;
|
||||
while (terminal_result_count_ > config_.result_capacity &&
|
||||
consumed < terminal_order_.size()) {
|
||||
const auto key = terminal_order_[consumed++];
|
||||
const auto found = records_.find(key);
|
||||
if (found == records_.end()) {
|
||||
continue;
|
||||
}
|
||||
const auto& state = found->second;
|
||||
std::lock_guard state_lock(state->mutex);
|
||||
if (!state->terminal) {
|
||||
continue;
|
||||
}
|
||||
retired_ids_.emplace(state->key, state->payload_hash);
|
||||
records_.erase(found);
|
||||
--terminal_result_count_;
|
||||
}
|
||||
if (consumed != 0) {
|
||||
terminal_order_.erase(
|
||||
terminal_order_.begin(),
|
||||
terminal_order_.begin() + static_cast<std::ptrdiff_t>(consumed));
|
||||
}
|
||||
}
|
||||
|
||||
std::optional<CommandOutcome> CommandLedger::wait(
|
||||
const Ticket& ticket,
|
||||
const SafetyClock::time_point deadline) const
|
||||
{
|
||||
if (!ticket.valid()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
std::unique_lock lock(ticket.state_->mutex);
|
||||
if (deadline == SafetyClock::time_point::max()) {
|
||||
ticket.state_->condition.wait(
|
||||
lock, [&ticket] { return ticket.state_->terminal; });
|
||||
} else if (!ticket.state_->condition.wait_until(
|
||||
lock, deadline,
|
||||
[&ticket] { return ticket.state_->terminal; })) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return ticket.state_->outcome;
|
||||
}
|
||||
|
||||
std::optional<CommandOutcome> CommandLedger::lookup(
|
||||
const CommandKey& key,
|
||||
const std::string& payload_hash) const
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
const auto found = records_.find(key);
|
||||
if (found == records_.end()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
std::lock_guard state_lock(found->second->mutex);
|
||||
if (found->second->payload_hash != payload_hash ||
|
||||
!found->second->terminal) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return found->second->outcome;
|
||||
}
|
||||
|
||||
std::size_t CommandLedger::acceptedIdCount() const
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
return records_.size() + retired_ids_.size();
|
||||
}
|
||||
|
||||
std::size_t CommandLedger::liveRecordCount() const
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
return records_.size();
|
||||
}
|
||||
|
||||
std::size_t CommandLedger::retiredIdCount() const
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
return retired_ids_.size();
|
||||
}
|
||||
|
||||
const char* toString(const CommandReservationStatus status) noexcept
|
||||
{
|
||||
switch (status) {
|
||||
case CommandReservationStatus::AcceptedNew: return "AcceptedNew";
|
||||
case CommandReservationStatus::JoinedInFlight: return "JoinedInFlight";
|
||||
case CommandReservationStatus::CachedResult: return "CachedResult";
|
||||
case CommandReservationStatus::CommandIdConflict:
|
||||
return "CommandIdConflict";
|
||||
case CommandReservationStatus::ResultEvicted: return "ResultEvicted";
|
||||
case CommandReservationStatus::LedgerExhausted: return "LedgerExhausted";
|
||||
case CommandReservationStatus::Invalid: return "Invalid";
|
||||
}
|
||||
return "Invalid";
|
||||
}
|
||||
|
||||
} // namespace cmvr::safety
|
||||
2518
cmvr-es/manager/safety_manager/src/safety_manager.cpp
Normal file
2518
cmvr-es/manager/safety_manager/src/safety_manager.cpp
Normal file
File diff suppressed because it is too large
Load Diff
92
cmvr-es/manager/safety_manager/src/safety_reason.cpp
Normal file
92
cmvr-es/manager/safety_manager/src/safety_reason.cpp
Normal file
@ -0,0 +1,92 @@
|
||||
#include "manager/safety_manager/include/safety_reason.h"
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
const char* toString(const SafetyReason reason) noexcept
|
||||
{
|
||||
switch (reason) {
|
||||
case SafetyReason::None: return "NONE";
|
||||
case SafetyReason::InvalidArgument: return "INVALID_ARGUMENT";
|
||||
case SafetyReason::Unauthenticated: return "UNAUTHENTICATED";
|
||||
case SafetyReason::PermissionDenied: return "PERMISSION_DENIED";
|
||||
case SafetyReason::RecoveryRpcDisabled: return "RECOVERY_RPC_DISABLED";
|
||||
case SafetyReason::DeviceNotFound: return "DEVICE_NOT_FOUND";
|
||||
case SafetyReason::DeviceUnavailable: return "DEVICE_UNAVAILABLE";
|
||||
case SafetyReason::UnsupportedCommand: return "UNSUPPORTED_COMMAND";
|
||||
case SafetyReason::SystemStarting: return "SYSTEM_STARTING";
|
||||
case SafetyReason::SystemStopping: return "SYSTEM_STOPPING";
|
||||
case SafetyReason::SafetyLatched: return "SAFETY_LATCHED";
|
||||
case SafetyReason::SafetyStateMissing: return "SAFETY_STATE_MISSING";
|
||||
case SafetyReason::SafetyStateStale: return "SAFETY_STATE_STALE";
|
||||
case SafetyReason::HardwareUnsafe: return "HARDWARE_UNSAFE";
|
||||
case SafetyReason::EmergencyStopActive: return "EMERGENCY_STOP_ACTIVE";
|
||||
case SafetyReason::ProtectiveStopActive: return "PROTECTIVE_STOP_ACTIVE";
|
||||
case SafetyReason::DeviceDisconnected: return "DEVICE_DISCONNECTED";
|
||||
case SafetyReason::DeviceFault: return "DEVICE_FAULT";
|
||||
case SafetyReason::DeviceNotReady: return "DEVICE_NOT_READY";
|
||||
case SafetyReason::DeviceStillMoving: return "DEVICE_STILL_MOVING";
|
||||
case SafetyReason::ControlBusy: return "CONTROL_BUSY";
|
||||
case SafetyReason::GenerationMismatch: return "GENERATION_MISMATCH";
|
||||
case SafetyReason::CommandIdRequired: return "COMMAND_ID_REQUIRED";
|
||||
case SafetyReason::CommandIdConflict: return "COMMAND_ID_CONFLICT";
|
||||
case SafetyReason::ResultEvicted: return "RESULT_EVICTED";
|
||||
case SafetyReason::LedgerExhausted: return "LEDGER_EXHAUSTED";
|
||||
case SafetyReason::Backpressure: return "BACKPRESSURE";
|
||||
case SafetyReason::DeadlineExceededBeforeDispatch:
|
||||
return "DEADLINE_EXCEEDED_BEFORE_DISPATCH";
|
||||
case SafetyReason::OutcomeUnknown: return "OUTCOME_UNKNOWN";
|
||||
case SafetyReason::ParticipantTimeout: return "PARTICIPANT_TIMEOUT";
|
||||
case SafetyReason::StopUnconfirmed: return "STOP_UNCONFIRMED";
|
||||
case SafetyReason::RecoveryEpochMismatch: return "RECOVERY_EPOCH_MISMATCH";
|
||||
case SafetyReason::RecoveryReasonRequired: return "RECOVERY_REASON_REQUIRED";
|
||||
case SafetyReason::RecoveryAuditFailed: return "RECOVERY_AUDIT_FAILED";
|
||||
case SafetyReason::InternalError: return "INTERNAL_ERROR";
|
||||
}
|
||||
return "INTERNAL_ERROR";
|
||||
}
|
||||
|
||||
bool retryWithSameCommandId(const SafetyReason reason) noexcept
|
||||
{
|
||||
switch (reason) {
|
||||
case SafetyReason::RecoveryRpcDisabled:
|
||||
case SafetyReason::SystemStopping:
|
||||
return true;
|
||||
case SafetyReason::None:
|
||||
case SafetyReason::InvalidArgument:
|
||||
case SafetyReason::Unauthenticated:
|
||||
case SafetyReason::PermissionDenied:
|
||||
case SafetyReason::DeviceNotFound:
|
||||
case SafetyReason::DeviceUnavailable:
|
||||
case SafetyReason::UnsupportedCommand:
|
||||
case SafetyReason::SystemStarting:
|
||||
case SafetyReason::SafetyLatched:
|
||||
case SafetyReason::SafetyStateMissing:
|
||||
case SafetyReason::SafetyStateStale:
|
||||
case SafetyReason::HardwareUnsafe:
|
||||
case SafetyReason::EmergencyStopActive:
|
||||
case SafetyReason::ProtectiveStopActive:
|
||||
case SafetyReason::DeviceDisconnected:
|
||||
case SafetyReason::DeviceFault:
|
||||
case SafetyReason::DeviceNotReady:
|
||||
case SafetyReason::DeviceStillMoving:
|
||||
case SafetyReason::ControlBusy:
|
||||
case SafetyReason::GenerationMismatch:
|
||||
case SafetyReason::CommandIdRequired:
|
||||
case SafetyReason::CommandIdConflict:
|
||||
case SafetyReason::ResultEvicted:
|
||||
case SafetyReason::LedgerExhausted:
|
||||
case SafetyReason::Backpressure:
|
||||
case SafetyReason::DeadlineExceededBeforeDispatch:
|
||||
case SafetyReason::OutcomeUnknown:
|
||||
case SafetyReason::ParticipantTimeout:
|
||||
case SafetyReason::StopUnconfirmed:
|
||||
case SafetyReason::RecoveryEpochMismatch:
|
||||
case SafetyReason::RecoveryReasonRequired:
|
||||
case SafetyReason::RecoveryAuditFailed:
|
||||
case SafetyReason::InternalError:
|
||||
return false;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace cmvr::safety
|
||||
305
cmvr-es/manager/safety_manager/src/safety_snapshot_store.cpp
Normal file
305
cmvr-es/manager/safety_manager/src/safety_snapshot_store.cpp
Normal file
@ -0,0 +1,305 @@
|
||||
#include "manager/safety_manager/include/safety_snapshot_store.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <mutex>
|
||||
#include <utility>
|
||||
|
||||
namespace cmvr::safety {
|
||||
|
||||
namespace {
|
||||
|
||||
std::uint64_t unixTimeMs() noexcept
|
||||
{
|
||||
const auto value = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
return value > 0 ? static_cast<std::uint64_t>(value) : 1U;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
bool SafetySnapshotStore::registerDevice(
|
||||
const DeviceSafetyDescriptor& descriptor,
|
||||
const std::uint64_t initial_generation)
|
||||
{
|
||||
if (descriptor.device_id.empty() ||
|
||||
descriptor.maximum_snapshot_age <= std::chrono::milliseconds::zero() ||
|
||||
initial_generation == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
Slot slot;
|
||||
slot.descriptor = descriptor;
|
||||
slot.snapshot.device_id = descriptor.device_id;
|
||||
slot.snapshot.device_generation = initial_generation;
|
||||
slot.snapshot.condition = SafetyCondition::Unknown;
|
||||
|
||||
std::unique_lock lock(mutex_);
|
||||
const auto inserted = slots_.emplace(descriptor.device_id, std::move(slot));
|
||||
if (inserted.second) {
|
||||
changed_.notify_all();
|
||||
}
|
||||
return inserted.second;
|
||||
}
|
||||
|
||||
bool SafetySnapshotStore::unregisterDevice(const std::string& device_id)
|
||||
{
|
||||
std::unique_lock lock(mutex_);
|
||||
const bool removed = slots_.erase(device_id) != 0;
|
||||
if (removed) {
|
||||
changed_.notify_all();
|
||||
}
|
||||
return removed;
|
||||
}
|
||||
|
||||
bool SafetySnapshotStore::publish(DeviceSafetySnapshot snapshot)
|
||||
{
|
||||
if (snapshot.device_id.empty() || snapshot.device_generation == 0 ||
|
||||
snapshot.sample_sequence == 0 ||
|
||||
snapshot.observed_at == SafetyClock::time_point{}) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::unique_lock lock(mutex_);
|
||||
const auto found = slots_.find(snapshot.device_id);
|
||||
if (found == slots_.end()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto& slot = found->second;
|
||||
const auto current_generation = slot.snapshot.device_generation;
|
||||
if (snapshot.device_generation < current_generation) {
|
||||
return false;
|
||||
}
|
||||
if (snapshot.device_generation == current_generation &&
|
||||
slot.has_sample &&
|
||||
snapshot.sample_sequence <= slot.snapshot.sample_sequence) {
|
||||
return false;
|
||||
}
|
||||
if (snapshot.observed_at_unix_ms == 0) {
|
||||
snapshot.observed_at_unix_ms = unixTimeMs();
|
||||
}
|
||||
slot.snapshot = std::move(snapshot);
|
||||
slot.has_sample = true;
|
||||
changed_.notify_all();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SafetySnapshotStore::markUnknown(
|
||||
const std::string& device_id,
|
||||
const SafetyReason reason,
|
||||
std::string source_id)
|
||||
{
|
||||
std::unique_lock lock(mutex_);
|
||||
const auto found = slots_.find(device_id);
|
||||
if (found == slots_.end()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto& slot = found->second;
|
||||
DeviceSafetySnapshot snapshot;
|
||||
snapshot.device_id = device_id;
|
||||
snapshot.device_generation = slot.snapshot.device_generation;
|
||||
snapshot.sample_sequence = slot.snapshot.sample_sequence + 1U;
|
||||
if (snapshot.sample_sequence == 0) {
|
||||
snapshot.sample_sequence = 1U;
|
||||
}
|
||||
snapshot.observed_at = SafetyClock::now();
|
||||
snapshot.observed_at_unix_ms = unixTimeMs();
|
||||
snapshot.condition = SafetyCondition::Unknown;
|
||||
snapshot.blockers.push_back(SafetyBlocker{
|
||||
reason,
|
||||
BlockerScope::Device,
|
||||
RecoveryRequirement::RefreshOnly,
|
||||
source_id.empty() ? device_id : std::move(source_id),
|
||||
{},
|
||||
snapshot.observed_at_unix_ms,
|
||||
snapshot.observed_at_unix_ms});
|
||||
slot.snapshot = std::move(snapshot);
|
||||
slot.has_sample = true;
|
||||
changed_.notify_all();
|
||||
return true;
|
||||
}
|
||||
|
||||
std::optional<std::uint64_t> SafetySnapshotStore::bumpGeneration(
|
||||
const std::string& device_id)
|
||||
{
|
||||
std::unique_lock lock(mutex_);
|
||||
const auto found = slots_.find(device_id);
|
||||
if (found == slots_.end()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
auto& slot = found->second;
|
||||
if (slot.snapshot.device_generation ==
|
||||
std::numeric_limits<std::uint64_t>::max()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
++slot.snapshot.device_generation;
|
||||
slot.snapshot.sample_sequence = 0;
|
||||
slot.snapshot.observed_at = {};
|
||||
slot.snapshot.observed_at_unix_ms = 0;
|
||||
slot.snapshot.condition = SafetyCondition::Unknown;
|
||||
slot.snapshot.blockers.clear();
|
||||
slot.has_sample = false;
|
||||
changed_.notify_all();
|
||||
return slot.snapshot.device_generation;
|
||||
}
|
||||
|
||||
SafetySnapshotView SafetySnapshotStore::viewOf_(
|
||||
const Slot& slot,
|
||||
const SafetyClock::time_point now)
|
||||
{
|
||||
SafetySnapshotView view;
|
||||
view.descriptor = slot.descriptor;
|
||||
view.snapshot = slot.snapshot;
|
||||
view.registered = true;
|
||||
view.has_sample = slot.has_sample;
|
||||
if (!slot.has_sample ||
|
||||
slot.snapshot.observed_at == SafetyClock::time_point{}) {
|
||||
return view;
|
||||
}
|
||||
|
||||
const auto elapsed = now <= slot.snapshot.observed_at
|
||||
? SafetyClock::duration::zero()
|
||||
: now - slot.snapshot.observed_at;
|
||||
view.sample_age = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
elapsed);
|
||||
view.fresh = view.sample_age <= slot.descriptor.maximum_snapshot_age;
|
||||
return view;
|
||||
}
|
||||
|
||||
SafetySnapshotView SafetySnapshotStore::get(
|
||||
const std::string& device_id,
|
||||
const SafetyClock::time_point now) const
|
||||
{
|
||||
std::shared_lock lock(mutex_);
|
||||
const auto found = slots_.find(device_id);
|
||||
if (found == slots_.end()) {
|
||||
return {};
|
||||
}
|
||||
return viewOf_(found->second, now);
|
||||
}
|
||||
|
||||
std::vector<SafetySnapshotView> SafetySnapshotStore::snapshot(
|
||||
const SafetyClock::time_point now) const
|
||||
{
|
||||
std::vector<SafetySnapshotView> result;
|
||||
std::shared_lock lock(mutex_);
|
||||
result.reserve(slots_.size());
|
||||
for (const auto& [id, slot] : slots_) {
|
||||
(void)id;
|
||||
result.push_back(viewOf_(slot, now));
|
||||
}
|
||||
std::sort(result.begin(), result.end(), [](const auto& lhs, const auto& rhs) {
|
||||
return lhs.descriptor.device_id < rhs.descriptor.device_id;
|
||||
});
|
||||
return result;
|
||||
}
|
||||
|
||||
bool SafetySnapshotStore::waitForNewerSample(
|
||||
const std::string& device_id,
|
||||
const std::uint64_t previous_sequence,
|
||||
const SafetyClock::time_point deadline,
|
||||
SafetySnapshotView& result) const
|
||||
{
|
||||
std::unique_lock lock(mutex_);
|
||||
const auto ready = [&]() {
|
||||
const auto found = slots_.find(device_id);
|
||||
return found == slots_.end() ||
|
||||
(found->second.has_sample &&
|
||||
found->second.snapshot.sample_sequence > previous_sequence);
|
||||
};
|
||||
if (!changed_.wait_until(lock, deadline, ready)) {
|
||||
return false;
|
||||
}
|
||||
const auto found = slots_.find(device_id);
|
||||
if (found == slots_.end()) {
|
||||
return false;
|
||||
}
|
||||
result = viewOf_(found->second, SafetyClock::now());
|
||||
return result.has_sample &&
|
||||
result.snapshot.sample_sequence > previous_sequence;
|
||||
}
|
||||
|
||||
const char* toString(const TriState value) noexcept
|
||||
{
|
||||
switch (value) {
|
||||
case TriState::Unknown: return "Unknown";
|
||||
case TriState::False: return "False";
|
||||
case TriState::True: return "True";
|
||||
}
|
||||
return "Unknown";
|
||||
}
|
||||
|
||||
const char* toString(const SafetyCondition value) noexcept
|
||||
{
|
||||
switch (value) {
|
||||
case SafetyCondition::Nominal: return "Nominal";
|
||||
case SafetyCondition::Restricted: return "Restricted";
|
||||
case SafetyCondition::Unsafe: return "Unsafe";
|
||||
case SafetyCondition::Unknown: return "Unknown";
|
||||
}
|
||||
return "Unknown";
|
||||
}
|
||||
|
||||
const char* toString(const CommandIntent value) noexcept
|
||||
{
|
||||
switch (value) {
|
||||
case CommandIntent::Observe: return "Observe";
|
||||
case CommandIntent::StartActivity: return "StartActivity";
|
||||
case CommandIntent::Configure: return "Configure";
|
||||
case CommandIntent::Actuate: return "Actuate";
|
||||
case CommandIntent::Stop: return "Stop";
|
||||
case CommandIntent::ResetFault: return "ResetFault";
|
||||
case CommandIntent::RecoverAdmission: return "RecoverAdmission";
|
||||
}
|
||||
return "Observe";
|
||||
}
|
||||
|
||||
const char* toString(const SafetyPolicyFamily value) noexcept
|
||||
{
|
||||
switch (value) {
|
||||
case SafetyPolicyFamily::Sensor: return "Sensor";
|
||||
case SafetyPolicyFamily::Control: return "Control";
|
||||
}
|
||||
return "Sensor";
|
||||
}
|
||||
|
||||
const char* toString(const SystemAdmissionState value) noexcept
|
||||
{
|
||||
switch (value) {
|
||||
case SystemAdmissionState::Starting: return "Starting";
|
||||
case SystemAdmissionState::Open: return "Open";
|
||||
case SystemAdmissionState::Stopping: return "Stopping";
|
||||
case SystemAdmissionState::Latched: return "Latched";
|
||||
case SystemAdmissionState::Recovering: return "Recovering";
|
||||
case SystemAdmissionState::ShuttingDown: return "ShuttingDown";
|
||||
}
|
||||
return "Starting";
|
||||
}
|
||||
|
||||
const char* toString(const DeviceAdmissionState value) noexcept
|
||||
{
|
||||
switch (value) {
|
||||
case DeviceAdmissionState::Observing: return "Observing";
|
||||
case DeviceAdmissionState::Open: return "Open";
|
||||
case DeviceAdmissionState::Blocked: return "Blocked";
|
||||
case DeviceAdmissionState::Quarantined: return "Quarantined";
|
||||
case DeviceAdmissionState::Recovering: return "Recovering";
|
||||
case DeviceAdmissionState::Removed: return "Removed";
|
||||
}
|
||||
return "Observing";
|
||||
}
|
||||
|
||||
const char* toString(const EnforcementMode value) noexcept
|
||||
{
|
||||
switch (value) {
|
||||
case EnforcementMode::Legacy: return "Legacy";
|
||||
case EnforcementMode::Shadow: return "Shadow";
|
||||
case EnforcementMode::EnforceSelected: return "EnforceSelected";
|
||||
case EnforcementMode::EnforceAll: return "EnforceAll";
|
||||
}
|
||||
return "Legacy";
|
||||
}
|
||||
|
||||
} // namespace cmvr::safety
|
||||
@ -1,102 +1,4 @@
|
||||
|
||||
add_library(service
|
||||
grpc/src/grpc_camera_service.cpp
|
||||
grpc/src/grpc_system_service.cpp
|
||||
grpc/src/grpc_speaker_service.cpp
|
||||
grpc/src/grpc_microphone_service.cpp
|
||||
grpc/src/grpc_head_service.cpp
|
||||
grpc/src/grpc_dexhand_service.cpp
|
||||
grpc/src/grpc_arm_service.cpp
|
||||
grpc/src/grpc_agv_service.cpp
|
||||
grpc/src/grpc_hlc_service.cpp
|
||||
../task/grpc_server_task/src/grpc_server_task.cpp
|
||||
)
|
||||
|
||||
target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
target_link_libraries(service PRIVATE
|
||||
cmvr_es::proto
|
||||
osqp
|
||||
cmvr_es::device_manager
|
||||
cmvr_es::device::microphone
|
||||
cmvr_es::task_manager
|
||||
cmvr_es::algorithms::controller
|
||||
cmvr_es::task
|
||||
cmvr_es::media_source_hub
|
||||
cmvr_es::device_media_source_adapter
|
||||
protobuf::libprotobuf
|
||||
)
|
||||
|
||||
add_library(cmvr_es::service ALIAS service)
|
||||
install(TARGETS service LIBRARY DESTINATION lib)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
add_executable(grpc_camera_stream_policy_test
|
||||
grpc/tests/grpc_camera_stream_policy_test.cpp
|
||||
)
|
||||
target_include_directories(grpc_camera_stream_policy_test
|
||||
PRIVATE
|
||||
${CMAKE_SOURCE_DIR}/cmvr-es
|
||||
)
|
||||
add_test(
|
||||
NAME grpc_camera_stream_policy_test
|
||||
COMMAND grpc_camera_stream_policy_test
|
||||
)
|
||||
set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10)
|
||||
endif()
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Unit test
|
||||
# --------------------------------------------------------
|
||||
find_package(OpenCV REQUIRED)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
|
||||
)
|
||||
|
||||
link_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
|
||||
)
|
||||
|
||||
|
||||
add_executable(grpc_arm_client_test
|
||||
grpc/src/grpc_arm_client_test.cpp
|
||||
)
|
||||
|
||||
|
||||
target_link_libraries(grpc_arm_client_test
|
||||
PRIVATE
|
||||
cmvr_es::device::canbus
|
||||
cmvr_es::device::ti5_canopen_motor_driver
|
||||
osqp
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog
|
||||
cmvr_es::proto
|
||||
ccd
|
||||
fcl
|
||||
cmvr_es::device_manager
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
|
||||
|
||||
add_executable(grpc_hlc_client_test
|
||||
grpc/src/grpc_hlc_client_test.cpp
|
||||
)
|
||||
|
||||
|
||||
target_link_libraries(grpc_hlc_client_test
|
||||
PRIVATE
|
||||
cmvr_es::device::canbus
|
||||
cmvr_es::device::ti5_canopen_motor_driver
|
||||
osqp
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog
|
||||
cmvr_es::proto
|
||||
ccd
|
||||
fcl
|
||||
cmvr_es::device_manager
|
||||
)
|
||||
# Service is intentionally split by transport. The QUIC edge target is added
|
||||
# from the project root before task targets; the gRPC tree is added here after
|
||||
# its manager and task dependencies are available.
|
||||
add_subdirectory(grpc)
|
||||
|
||||
@ -6,13 +6,33 @@
|
||||
|
||||
## 当前结构
|
||||
|
||||
`service/` 顶层只按传输协议保留两个子目录：`grpc/` 和 `quic_edge/`。gRPC
|
||||
内部再按运行角色分层，避免把队列、停止控制、客户端和服务端实现混在同一层。
|
||||
|
||||
```text
|
||||
service/
|
||||
├── grpc/
|
||||
│ ├── action/ # ActionQueue 校验、账本和 FIFO 执行器
|
||||
│ ├── client/ # 边缘端使用的 gRPC client
|
||||
│ ├── server/ # gRPC service、协调器和安全扩展
|
||||
│ │ ├── include/
|
||||
│ │ ├── src/
|
||||
│ │ └── tests/
|
||||
│ └── stop_all/ # 高优先级 StopAll 通道
|
||||
└── quic_edge/ # QUIC client、控制状态机和媒体 packetizer
|
||||
```
|
||||
|
||||
| 目录 | 职责 |
|
||||
| --- | --- |
|
||||
| `grpc/` | 入站设备控制、状态查询和兼容流式接口 |
|
||||
| `grpc/action/` | SystemService ActionQueue 的校验、幂等账本和边缘端 FIFO 执行器 |
|
||||
| `grpc/client/` | 面向边缘端内部调用的 gRPC client |
|
||||
| `grpc/server/` | 入站设备控制、状态查询、兼容流式接口和安全控制面 |
|
||||
| `grpc/stop_all/` | 不进入普通命令队列的高优先级停止通道 |
|
||||
| `quic_edge/` | 边缘端主动连接平台的 QUIC client、控制状态机和媒体 packetizer |
|
||||
| `quic_edge/tests/` | 已登记到 CTest 的 QUIC 协议测试 |
|
||||
|
||||
两个遗留 gRPC client test 位于 `grpc/src/*_client_test.cpp`，当前没有通过 `add_test()` 登记。
|
||||
两个遗留 gRPC client test 位于 `grpc/server/tests/*_client_test.cpp`，当前没有通过
|
||||
`add_test()` 登记；它们是历史可执行文件，不代表默认自动覆盖。
|
||||
|
||||
gRPC 和 QUIC 的职责边界：
|
||||
|
||||
@ -21,6 +41,104 @@ gRPC 和 QUIC 的职责边界：
|
||||
- 实时音视频使用 QUIC DATAGRAM；
|
||||
- `quic_edge/` 不是平台 Gateway，也不是浏览器服务器。
|
||||
|
||||
## SystemService 设备清单
|
||||
|
||||
`SystemService/GetDeviceList` 返回 `DeviceManager` 的当前只读快照，只包含
|
||||
`enabled=true` 的设备。启用但创建、初始化、启动或健康检查失败的设备仍会返回，
|
||||
并通过 `manager_state`、`health`、`has_error` 和 `error_message` 描述异常。
|
||||
接口同时返回稳定的 `device_type` 和仅用于展示/诊断的具体 `type_name`；调用方
|
||||
不得使用 `type_name` 做设备类别判断。
|
||||
|
||||
该 RPC 不修改配置、不动态注册设备，也不触发设备生命周期操作。启用 reflection
|
||||
后可直接查询：
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.SystemService/GetDeviceList
|
||||
```
|
||||
|
||||
## SystemService ActionQueue
|
||||
|
||||
`SystemService/ExecuteActionQueue` 接收一个完整的有限动作序列，在边缘端排队并
|
||||
逐步串行执行，所有步骤结束后返回最终结果。平台只需要提交一次请求，因此连续机械臂
|
||||
动作不会再受到每个单独 gRPC 往返和 Wi-Fi 抖动的影响。
|
||||
|
||||
当前 v1 仅允许以下 `ActionStep.command`：
|
||||
|
||||
- 机械臂同步 `MoveJ`、`MoveL`；
|
||||
- AGV 同步 `navigateToPose`、`navigateToStation`、`followPath`；
|
||||
- 边缘端本地 `delay`。
|
||||
|
||||
机械臂和 AGV 请求复用各自已有的类型化 Request，目标设备仍由每一步的
|
||||
`header.device_id` 指定。所有运动步骤必须设置 `asynchronous=false`；`MoveL` v1 仅接受
|
||||
Base frame；AGV 后端还必须明确支持同步导航终态确认。`speedJ`、`speedL`、`servoJ`、
|
||||
AGV `translate`、速度控制、查询和流式 RPC 都不属于 ActionQueue v1。
|
||||
|
||||
ActionQueue 遵循以下执行语义：
|
||||
|
||||
- 平台先调用 `GetSystemInfo` 读取 `action_service_instance_id`，并在每次提交和重试中填入
|
||||
`expected_service_instance_id`。ActionQueue 账本随服务实例重建；若断线期间边缘服务重启，
|
||||
旧实例 ID 会被拒绝，平台必须先对账，不能用新 ID 自动重放不确定的动作；
|
||||
- `action_id` 是必填的全局唯一幂等键；同一服务实例内，相同内容的已受理请求不会重复下发
|
||||
设备命令，相同 ID 但内容不同的请求必须拒绝。服务端缓存最近 4096 个完整结果，更早的
|
||||
已执行 ID 由精确 retired-ID 账本 fail-closed 拒绝、不会重跑；单实例最多记录
|
||||
262144 个已受理 ID，达到容量后仅拒绝新 ID，已有 ID 仍可查询；
|
||||
- 入队前校验全部步骤、设备、参数和同步能力，校验失败时不会执行任何步骤；
|
||||
- v1 每个请求最多 256 步、序列化大小最多 512 KiB、排队或执行中的 Action 最多 64 个、
|
||||
同时提交或等待结果的 RPC 最多 256 个；Action 与单步超时上限均为 24 小时，
|
||||
`total_timeout_ms=0` 使用 30 分钟默认值，AGV 路径最多 4096 段；
|
||||
- `total_timeout_ms` 包含排队与执行时间，单步 `timeout_ms=0` 时继承 Action 剩余时间
|
||||
或服务端默认值；所有超时值均由服务端施加上限；
|
||||
- 任一步失败、取消或超时后立即停止序列，不再执行后续步骤；`completed_steps` 表示此前
|
||||
成功完成的步骤数，`failed_step_index` 仅在存在对应失败步骤时出现；
|
||||
- Action 一旦受理，不因平台连接中断而自动取消；断线只结束该 RPC waiter，边缘动作继续。
|
||||
平台可用相同 `action_id` 重试并取得仍在缓存中的同一次执行结果；
|
||||
- `StopAll`、机械臂 `stopMotion`、AGV `cancelNavigation` 和软件急停不进入 FIFO，必须
|
||||
作为高优先级安全/抢占路径执行。它们仍不具备功能安全等级。
|
||||
- 机械臂步骤超时会立即走 typed `stopMotion` 并等待停车确认；若无法确认停车，设备控制权
|
||||
保持隔离，不会继续后续步骤或接受新的普通控制命令；需先按设备安全流程确认状态，再
|
||||
重启边缘服务恢复控制。
|
||||
- AGV 的取消 ACK、零速度 ACK 均不等于停稳；ActionQueue 和安全停止 RPC 只有在导航任务
|
||||
终态且底盘连续零速度采样确认后才释放控制权，否则同样保留隔离。
|
||||
|
||||
已知的执行完成、业务失败、取消、超时和预校验拒绝由 `ActionResultCode` 与
|
||||
`CommandHeader.Feedback` 表达。`ActionDeduplicationStatus` 结构化区分新受理、合并等待、
|
||||
缓存结果、已淘汰结果、账本耗尽、ID 冲突和服务实例不匹配；平台不得通过解析错误字符串
|
||||
判断动作是否执行过。
|
||||
`ACTION_RESULT_CODE_UNSPECIFIED` 不得作为服务端最终结果。
|
||||
|
||||
## MotorService
|
||||
|
||||
`MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的
|
||||
`MotorManager` 和 `AbstractMotor`，不直接持有现场总线或厂商驱动。
|
||||
|
||||
关键文件：
|
||||
|
||||
- Proto：[`../../protos/cmvr/api/motor_service.proto`](../../protos/cmvr/api/motor_service.proto)
|
||||
和 [`../../protos/cmvr/api/motor_command.proto`](../../protos/cmvr/api/motor_command.proto)
|
||||
- 实现：[`grpc/server/include/grpc_motor_service.h`](grpc/server/include/grpc_motor_service.h)
|
||||
和 [`grpc/server/src/grpc_motor_service.cpp`](grpc/server/src/grpc_motor_service.cpp)
|
||||
- 注册：[`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp)
|
||||
|
||||
服务按单电机仲裁。同步 Profile 命令、Cyclic Position/Velocity 双向流、
|
||||
`setEnabled`、状态读取和软件 `emergencyStop` 共用同一控制权状态：
|
||||
|
||||
- 同一电机已有 owner 时拒绝新的控制调用；
|
||||
- cyclic 流首帧必须是 `open`，后续 setpoint sequence 必须严格递增；
|
||||
- reader 使用 latest-wins 邮箱，客户端必须持续并发读取反馈；
|
||||
- 取消、deadline、watchdog、非法帧、后端拒绝或写失败都会触发 Quick Stop；
|
||||
- 任何清理 Quick Stop 未确认时，服务进入 fail-closed 锁存；
|
||||
- 只有成功执行 `setEnabled(true)` 才解除服务内软件急停锁存；
|
||||
- 服务层 Quick Stop 和 `emergencyStop` 都不具备功能安全等级。
|
||||
|
||||
AUBO 控制柜 IO 不经过 `MotorService`，由
|
||||
`ArmService/ExecuteJsonCommand` 转发到目标 `RobotArm`。厂商命令和安全约束见
|
||||
[AUBO 控制柜 IO](../devices/arm/aubo_arm/README.md)。
|
||||
旧的 `SystemService/ExecuteJsonCommand` 已移除；相机 PTZ 应使用类型化的
|
||||
`CameraService/ControlPtz`。
|
||||
|
||||
## 新增 gRPC Service
|
||||
|
||||
当前没有动态 service registry，必须完成以下全部步骤。
|
||||
@ -40,8 +158,9 @@ import 路径必须相对于 `protos/`。兼容规则见 [`../../protos/README.m
|
||||
|
||||
```text
|
||||
service/grpc/
|
||||
├── include/grpc_example_service.h
|
||||
└── src/grpc_example_service.cpp
|
||||
└── server/
|
||||
├── include/grpc_example_service.h
|
||||
└── src/grpc_example_service.cpp
|
||||
```
|
||||
|
||||
实现类继承生成的：
|
||||
@ -103,7 +222,7 @@ cmvr::api::ExampleService::Service
|
||||
|
||||
- 检查 `context->IsCancelled()`；
|
||||
- 检查 `Read()` / `Write()` 返回；
|
||||
- 使用 RAII 或 MediaSourceHub Subscription 释放 producer lease；
|
||||
- 使用 RAII 或 MediaSourceManager Subscription 释放 producer lease；
|
||||
- 不持有设备状态锁进行网络写；
|
||||
- 为 wait/read 使用有限 timeout；
|
||||
- 慢客户端不能阻塞设备生产线程；
|
||||
@ -111,7 +230,7 @@ cmvr::api::ExampleService::Service
|
||||
- gRPC RGB 流在积压超过 `camera_stream_max_pending_frames` 或帧龄超过
|
||||
`camera_stream_max_frame_age_ms` 时主动丢弃旧帧，请求 IDR，并从下一个关键帧恢复。
|
||||
|
||||
当前仅 gRPC RGB 和麦克风流使用 MediaSourceHub；Depth/RGBD 仍直接读取设备帧。
|
||||
当前仅 gRPC RGB 和麦克风流使用 MediaSourceManager；Depth/RGBD 仍直接读取设备帧。
|
||||
|
||||
gRPC 相机实时流默认最多保留 2 帧积压、最大允许 250 ms 帧龄。两个配置项填 0
|
||||
时使用上述默认值。该策略以低延迟为目标，不保证每个视频帧都到达客户端；控制命令
|
||||
|
||||
120
cmvr-es/service/grpc/CMakeLists.txt
Normal file
120
cmvr-es/service/grpc/CMakeLists.txt
Normal file
@ -0,0 +1,120 @@
|
||||
|
||||
add_library(service
|
||||
stop_all/src/stop_operation_dispatcher.cpp
|
||||
action/src/action_queue_executor.cpp
|
||||
server/src/camera_ptz_activity_registry.cpp
|
||||
server/src/media_activity_coordinator.cpp
|
||||
server/src/motor_activity_coordinator.cpp
|
||||
server/src/grpc_camera_service.cpp
|
||||
server/src/grpc_command_transaction.cpp
|
||||
server/src/grpc_error_logging_interceptor.cpp
|
||||
server/src/grpc_recovery_audit.cpp
|
||||
server/src/grpc_safety_proto.cpp
|
||||
server/src/grpc_safety_participants.cpp
|
||||
server/src/grpc_security.cpp
|
||||
server/src/grpc_system_service.cpp
|
||||
server/src/grpc_speaker_service.cpp
|
||||
server/src/grpc_microphone_service.cpp
|
||||
server/src/grpc_head_service.cpp
|
||||
server/src/grpc_dexhand_service.cpp
|
||||
server/src/grpc_arm_service.cpp
|
||||
server/src/grpc_arm_teleop_service.cpp
|
||||
server/src/grpc_robot_arm_teleop_backend.cpp
|
||||
server/src/grpc_motor_service.cpp
|
||||
server/src/grpc_agv_service.cpp
|
||||
server/src/grpc_hlc_service.cpp
|
||||
../../task/grpc_server_task/src/grpc_server_task.cpp
|
||||
)
|
||||
|
||||
target_include_directories(service PUBLIC
|
||||
${CMAKE_SOURCE_DIR}/cmvr-es
|
||||
)
|
||||
|
||||
target_link_libraries(service PRIVATE
|
||||
cmvr_es::proto
|
||||
cmvr_es::stop_all_admission_gate
|
||||
cmvr_es::camera_operational_activity_registry
|
||||
osqp
|
||||
cmvr_es::control_authority_manager
|
||||
cmvr_es::device_manager
|
||||
cmvr_es::task_manager
|
||||
cmvr_es::algorithms::controller
|
||||
cmvr_es::task
|
||||
cmvr_es::media_source_manager
|
||||
cmvr_es::device_media_source_adapter
|
||||
protobuf::libprotobuf
|
||||
)
|
||||
|
||||
add_library(cmvr_es::service ALIAS service)
|
||||
install(TARGETS service LIBRARY DESTINATION lib)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
add_executable(grpc_camera_stream_policy_test
|
||||
server/tests/grpc_camera_stream_policy_test.cpp
|
||||
)
|
||||
target_include_directories(grpc_camera_stream_policy_test
|
||||
PRIVATE
|
||||
${CMAKE_SOURCE_DIR}/cmvr-es
|
||||
)
|
||||
add_test(
|
||||
NAME grpc_camera_stream_policy_test
|
||||
COMMAND grpc_camera_stream_policy_test
|
||||
)
|
||||
set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10)
|
||||
endif()
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Unit test
|
||||
# --------------------------------------------------------
|
||||
find_package(OpenCV REQUIRED)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
|
||||
)
|
||||
|
||||
link_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
|
||||
)
|
||||
|
||||
|
||||
add_executable(grpc_arm_client_test
|
||||
server/tests/grpc_arm_client_test.cpp
|
||||
)
|
||||
|
||||
|
||||
target_link_libraries(grpc_arm_client_test
|
||||
PRIVATE
|
||||
cmvr_es::device::canbus
|
||||
cmvr_es::device::ti5_canopen_motor_driver
|
||||
osqp
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog
|
||||
cmvr_es::proto
|
||||
ccd
|
||||
fcl
|
||||
cmvr_es::device_manager
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
|
||||
|
||||
add_executable(grpc_hlc_client_test
|
||||
server/tests/grpc_hlc_client_test.cpp
|
||||
)
|
||||
|
||||
|
||||
target_link_libraries(grpc_hlc_client_test
|
||||
PRIVATE
|
||||
cmvr_es::device::canbus
|
||||
cmvr_es::device::ti5_canopen_motor_driver
|
||||
osqp
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog
|
||||
cmvr_es::proto
|
||||
ccd
|
||||
fcl
|
||||
cmvr_es::device_manager
|
||||
)
|
||||
102
cmvr-es/service/grpc/action/include/action_queue_executor.h
Normal file
102
cmvr-es/service/grpc/action/include/action_queue_executor.h
Normal file
@ -0,0 +1,102 @@
|
||||
#ifndef CMVR_ES_ACTION_QUEUE_EXECUTOR_H
|
||||
#define CMVR_ES_ACTION_QUEUE_EXECUTOR_H
|
||||
|
||||
#include <chrono>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "cmvr/api/system_command.pb.h"
|
||||
#include "manager/safety_manager/include/safety_types.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
class DeviceManager;
|
||||
}
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
// Owns the process-local FIFO used by SystemService ActionQueue requests.
|
||||
// The executor intentionally has no grpc::ServerContext dependency: once a
|
||||
// request is accepted, loss of the platform connection must not cancel device
|
||||
// motion on the edge.
|
||||
class ActionQueueExecutor final {
|
||||
public:
|
||||
static constexpr std::size_t kDefaultMaxAcceptedActionIds =
|
||||
256U * 1024U;
|
||||
|
||||
enum class WaitResult {
|
||||
Terminal,
|
||||
CanceledBeforeAdmission,
|
||||
CanceledAfterAdmission,
|
||||
};
|
||||
|
||||
// Identifies one participant in a StopAll round. Multiple concurrent
|
||||
// StopAll callers join the same round; ActionQueue admission resumes only
|
||||
// after every ticket in that round has been completed successfully.
|
||||
struct StopAllTicket {
|
||||
std::uint64_t generation{0};
|
||||
std::uint64_t ticket_id{0};
|
||||
bool active_action_stop_confirmed{false};
|
||||
|
||||
bool valid() const noexcept
|
||||
{
|
||||
return generation != 0U && ticket_id != 0U;
|
||||
}
|
||||
};
|
||||
|
||||
explicit ActionQueueExecutor(
|
||||
device::DeviceManager& device_manager,
|
||||
std::size_t max_accepted_action_ids =
|
||||
kDefaultMaxAcceptedActionIds);
|
||||
~ActionQueueExecutor();
|
||||
|
||||
ActionQueueExecutor(const ActionQueueExecutor&) = delete;
|
||||
ActionQueueExecutor& operator=(const ActionQueueExecutor&) = delete;
|
||||
|
||||
// Validates, idempotently enqueues, and waits for the terminal result.
|
||||
// Protocol and execution outcomes are represented in Feedback.
|
||||
WaitResult submitAndWait(
|
||||
const api::ActionQueueCommand_Request& request,
|
||||
api::ActionQueueCommand_Feedback& feedback,
|
||||
const std::function<bool()>& waiter_canceled = {},
|
||||
safety::CommandActor actor = {});
|
||||
|
||||
// Starts (or joins) a temporary StopAll round. New action IDs are rejected
|
||||
// and queued/active actions are canceled. By default the executor also
|
||||
// requests a typed stop for the active action. SystemService delegates that
|
||||
// stop to its whole-machine sweep so one slow Action backend cannot delay
|
||||
// stop requests for every other device.
|
||||
// Existing action IDs remain queryable for idempotent reconciliation. An
|
||||
// invalid ticket means permanent shutdown has already started.
|
||||
StopAllTicket beginStopAll(bool delegate_active_stop = false);
|
||||
|
||||
// Completes a StopAll participant after the caller has stopped and
|
||||
// confirmed all other devices. The queue resumes only when every ticket in
|
||||
// the current round reports success and the worker is idle. True means this
|
||||
// ticket was completed successfully; another concurrent ticket may still
|
||||
// keep the queue paused. False is fail-closed: the ticket was stale,
|
||||
// shutdown won the race, a stop was not confirmed, or the executor was not
|
||||
// idle when the last ticket completed.
|
||||
bool finishStopAll(
|
||||
const StopAllTicket& ticket,
|
||||
bool all_devices_stop_confirmed);
|
||||
|
||||
// Permanently rejects new actions, cancels queued/active work, requests a
|
||||
// typed stop, and tells the worker to exit after canceled work is drained.
|
||||
// This transition is irreversible for this executor instance. Returns true
|
||||
// when the active Action devices reported a confirmed stop.
|
||||
bool disableForShutdown();
|
||||
|
||||
bool waitForIdle(std::chrono::milliseconds timeout);
|
||||
const std::string& instanceId() const noexcept;
|
||||
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> impl_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
#endif // CMVR_ES_ACTION_QUEUE_EXECUTOR_H
|
||||
2528
cmvr-es/service/grpc/action/src/action_queue_executor.cpp
Normal file
2528
cmvr-es/service/grpc/action/src/action_queue_executor.cpp
Normal file
File diff suppressed because it is too large
Load Diff
16
cmvr-es/service/grpc/client/CMakeLists.txt
Normal file
16
cmvr-es/service/grpc/client/CMakeLists.txt
Normal file
@ -0,0 +1,16 @@
|
||||
find_package(Threads REQUIRED)
|
||||
|
||||
add_library(arm_teleop_client STATIC
|
||||
src/grpc_arm_teleop_client.cpp
|
||||
)
|
||||
target_compile_features(arm_teleop_client PUBLIC cxx_std_17)
|
||||
target_include_directories(arm_teleop_client PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es)
|
||||
target_link_libraries(arm_teleop_client
|
||||
PUBLIC
|
||||
cmvr_es::proto
|
||||
PRIVATE
|
||||
Threads::Threads
|
||||
)
|
||||
|
||||
add_library(cmvr_es::arm_teleop_client ALIAS arm_teleop_client)
|
||||
install(TARGETS arm_teleop_client ARCHIVE DESTINATION lib)
|
||||
80
cmvr-es/service/grpc/client/include/grpc_arm_teleop_client.h
Normal file
80
cmvr-es/service/grpc/client/include/grpc_arm_teleop_client.h
Normal file
@ -0,0 +1,80 @@
|
||||
#ifndef CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H
|
||||
#define CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
|
||||
#include <grpcpp/channel.h>
|
||||
#include <grpcpp/client_context.h>
|
||||
#include <grpcpp/support/status.h>
|
||||
#include <grpcpp/support/sync_stream.h>
|
||||
|
||||
#include "cmvr/api/arm_teleop_v1.grpc.pb.h"
|
||||
|
||||
namespace cmvr::teleop {
|
||||
|
||||
// One synchronous gRPC stream/session. Connection retry and worker ownership
|
||||
// belong to UmeTeleopTask; robot algorithms and kinematics belong to UME.
|
||||
class GrpcArmTeleopClient final {
|
||||
public:
|
||||
using Api = api::armteleop::v1::ArmTeleopService;
|
||||
using ClientFrame = api::armteleop::v1::ClientFrame;
|
||||
using ServerFrame = api::armteleop::v1::ServerFrame;
|
||||
using OpenSession = api::armteleop::v1::OpenSession;
|
||||
using JointSetpoint = api::armteleop::v1::JointSetpoint;
|
||||
using ClientHeartbeat = api::armteleop::v1::ClientHeartbeat;
|
||||
using StopSession = api::armteleop::v1::StopSession;
|
||||
using FrameCallback = std::function<void(const ServerFrame&)>;
|
||||
using CancelPredicate = std::function<bool()>;
|
||||
|
||||
explicit GrpcArmTeleopClient(
|
||||
std::shared_ptr<grpc::ChannelInterface> channel);
|
||||
~GrpcArmTeleopClient();
|
||||
|
||||
GrpcArmTeleopClient(const GrpcArmTeleopClient&) = delete;
|
||||
GrpcArmTeleopClient& operator=(const GrpcArmTeleopClient&) = delete;
|
||||
|
||||
// Blocks until the peer closes the stream or tryCancel() is called. The
|
||||
// OpenSession frame is always the first client frame.
|
||||
grpc::Status runSession(const OpenSession& open_session,
|
||||
FrameCallback callback = {},
|
||||
CancelPredicate cancel_requested = {});
|
||||
|
||||
// These methods only transport already-computed protocol values.
|
||||
bool sendSetpoint(const JointSetpoint& setpoint,
|
||||
std::uint64_t expected_session_generation = 0);
|
||||
bool sendHeartbeat(const ClientHeartbeat& heartbeat,
|
||||
std::uint64_t expected_session_generation = 0);
|
||||
bool sendStop(const StopSession& stop,
|
||||
std::uint64_t expected_session_generation = 0);
|
||||
|
||||
bool isSessionActive() const;
|
||||
std::uint64_t activeSessionGeneration() const;
|
||||
|
||||
// Thread-safe and intentionally named after the gRPC primitive used. It
|
||||
// interrupts a blocked Read/Write/Finish so the owning Task can join.
|
||||
void tryCancel();
|
||||
|
||||
private:
|
||||
using Stream = grpc::ClientReaderWriterInterface<ClientFrame, ServerFrame>;
|
||||
|
||||
bool writeFrame(const ClientFrame& frame,
|
||||
std::uint64_t expected_session_generation);
|
||||
void clearSession(const std::shared_ptr<grpc::ClientContext>& context,
|
||||
const std::shared_ptr<Stream>& stream);
|
||||
|
||||
std::unique_ptr<Api::StubInterface> stub_;
|
||||
|
||||
mutable std::mutex lifecycle_mutex_;
|
||||
std::mutex write_mutex_;
|
||||
std::shared_ptr<grpc::ClientContext> active_context_;
|
||||
std::shared_ptr<Stream> active_stream_;
|
||||
std::uint64_t next_session_generation_{0};
|
||||
std::uint64_t active_session_generation_{0};
|
||||
};
|
||||
|
||||
} // namespace cmvr::teleop
|
||||
|
||||
#endif // CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H
|
||||
223
cmvr-es/service/grpc/client/src/grpc_arm_teleop_client.cpp
Normal file
223
cmvr-es/service/grpc/client/src/grpc_arm_teleop_client.cpp
Normal file
@ -0,0 +1,223 @@
|
||||
#include "service/grpc/client/include/grpc_arm_teleop_client.h"
|
||||
|
||||
#include <exception>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
namespace cmvr::teleop {
|
||||
namespace {
|
||||
|
||||
grpc::Status clientStatus(const grpc::StatusCode code, const char* detail)
|
||||
{
|
||||
return grpc::Status(code, detail);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
GrpcArmTeleopClient::GrpcArmTeleopClient(
|
||||
std::shared_ptr<grpc::ChannelInterface> channel)
|
||||
{
|
||||
if (channel) {
|
||||
stub_ = Api::NewStub(channel);
|
||||
}
|
||||
}
|
||||
|
||||
GrpcArmTeleopClient::~GrpcArmTeleopClient()
|
||||
{
|
||||
tryCancel();
|
||||
}
|
||||
|
||||
grpc::Status GrpcArmTeleopClient::runSession(
|
||||
const OpenSession& open_session,
|
||||
FrameCallback callback,
|
||||
CancelPredicate cancel_requested)
|
||||
{
|
||||
if (!stub_) {
|
||||
return clientStatus(
|
||||
grpc::StatusCode::FAILED_PRECONDITION,
|
||||
"arm teleop client has no channel");
|
||||
}
|
||||
if (cancel_requested && cancel_requested()) {
|
||||
return clientStatus(
|
||||
grpc::StatusCode::CANCELLED,
|
||||
"arm teleop session cancelled before start");
|
||||
}
|
||||
|
||||
auto context = std::make_shared<grpc::ClientContext>();
|
||||
{
|
||||
std::lock_guard lock(lifecycle_mutex_);
|
||||
if (active_context_) {
|
||||
return clientStatus(
|
||||
grpc::StatusCode::ALREADY_EXISTS,
|
||||
"arm teleop session is already active");
|
||||
}
|
||||
// Publish the context before opening/writing the stream so tryCancel()
|
||||
// can interrupt every blocking phase of the synchronous RPC.
|
||||
active_context_ = context;
|
||||
}
|
||||
// Closes the small race where the owning Task requests stop immediately
|
||||
// before active_context_ becomes visible to tryCancel().
|
||||
if (cancel_requested && cancel_requested()) {
|
||||
context->TryCancel();
|
||||
}
|
||||
|
||||
auto unique_stream = stub_->Teleoperate(context.get());
|
||||
if (!unique_stream) {
|
||||
clearSession(context, {});
|
||||
return clientStatus(
|
||||
grpc::StatusCode::UNAVAILABLE,
|
||||
"failed to create arm teleop stream");
|
||||
}
|
||||
auto stream = std::shared_ptr<Stream>(std::move(unique_stream));
|
||||
|
||||
ClientFrame first_frame;
|
||||
*first_frame.mutable_open() = open_session;
|
||||
{
|
||||
std::lock_guard write_lock(write_mutex_);
|
||||
if (!stream->Write(first_frame)) {
|
||||
const grpc::Status status = stream->Finish();
|
||||
clearSession(context, stream);
|
||||
return status.ok()
|
||||
? clientStatus(
|
||||
grpc::StatusCode::UNAVAILABLE,
|
||||
"peer closed before OpenSession was written")
|
||||
: status;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard lock(lifecycle_mutex_);
|
||||
// Cancellation can race the initial Write. Keeping the stream visible
|
||||
// is safe; subsequent writes will fail and runSession will clean it.
|
||||
if (active_context_ == context) {
|
||||
active_stream_ = stream;
|
||||
active_session_generation_ = ++next_session_generation_;
|
||||
}
|
||||
}
|
||||
|
||||
bool callback_failed = false;
|
||||
std::string callback_error;
|
||||
ServerFrame frame;
|
||||
while (stream->Read(&frame)) {
|
||||
if (!callback) {
|
||||
continue;
|
||||
}
|
||||
try {
|
||||
callback(frame);
|
||||
} catch (const std::exception& error) {
|
||||
callback_failed = true;
|
||||
callback_error = error.what();
|
||||
context->TryCancel();
|
||||
break;
|
||||
} catch (...) {
|
||||
callback_failed = true;
|
||||
callback_error = "server-frame callback raised an unknown exception";
|
||||
context->TryCancel();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard write_lock(write_mutex_);
|
||||
stream->WritesDone();
|
||||
}
|
||||
const grpc::Status status = stream->Finish();
|
||||
clearSession(context, stream);
|
||||
|
||||
if (callback_failed) {
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::INTERNAL,
|
||||
"arm teleop callback failed: " + callback_error);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
bool GrpcArmTeleopClient::sendSetpoint(
|
||||
const JointSetpoint& setpoint,
|
||||
const std::uint64_t expected_session_generation)
|
||||
{
|
||||
ClientFrame frame;
|
||||
*frame.mutable_setpoint() = setpoint;
|
||||
return writeFrame(frame, expected_session_generation);
|
||||
}
|
||||
|
||||
bool GrpcArmTeleopClient::sendHeartbeat(
|
||||
const ClientHeartbeat& heartbeat,
|
||||
const std::uint64_t expected_session_generation)
|
||||
{
|
||||
ClientFrame frame;
|
||||
*frame.mutable_heartbeat() = heartbeat;
|
||||
return writeFrame(frame, expected_session_generation);
|
||||
}
|
||||
|
||||
bool GrpcArmTeleopClient::sendStop(
|
||||
const StopSession& stop,
|
||||
const std::uint64_t expected_session_generation)
|
||||
{
|
||||
ClientFrame frame;
|
||||
*frame.mutable_stop() = stop;
|
||||
return writeFrame(frame, expected_session_generation);
|
||||
}
|
||||
|
||||
bool GrpcArmTeleopClient::isSessionActive() const
|
||||
{
|
||||
std::lock_guard lock(lifecycle_mutex_);
|
||||
return active_stream_ != nullptr;
|
||||
}
|
||||
|
||||
std::uint64_t GrpcArmTeleopClient::activeSessionGeneration() const
|
||||
{
|
||||
std::lock_guard lock(lifecycle_mutex_);
|
||||
return active_session_generation_;
|
||||
}
|
||||
|
||||
void GrpcArmTeleopClient::tryCancel()
|
||||
{
|
||||
std::shared_ptr<grpc::ClientContext> context;
|
||||
{
|
||||
std::lock_guard lock(lifecycle_mutex_);
|
||||
context = active_context_;
|
||||
}
|
||||
if (context) {
|
||||
context->TryCancel();
|
||||
}
|
||||
}
|
||||
|
||||
bool GrpcArmTeleopClient::writeFrame(
|
||||
const ClientFrame& frame,
|
||||
const std::uint64_t expected_session_generation)
|
||||
{
|
||||
std::shared_ptr<Stream> stream;
|
||||
{
|
||||
std::lock_guard lock(lifecycle_mutex_);
|
||||
if (expected_session_generation != 0 &&
|
||||
expected_session_generation != active_session_generation_) {
|
||||
return false;
|
||||
}
|
||||
stream = active_stream_;
|
||||
}
|
||||
if (!stream) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// gRPC permits one read and one write concurrently, but concurrent writes
|
||||
// must be serialized by the application.
|
||||
std::lock_guard write_lock(write_mutex_);
|
||||
return stream->Write(frame);
|
||||
}
|
||||
|
||||
void GrpcArmTeleopClient::clearSession(
|
||||
const std::shared_ptr<grpc::ClientContext>& context,
|
||||
const std::shared_ptr<Stream>& stream)
|
||||
{
|
||||
std::lock_guard lock(lifecycle_mutex_);
|
||||
if (active_context_ == context) {
|
||||
active_context_.reset();
|
||||
}
|
||||
if (!stream || active_stream_ == stream) {
|
||||
active_stream_.reset();
|
||||
active_session_generation_ = 0;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace cmvr::teleop
|
||||
@ -73,6 +73,11 @@ public:
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
|
||||
|
||||
grpc::Status relocalize(grpc::ServerContext* context,
|
||||
const api::AgvRelocalizeCommand_Request* request,
|
||||
api::AgvRelocalizeCommand_Feedback* response) override;
|
||||
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
};
|
||||
|
||||
@ -0,0 +1,114 @@
|
||||
#ifndef CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H
|
||||
#define CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H
|
||||
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "devices/camera/abstract_camera.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
// Tracks successful CameraService::StartCamera calls. StopAll uses this
|
||||
// registry to stop the corresponding operational pipelines without invoking
|
||||
// AbstractDevice::stop().
|
||||
class CameraOperationalActivityRegistry final {
|
||||
public:
|
||||
enum class DispatchResult {
|
||||
Success,
|
||||
RejectedByStopAll,
|
||||
RejectedByDispatchFence,
|
||||
DeviceFailure,
|
||||
};
|
||||
|
||||
using DispatchFence = std::function<bool()>;
|
||||
|
||||
struct ActivityToken {
|
||||
std::string device_id;
|
||||
std::uint64_t activity_generation{0U};
|
||||
bool owns_start{false};
|
||||
|
||||
bool valid() const noexcept
|
||||
{
|
||||
return !device_id.empty() && activity_generation != 0U;
|
||||
}
|
||||
};
|
||||
|
||||
DispatchResult start(
|
||||
const std::string& device_id,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
ActivityToken* token = nullptr,
|
||||
DispatchFence dispatch_fence = {});
|
||||
|
||||
// Rolls back only the exact activity created by start(). A newer start for
|
||||
// the same device is never stopped by an older request finishing late.
|
||||
bool stopIfCurrent(const ActivityToken& token);
|
||||
|
||||
// Explicit StopCamera retains its legacy lifecycle behavior, but is
|
||||
// serialized here so it cannot race an operational StopAll stop.
|
||||
DispatchResult stopLifecycle(
|
||||
const std::string& device_id,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
DispatchFence dispatch_fence = {});
|
||||
|
||||
// Reconciles a lifecycle stop performed outside CameraService.
|
||||
void markCameraStopped(const std::string& device_id);
|
||||
|
||||
// StopAll must close the process-wide admission gate first. Successful
|
||||
// entries are removed. Failed entries remain quarantined for a later
|
||||
// StopAll retry.
|
||||
bool stopAllActivities(std::vector<std::string>* failures = nullptr);
|
||||
|
||||
// StopAll must close the process-wide admission gate first. Stops only the
|
||||
// operational pipeline tracked for device_id and never invokes the camera
|
||||
// lifecycle stop().
|
||||
bool stopActivitiesForDevice(
|
||||
const std::string& device_id,
|
||||
std::vector<std::string>* failures = nullptr);
|
||||
|
||||
// If no StartCamera activity is tracked, StopAll can still quiesce the
|
||||
// camera currently present in DeviceManager's inventory. A tracked camera
|
||||
// takes precedence over the fallback. Repeated calls in one StopAll round
|
||||
// do not stop the same instance twice.
|
||||
bool stopActivitiesForDevice(
|
||||
const std::string& device_id,
|
||||
const std::shared_ptr<device::AbstractCamera>& fallback_camera,
|
||||
std::vector<std::string>* failures = nullptr);
|
||||
|
||||
std::size_t activeCameraCount() const;
|
||||
// Does not wait for per-device driver I/O. This conservative snapshot
|
||||
// includes devices with an admitted or historical dispatch state, allowing
|
||||
// StopAll to cover activity absent from the DeviceManager snapshot.
|
||||
std::vector<std::string> trackedDeviceIds() const;
|
||||
std::vector<std::string> activeDeviceIds() const;
|
||||
void clearForTesting();
|
||||
|
||||
private:
|
||||
struct DeviceState {
|
||||
mutable std::mutex mutex;
|
||||
std::shared_ptr<device::AbstractCamera> active_camera;
|
||||
std::weak_ptr<device::AbstractCamera> last_stopped_camera;
|
||||
std::uint64_t last_stopped_generation{0U};
|
||||
std::uint64_t activity_generation{0U};
|
||||
std::atomic<bool> active{false};
|
||||
};
|
||||
|
||||
std::shared_ptr<DeviceState> stateForDevice(
|
||||
const std::string& device_id,
|
||||
bool create);
|
||||
|
||||
mutable std::mutex states_mutex_;
|
||||
std::unordered_map<std::string, std::shared_ptr<DeviceState>> states_;
|
||||
};
|
||||
|
||||
CameraOperationalActivityRegistry&
|
||||
globalCameraOperationalActivityRegistry();
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
#endif // CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H
|
||||
@ -0,0 +1,96 @@
|
||||
#ifndef CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H
|
||||
#define CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H
|
||||
|
||||
#include <atomic>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "devices/camera/abstract_camera.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
// Tracks PTZ commands whose START has not yet been paired with a successful
|
||||
// STOP. The registry is an operational control boundary; it never invokes a
|
||||
// camera lifecycle method.
|
||||
class CameraPtzActivityRegistry final {
|
||||
public:
|
||||
enum class DispatchResult {
|
||||
Success,
|
||||
RejectedByStopAll,
|
||||
RejectedByDispatchFence,
|
||||
DeviceFailure,
|
||||
};
|
||||
|
||||
using DispatchFence = std::function<bool()>;
|
||||
|
||||
DispatchResult control(
|
||||
const std::string& device_id,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
device::PtzCommand command,
|
||||
bool stop,
|
||||
int speed,
|
||||
DispatchFence dispatch_fence = {});
|
||||
|
||||
// Reconciles externally stopped camera PTZ state with this registry. A
|
||||
// camera backend can call this if it stops PTZ outside CameraService.
|
||||
void markCameraStopped(const std::string& device_id);
|
||||
|
||||
// StopAll must close the process-wide admission gate before calling this.
|
||||
// A true result means every tracked START received a successful matching
|
||||
// STOP. Failed entries are retained so a later StopAll can retry them.
|
||||
bool stopAllActivities(std::vector<std::string>* failures = nullptr);
|
||||
|
||||
// StopAll must close the process-wide admission gate first. Stops only PTZ
|
||||
// commands tracked for device_id. Calls for different physical cameras may
|
||||
// execute concurrently; calls for one camera remain ordered with control().
|
||||
bool stopActivitiesForDevice(
|
||||
const std::string& device_id,
|
||||
std::vector<std::string>* failures = nullptr);
|
||||
|
||||
std::size_t activeCommandCount() const;
|
||||
// Does not wait for per-device driver I/O. This conservative snapshot
|
||||
// includes devices with an admitted or historical dispatch state, allowing
|
||||
// StopAll to cover activity absent from the DeviceManager snapshot.
|
||||
std::vector<std::string> trackedDeviceIds() const;
|
||||
std::vector<std::string> activeDeviceIds() const;
|
||||
void clearForTesting();
|
||||
|
||||
private:
|
||||
struct PtzCommandHash {
|
||||
std::size_t operator()(device::PtzCommand command) const noexcept
|
||||
{
|
||||
return static_cast<std::size_t>(command);
|
||||
}
|
||||
};
|
||||
|
||||
struct ActiveCommand {
|
||||
std::shared_ptr<device::AbstractCamera> camera;
|
||||
int speed{0};
|
||||
};
|
||||
|
||||
using CameraCommands = std::unordered_map<
|
||||
device::PtzCommand, ActiveCommand, PtzCommandHash>;
|
||||
|
||||
struct DeviceState {
|
||||
mutable std::mutex mutex;
|
||||
CameraCommands commands;
|
||||
std::atomic<std::size_t> active_command_count{0U};
|
||||
};
|
||||
|
||||
std::shared_ptr<DeviceState> stateForDevice(
|
||||
const std::string& device_id,
|
||||
bool create);
|
||||
|
||||
mutable std::mutex states_mutex_;
|
||||
std::unordered_map<std::string, std::shared_ptr<DeviceState>> states_;
|
||||
};
|
||||
|
||||
CameraPtzActivityRegistry& globalCameraPtzActivityRegistry();
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
#endif // CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H
|
||||
98
cmvr-es/service/grpc/server/include/grpc_agv_service.h
Normal file
98
cmvr-es/service/grpc/server/include/grpc_agv_service.h
Normal file
@ -0,0 +1,98 @@
|
||||
#ifndef CMVR_ES_GRPC_AGV_SERVICE_H
|
||||
#define CMVR_ES_GRPC_AGV_SERVICE_H
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/api/agv_service.grpc.pb.h"
|
||||
#include "devices/agv/abstract_agv.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
class GrpcSecurityGateway;
|
||||
|
||||
class gRPCAgvServiceImpl final : public api::AgvService::Service {
|
||||
public:
|
||||
gRPCAgvServiceImpl();
|
||||
explicit gRPCAgvServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
~gRPCAgvServiceImpl() override = default;
|
||||
|
||||
grpc::Status getRuntimeState(grpc::ServerContext* context,
|
||||
const api::AgvRuntimeStateCommand_Request* request,
|
||||
api::AgvRuntimeStateCommand_Feedback* response) override;
|
||||
grpc::Status getNavigationStatus(grpc::ServerContext* context,
|
||||
const api::AgvNavigationStatusCommand_Request* request,
|
||||
api::AgvNavigationStatusCommand_Feedback* response) override;
|
||||
grpc::Status emergencyStop(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status clearFault(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status navigateToPose(grpc::ServerContext* context,
|
||||
const api::AgvNavigateToPoseCommand_Request* request,
|
||||
api::AgvNavigateToPoseCommand_Feedback* response) override;
|
||||
grpc::Status navigateToStation(grpc::ServerContext* context,
|
||||
const api::AgvNavigateToStationCommand_Request* request,
|
||||
api::AgvNavigateToStationCommand_Feedback* response) override;
|
||||
grpc::Status followPath(grpc::ServerContext* context,
|
||||
const api::AgvFollowPathCommand_Request* request,
|
||||
api::AgvFollowPathCommand_Feedback* response) override;
|
||||
grpc::Status pauseNavigation(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status resumeNavigation(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status cancelNavigation(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status setVelocity(grpc::ServerContext* context,
|
||||
const api::AgvSetVelocityCommand_Request* request,
|
||||
api::AgvSetVelocityCommand_Feedback* response) override;
|
||||
grpc::Status stopVelocityControl(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status listMaps(grpc::ServerContext* context,
|
||||
const api::AgvListMapsCommand_Request* request,
|
||||
api::AgvListMapsCommand_Feedback* response) override;
|
||||
grpc::Status listStations(grpc::ServerContext* context,
|
||||
const api::AgvListStationsCommand_Request* request,
|
||||
api::AgvListStationsCommand_Feedback* response) override;
|
||||
grpc::Status switchMap(grpc::ServerContext* context,
|
||||
const api::AgvMapCommand_Request* request,
|
||||
api::AgvMapCommand_Feedback* response) override;
|
||||
grpc::Status uploadMap(grpc::ServerContext* context,
|
||||
const api::AgvMapCommand_Request* request,
|
||||
api::AgvMapCommand_Feedback* response) override;
|
||||
grpc::Status downloadMap(grpc::ServerContext* context,
|
||||
const api::AgvMapCommand_Request* request,
|
||||
api::AgvMapCommand_Feedback* response) override;
|
||||
grpc::Status startMapping(grpc::ServerContext* context,
|
||||
const api::AgvStartMappingCommand_Request* request,
|
||||
api::AgvStartMappingCommand_Feedback* response) override;
|
||||
grpc::Status streamMap(grpc::ServerContext* context,
|
||||
const api::AgvMapStreamCommand_Request* request,
|
||||
grpc::ServerWriter<api::AgvMapStreamCommand_Feedback>* writer) override;
|
||||
grpc::Status stopMapping(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status translate(
|
||||
grpc::ServerContext* context,
|
||||
const api::AgvTranslateCommand_Request* request,
|
||||
api::AgvTranslateCommand_Feedback* response) override;
|
||||
|
||||
|
||||
grpc::Status relocalize(grpc::ServerContext* context,
|
||||
const api::AgvRelocalizeCommand_Request* request,
|
||||
api::AgvRelocalizeCommand_Feedback* response) override;
|
||||
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
#endif // CMVR_ES_GRPC_AGV_SERVICE_H
|
||||
71
cmvr-es/service/grpc/server/include/grpc_arm_service.h
Normal file
71
cmvr-es/service/grpc/server/include/grpc_arm_service.h
Normal file
@ -0,0 +1,71 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/api/arm_service.grpc.pb.h"
|
||||
#include "devices/arm/robot_arm.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
class GrpcSecurityGateway;
|
||||
|
||||
class gRPCArmServiceImpl final : public api::ArmService::Service {
|
||||
public:
|
||||
gRPCArmServiceImpl();
|
||||
explicit gRPCArmServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
~gRPCArmServiceImpl() override = default;
|
||||
|
||||
grpc::Status torqueOff(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status torqueOn(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status moveJ(grpc::ServerContext* context,
|
||||
const api::MoveJ_Request* request,
|
||||
api::MoveJ_Response* response) override;
|
||||
grpc::Status moveL(grpc::ServerContext* context,
|
||||
const api::MoveL_Request* request,
|
||||
api::MoveL_Response* response) override;
|
||||
grpc::Status speedJ(grpc::ServerContext* context,
|
||||
const api::SpeedJ_Request* request,
|
||||
api::SpeedJ_Response* response) override;
|
||||
grpc::Status speedL(grpc::ServerContext* context,
|
||||
const api::SpeedL_Request* request,
|
||||
api::SpeedL_Response* response) override;
|
||||
grpc::Status servoJ(grpc::ServerContext* context,
|
||||
const api::ServoJ_Request* request,
|
||||
api::ServoJ_Response* response) override;
|
||||
grpc::Status stopMotion(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
grpc::Status getJointState(grpc::ServerContext* context,
|
||||
const api::JointRequest* request,
|
||||
api::JointResponse* response) override;
|
||||
grpc::Status getPose(grpc::ServerContext* context,
|
||||
const api::GetPose_Request* request,
|
||||
api::GetPose_Response* response) override;
|
||||
grpc::Status calibrateZeroQ(grpc::ServerContext* context,
|
||||
const api::CalibrateZeroQ_Request* request,
|
||||
api::CalibrateZeroQ_Response* response) override;
|
||||
grpc::Status getPoseMatrix(grpc::ServerContext* context,
|
||||
const api::GetPoseMatrix_Request* request,
|
||||
api::GetPoseMatrix_Response* response) override;
|
||||
grpc::Status computeForwardKinematics(grpc::ServerContext* context,
|
||||
const api::ComputeForwardKinematics_Request* request,
|
||||
api::ComputeForwardKinematics_Response* response) override;
|
||||
grpc::Status ExecuteJsonCommand(grpc::ServerContext* context,
|
||||
const api::JsonDeviceCommand_Request* request,
|
||||
api::JsonDeviceCommand_Feedback* response) override;
|
||||
grpc::Status clearFault(grpc::ServerContext *context,
|
||||
const cmvr::api::CommandHeader_Request *request,
|
||||
cmvr::api::CommandHeader_Feedback *response) override;
|
||||
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::service
|
||||
105
cmvr-es/service/grpc/server/include/grpc_arm_teleop_service.h
Normal file
105
cmvr-es/service/grpc/server/include/grpc_arm_teleop_service.h
Normal file
@ -0,0 +1,105 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include <grpcpp/grpcpp.h>
|
||||
|
||||
#include "cmvr/api/arm_teleop_v1.grpc.pb.h"
|
||||
#include "manager/control_authority_manager/include/control_authority_manager.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
class GrpcSecurityGateway;
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
namespace cmvr::safety {
|
||||
class SafetyManager;
|
||||
}
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
namespace arm_teleop = cmvr::api::armteleop::v1;
|
||||
|
||||
struct ArmTeleopBackendResult {
|
||||
bool success{false};
|
||||
grpc::StatusCode status_code{grpc::StatusCode::INTERNAL};
|
||||
std::string detail;
|
||||
|
||||
static ArmTeleopBackendResult ok()
|
||||
{
|
||||
return {true, grpc::StatusCode::OK, {}};
|
||||
}
|
||||
|
||||
static ArmTeleopBackendResult failure(
|
||||
const grpc::StatusCode code,
|
||||
std::string message)
|
||||
{
|
||||
return {false, code, std::move(message)};
|
||||
}
|
||||
};
|
||||
|
||||
struct ArmTeleopBackendSnapshot {
|
||||
arm_teleop::JointState joint_state;
|
||||
arm_teleop::RobotSafetyState safety;
|
||||
};
|
||||
|
||||
// Execution boundary for ArmTeleopService. The first implementation registers a
|
||||
// disabled backend in production and injects a fake backend in tests. A future
|
||||
// RobotArm adapter must live behind this interface so the gRPC reader thread can
|
||||
// remain a bounded mailbox producer and never touch hardware. Implementations
|
||||
// must keep every call bounded and non-blocking with respect to hardware I/O;
|
||||
// snapshot() must return cached state rather than synchronously polling a bus.
|
||||
class ArmTeleopBackend {
|
||||
public:
|
||||
virtual ~ArmTeleopBackend() = default;
|
||||
|
||||
virtual bool available() const noexcept = 0;
|
||||
virtual std::string unavailableReason() const { return {}; }
|
||||
virtual arm_teleop::RobotManifest manifest() const = 0;
|
||||
virtual bool supportsForceFeedback() const noexcept = 0;
|
||||
virtual ArmTeleopBackendResult open(
|
||||
const arm_teleop::OpenSession& request) = 0;
|
||||
// The deadline is computed from the receiver's local monotonic clock.
|
||||
// Implementations must re-check it immediately before committing a
|
||||
// hardware command; the protobuf valid_for duration is never interpreted
|
||||
// as a cross-machine absolute timestamp.
|
||||
virtual ArmTeleopBackendResult applySetpoint(
|
||||
const arm_teleop::JointSetpoint& setpoint,
|
||||
std::chrono::steady_clock::time_point deadline) = 0;
|
||||
virtual ArmTeleopBackendResult stop(
|
||||
arm_teleop::StopReason reason,
|
||||
const std::string& detail) = 0;
|
||||
virtual ArmTeleopBackendSnapshot snapshot() const = 0;
|
||||
};
|
||||
|
||||
std::shared_ptr<ArmTeleopBackend> makeDisabledArmTeleopBackend();
|
||||
|
||||
class ArmTeleopServiceImpl final
|
||||
: public arm_teleop::ArmTeleopService::Service {
|
||||
public:
|
||||
explicit ArmTeleopServiceImpl(
|
||||
std::shared_ptr<ArmTeleopBackend> backend =
|
||||
makeDisabledArmTeleopBackend(),
|
||||
control::ControlAuthorityManager* authority = nullptr,
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway = nullptr,
|
||||
safety::SafetyManager* safety_manager = nullptr);
|
||||
~ArmTeleopServiceImpl() override = default;
|
||||
|
||||
grpc::Status Teleoperate(
|
||||
grpc::ServerContext* context,
|
||||
grpc::ServerReaderWriter<arm_teleop::ServerFrame,
|
||||
arm_teleop::ClientFrame>* stream) override;
|
||||
|
||||
private:
|
||||
std::shared_ptr<ArmTeleopBackend> backend_;
|
||||
control::ControlAuthorityManager* authority_{nullptr};
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
safety::SafetyManager* safety_manager_{nullptr};
|
||||
};
|
||||
|
||||
} // namespace cmvr::service
|
||||
52
cmvr-es/service/grpc/server/include/grpc_camera_service.h
Normal file
52
cmvr-es/service/grpc/server/include/grpc_camera_service.h
Normal file
@ -0,0 +1,52 @@
|
||||
//
|
||||
// Created by xtkuang on 2025/6/1.
|
||||
//
|
||||
|
||||
#ifndef GRPC_CAMERA_SERVICE_H
|
||||
#define GRPC_CAMERA_SERVICE_H
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/api/camera_service.grpc.pb.h"
|
||||
#include "common/base/grpc_utils.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
#include "devices/camera/abstract_camera.h"
|
||||
#include "service/grpc/server/include/grpc_camera_stream_policy.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
class GrpcSecurityGateway;
|
||||
|
||||
class gRPCCameraServiceImpl final: public api::CameraService::Service {
|
||||
public:
|
||||
explicit gRPCCameraServiceImpl(
|
||||
CameraStreamLowLatencyConfig stream_config = {},
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway = nullptr);
|
||||
~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;
|
||||
grpc::Status StopCamera(grpc::ServerContext* context, const api::StopCameraCommand_Request* request, api::StopCameraCommand_Feedback* response) override;
|
||||
grpc::Status GetRGBImage(grpc::ServerContext* context, const api::GetRGBImageCommand_Request* request, api::GetRGBImageCommand_Feedback* response) override;
|
||||
grpc::Status GetDepthImage(grpc::ServerContext* context, const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response) override;
|
||||
grpc::Status GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) override;
|
||||
grpc::Status StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) override;
|
||||
grpc::Status StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) override;
|
||||
grpc::Status ControlPtz(grpc::ServerContext* context, const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response) override;
|
||||
grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream) override;
|
||||
grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream) override;
|
||||
grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream) override;
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
CameraStreamLowLatencyConfig stream_config_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
|
||||
//双向流读写线程
|
||||
std::shared_ptr<std::thread> read_thread_ = nullptr;
|
||||
std::shared_ptr<std::thread> write_thread_ = nullptr;
|
||||
std::atomic<bool> running_{false};
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif //GRPC_CAMERA_SERVICE_H
|
||||
@ -0,0 +1,60 @@
|
||||
#ifndef CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H
|
||||
#define CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
inline constexpr size_t kDefaultCameraStreamMaxPendingFrames = 2;
|
||||
inline constexpr uint32_t kDefaultCameraStreamMaxFrameAgeMs = 250;
|
||||
|
||||
struct CameraStreamLowLatencyConfig {
|
||||
size_t max_pending_frames{kDefaultCameraStreamMaxPendingFrames};
|
||||
std::chrono::milliseconds max_frame_age{
|
||||
kDefaultCameraStreamMaxFrameAgeMs};
|
||||
};
|
||||
|
||||
inline CameraStreamLowLatencyConfig makeCameraStreamLowLatencyConfig(
|
||||
const uint32_t max_pending_frames,
|
||||
const uint32_t max_frame_age_ms) noexcept {
|
||||
CameraStreamLowLatencyConfig config;
|
||||
config.max_pending_frames = max_pending_frames == 0
|
||||
? kDefaultCameraStreamMaxPendingFrames
|
||||
: static_cast<size_t>(max_pending_frames);
|
||||
config.max_frame_age = std::chrono::milliseconds(
|
||||
max_frame_age_ms == 0
|
||||
? kDefaultCameraStreamMaxFrameAgeMs
|
||||
: max_frame_age_ms);
|
||||
return config;
|
||||
}
|
||||
|
||||
inline std::optional<uint64_t> cameraFrameAgeNs(
|
||||
const uint64_t capture_time_ns,
|
||||
const uint64_t now_ns) noexcept {
|
||||
if (capture_time_ns == 0 || now_ns < capture_time_ns) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return now_ns - capture_time_ns;
|
||||
}
|
||||
|
||||
inline bool cameraFrameExceedsAgeLimit(
|
||||
const uint64_t capture_time_ns,
|
||||
const uint64_t now_ns,
|
||||
const std::chrono::milliseconds max_frame_age) noexcept {
|
||||
const auto age_ns = cameraFrameAgeNs(capture_time_ns, now_ns);
|
||||
if (!age_ns || max_frame_age.count() <= 0) {
|
||||
return false;
|
||||
}
|
||||
const auto max_age_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
max_frame_age).count();
|
||||
return *age_ns > static_cast<uint64_t>(max_age_ns);
|
||||
}
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
#endif // CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H
|
||||
211
cmvr-es/service/grpc/server/include/grpc_command_transaction.h
Normal file
211
cmvr-es/service/grpc/server/include/grpc_command_transaction.h
Normal file
@ -0,0 +1,211 @@
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
|
||||
#include <grpcpp/server_context.h>
|
||||
#include <grpcpp/support/status.h>
|
||||
|
||||
#include <google/protobuf/message.h>
|
||||
|
||||
#include "cmvr/api/common.pb.h"
|
||||
#include "manager/safety_manager/include/safety_manager.h"
|
||||
#include "service/grpc/server/include/grpc_security.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
grpc::Status grpcStatusForSafetyReason(
|
||||
safety::SafetyReason reason,
|
||||
const std::string& detail = {});
|
||||
|
||||
struct GrpcStreamingSafetyOpen {
|
||||
std::string full_method_name;
|
||||
std::string device_id;
|
||||
std::string session_id;
|
||||
std::string expected_service_instance_id;
|
||||
std::optional<std::uint64_t> expected_device_generation;
|
||||
std::uint64_t authority_generation{0};
|
||||
safety::SafetyClock::time_point deadline{
|
||||
safety::SafetyClock::time_point::max()};
|
||||
};
|
||||
|
||||
// Binds a long-lived control stream to one coordinator permit. Stream
|
||||
// protocols retain their own sequence, watchdog, and control-lease rules;
|
||||
// this object owns the safety epoch/device-generation checks shared by all of
|
||||
// them. It deliberately does not use the unary idempotency ledger.
|
||||
class GrpcStreamingSafetySession final {
|
||||
public:
|
||||
GrpcStreamingSafetySession(
|
||||
safety::SafetyManager& coordinator,
|
||||
const GrpcRequestContext& request_context,
|
||||
GrpcStreamingSafetyOpen open);
|
||||
|
||||
GrpcStreamingSafetySession(
|
||||
GrpcStreamingSafetySession&&) noexcept = default;
|
||||
GrpcStreamingSafetySession& operator=(
|
||||
GrpcStreamingSafetySession&&) noexcept = default;
|
||||
GrpcStreamingSafetySession(
|
||||
const GrpcStreamingSafetySession&) = delete;
|
||||
GrpcStreamingSafetySession& operator=(
|
||||
const GrpcStreamingSafetySession&) = delete;
|
||||
|
||||
bool admitted() const noexcept { return permit_.has_value(); }
|
||||
const grpc::Status& status() const noexcept { return status_; }
|
||||
const safety::AdmissionDecision& admissionDecision() const noexcept
|
||||
{
|
||||
return admission_decision_;
|
||||
}
|
||||
|
||||
bool revalidate();
|
||||
safety::DispatchGuard beginDispatch();
|
||||
std::uint64_t safetyEpoch() const noexcept;
|
||||
std::uint64_t deviceGeneration() const noexcept;
|
||||
std::uint64_t authorityGeneration() const noexcept;
|
||||
|
||||
private:
|
||||
void reject_(safety::SafetyReason reason, std::string detail);
|
||||
|
||||
safety::SafetyManager* coordinator_{nullptr};
|
||||
std::optional<safety::AdmissionPermit> permit_;
|
||||
safety::AdmissionDecision admission_decision_;
|
||||
grpc::Status status_;
|
||||
};
|
||||
|
||||
// Owns one unary command from identity reservation through the final hardware
|
||||
// dispatch fence. Legacy/Shadow calls without a command ID still use admission,
|
||||
// but deliberately remain outside the idempotency ledger for wire compatibility.
|
||||
class GrpcCommandTransaction final {
|
||||
public:
|
||||
GrpcCommandTransaction(
|
||||
safety::SafetyManager& coordinator,
|
||||
GrpcRequestContext request_context,
|
||||
GrpcMethodPolicy method_policy,
|
||||
const google::protobuf::Message& request,
|
||||
google::protobuf::Message& response);
|
||||
~GrpcCommandTransaction() noexcept;
|
||||
|
||||
GrpcCommandTransaction(GrpcCommandTransaction&& other) noexcept;
|
||||
GrpcCommandTransaction& operator=(
|
||||
GrpcCommandTransaction&& other) noexcept;
|
||||
GrpcCommandTransaction(const GrpcCommandTransaction&) = delete;
|
||||
GrpcCommandTransaction& operator=(const GrpcCommandTransaction&) = delete;
|
||||
|
||||
bool shouldExecute() const noexcept { return should_execute_; }
|
||||
const grpc::Status& status() const noexcept { return status_; }
|
||||
const safety::AdmissionDecision& admissionDecision() const noexcept
|
||||
{
|
||||
return admission_decision_;
|
||||
}
|
||||
|
||||
// Must be called immediately before the first driver/SDK mutation. The
|
||||
// returned guard remains owned by this transaction until finish().
|
||||
bool beginDispatch();
|
||||
// Long-running unary commands may submit more than one hardware command.
|
||||
// Revalidate the original permit between submissions, then hold the
|
||||
// returned guard only around one driver/SDK mutation.
|
||||
bool revalidate();
|
||||
safety::DispatchGuard beginScopedDispatch();
|
||||
// Internal mitigation for a command-owned activity. This obtains a fresh
|
||||
// Stop-lane permit, so an expired/revoked Actuate permit cannot suppress a
|
||||
// physical stop.
|
||||
safety::DispatchGuard beginSafetyStopDispatch();
|
||||
const grpc::Status& dispatchStatus() const noexcept
|
||||
{
|
||||
return dispatch_status_;
|
||||
}
|
||||
|
||||
grpc::Status finish(
|
||||
grpc::Status operation_status,
|
||||
safety::SafetyReason reason = safety::SafetyReason::None,
|
||||
std::optional<safety::CommandLifecycle> lifecycle = std::nullopt);
|
||||
grpc::Status finishException(std::string detail) noexcept;
|
||||
|
||||
const std::string& deviceId() const noexcept { return device_id_; }
|
||||
const std::string& commandId() const noexcept { return command_id_; }
|
||||
std::uint64_t safetyEpoch() const noexcept
|
||||
{
|
||||
return admission_decision_.safety_epoch;
|
||||
}
|
||||
std::uint64_t deviceGeneration() const noexcept
|
||||
{
|
||||
return admission_decision_.device_generation;
|
||||
}
|
||||
|
||||
private:
|
||||
void initialize_(const google::protobuf::Message& request);
|
||||
void rejectBeforeDispatch_(
|
||||
safety::SafetyReason reason,
|
||||
std::string detail,
|
||||
grpc::Status status,
|
||||
bool complete_reserved_record);
|
||||
bool restoreOutcome_(const safety::CommandOutcome& outcome);
|
||||
bool completeLedger_(
|
||||
safety::CommandLifecycle lifecycle,
|
||||
safety::SafetyReason reason,
|
||||
const std::string& detail,
|
||||
bool hardware_submission_possible) noexcept;
|
||||
void populateFeedback_(
|
||||
bool success,
|
||||
safety::SafetyReason reason,
|
||||
safety::CommandLifecycle lifecycle,
|
||||
const std::string& detail);
|
||||
void abandon_() noexcept;
|
||||
|
||||
safety::SafetyManager* coordinator_{nullptr};
|
||||
GrpcRequestContext request_context_;
|
||||
GrpcMethodPolicy method_policy_;
|
||||
google::protobuf::Message* response_{nullptr};
|
||||
safety::CommandLedger::Ticket ledger_ticket_;
|
||||
std::optional<safety::AdmissionPermit> permit_;
|
||||
std::optional<safety::DispatchGuard> dispatch_guard_;
|
||||
safety::AdmissionDecision admission_decision_;
|
||||
grpc::Status status_;
|
||||
grpc::Status dispatch_status_;
|
||||
std::string device_id_;
|
||||
std::string command_id_;
|
||||
std::string payload_hash_;
|
||||
safety::SafetyClock::time_point deadline_{
|
||||
safety::SafetyClock::time_point::max()};
|
||||
bool should_execute_{false};
|
||||
bool owns_ledger_record_{false};
|
||||
bool dispatch_started_{false};
|
||||
bool completed_{false};
|
||||
std::uint64_t safety_stop_sequence_{0};
|
||||
};
|
||||
|
||||
using GrpcUnaryCommandOperation =
|
||||
std::function<grpc::Status(GrpcCommandTransaction&)>;
|
||||
|
||||
grpc::Status executeRegisteredGrpcCommand(
|
||||
const std::shared_ptr<GrpcSecurityGateway>& gateway,
|
||||
grpc::ServerContext* server_context,
|
||||
safety::SafetyManager& coordinator,
|
||||
const std::string& full_method_name,
|
||||
const google::protobuf::Message* request,
|
||||
google::protobuf::Message* response,
|
||||
GrpcUnaryCommandOperation operation);
|
||||
|
||||
// For a legacy RPC whose validated request selects one of several fixed
|
||||
// server-side intents. Gateway authorization still uses the registered method
|
||||
// policy; the supplied policy only narrows Coordinator admission after the
|
||||
// server has parsed the request (for example PTZ START versus STOP).
|
||||
grpc::Status executeServerDerivedGrpcCommand(
|
||||
const std::shared_ptr<GrpcSecurityGateway>& gateway,
|
||||
grpc::ServerContext* server_context,
|
||||
safety::SafetyManager& coordinator,
|
||||
const std::string& full_method_name,
|
||||
GrpcMethodPolicy effective_policy,
|
||||
const google::protobuf::Message* request,
|
||||
google::protobuf::Message* response,
|
||||
GrpcUnaryCommandOperation operation);
|
||||
|
||||
// Stable across processes and protobuf map iteration order. Transport identity
|
||||
// fields and client timestamps are excluded; device generation remains part of
|
||||
// the semantic payload.
|
||||
std::string deterministicGrpcPayloadHash(
|
||||
const std::string& full_method_name,
|
||||
const google::protobuf::Message& request);
|
||||
|
||||
} // namespace cmvr::service
|
||||
37
cmvr-es/service/grpc/server/include/grpc_dexhand_service.h
Normal file
37
cmvr-es/service/grpc/server/include/grpc_dexhand_service.h
Normal file
@ -0,0 +1,37 @@
|
||||
//
|
||||
// Created by linbo on 2025/7/3.
|
||||
//
|
||||
|
||||
#ifndef GRPC_DEXHAND_SERVICE_H
|
||||
#define GRPC_DEXHAND_SERVICE_H
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/api/dexhand_service.grpc.pb.h"
|
||||
#include "common/base/grpc_utils.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
#include "devices/dexhand/abstract_dexhand.h"
|
||||
namespace cmvr::service {
|
||||
class GrpcSecurityGateway;
|
||||
class gRPCDexHandServiceImpl final: public api::DexHandService::Service {
|
||||
public:
|
||||
gRPCDexHandServiceImpl();
|
||||
explicit gRPCDexHandServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
~gRPCDexHandServiceImpl() override = default;
|
||||
grpc::Status GetStatus(grpc::ServerContext* context, const api::GetDexHandStateCommand_Request* request,api::GetDexHandStateCommand_Feedback* response) override;
|
||||
grpc::Status SetDexHandPos(grpc::ServerContext* context, const cmvr::api::SetDexHandPositionsCommand_Request* request, cmvr::api::SetDexHandPositionsCommand_Feedback* response) override;
|
||||
grpc::Status SetDexHandAngle(grpc::ServerContext* context, const cmvr::api::SetDexHandAnglesCommand_Request* request, cmvr::api::SetDexHandAnglesCommand_Feedback* response) override;
|
||||
grpc::Status SetDexHandForce(grpc::ServerContext* context, const cmvr::api::SetDexHandForceCommand_Request* request, cmvr::api::SetDexHandForceCommand_Feedback* response) override;
|
||||
grpc::Status SetDexHandSpeed(grpc::ServerContext* context, const cmvr::api::SetDexHandSpeedCommand_Request* request, cmvr::api::SetDexHandSpeedCommand_Feedback* response) override;
|
||||
grpc::Status SetDexHandPresetAct(grpc::ServerContext* context, const cmvr::api::SetDexHandPresetActCommand_Request* request, cmvr::api::SetDexHandPresetActCommand_Feedback* response) override;
|
||||
grpc::Status GetSensorData(grpc::ServerContext* context, const cmvr::api::GetSensorDataCommand_Request* request, cmvr::api::GetSensorDataCommand_Feedback* response) override;
|
||||
grpc::Status GetSensorDataStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetSensorDataStreamCommand_Feedback, cmvr::api::GetSensorDataStreamCommand_Request>* stream) override;
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif //GRPC_DEXHAND_SERVICE_H
|
||||
@ -0,0 +1,37 @@
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <grpcpp/support/server_interceptor.h>
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
enum class GrpcFailureKind {
|
||||
APPLICATION,
|
||||
GRPC_STATUS,
|
||||
MALFORMED_RESPONSE,
|
||||
};
|
||||
|
||||
enum class GrpcFailureSeverity {
|
||||
INFO,
|
||||
WARNING,
|
||||
ERROR,
|
||||
};
|
||||
|
||||
struct GrpcFailureRecord {
|
||||
GrpcFailureKind kind{GrpcFailureKind::APPLICATION};
|
||||
std::string method;
|
||||
std::string peer;
|
||||
grpc::StatusCode status_code{grpc::StatusCode::OK};
|
||||
GrpcFailureSeverity severity{GrpcFailureSeverity::ERROR};
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
using GrpcFailureSink = std::function<void(const GrpcFailureRecord&)>;
|
||||
|
||||
std::unique_ptr<grpc::experimental::ServerInterceptorFactoryInterface>
|
||||
makeGrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink = {});
|
||||
|
||||
} // namespace cmvr::service
|
||||
56
cmvr-es/service/grpc/server/include/grpc_head_service.h
Normal file
56
cmvr-es/service/grpc/server/include/grpc_head_service.h
Normal file
@ -0,0 +1,56 @@
|
||||
#ifndef BIO_HEAD_SERVICE_H
|
||||
#define BIO_HEAD_SERVICE_H
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/api/biohead_service.grpc.pb.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
#include "devices/biohead/abstract_biohead.h"
|
||||
|
||||
namespace cmvr::service
|
||||
{
|
||||
class GrpcSecurityGateway;
|
||||
class gRPCMBioHeadServiceImpl : public api::BioHeadService::Service {
|
||||
public:
|
||||
gRPCMBioHeadServiceImpl();
|
||||
explicit gRPCMBioHeadServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
~gRPCMBioHeadServiceImpl() override = default;
|
||||
|
||||
grpc::Status SetExpression(grpc::ServerContext* context,
|
||||
const api::SetFacialExpression_Request* request,
|
||||
api::SetFacialExpression_Feedback* response) override;
|
||||
|
||||
grpc::Status StreamExpression(grpc::ServerContext* context,
|
||||
grpc::ServerReaderWriter<api::StreamFacialExpression_Feedback, api::StreamFacialExpression_Request>* stream) override;
|
||||
|
||||
|
||||
grpc::Status GetSystemStatus(grpc::ServerContext* context,
|
||||
const api::GetStatus_Request* request,
|
||||
api::GetStatus_Feedback* response) override;
|
||||
|
||||
grpc::Status EmergencyStop(grpc::ServerContext* context,
|
||||
const api::EmergencyStop_Request* request,
|
||||
api::EmergencyStop_Feedback* response) override;
|
||||
grpc::Status SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response) override;
|
||||
|
||||
grpc::Status SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response) override;
|
||||
|
||||
grpc::Status Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response) override;
|
||||
|
||||
grpc::Status Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response) override;
|
||||
|
||||
grpc::Status ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response) override;
|
||||
|
||||
grpc::Status ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response) override;
|
||||
|
||||
grpc::Status ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response) override;
|
||||
|
||||
grpc::Status ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) override;
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
};
|
||||
} // namespace cmvr::service
|
||||
|
||||
#endif // BIO_HEAD_SERVICE_H
|
||||
30
cmvr-es/service/grpc/server/include/grpc_hlc_service.h
Normal file
30
cmvr-es/service/grpc/server/include/grpc_hlc_service.h
Normal file
@ -0,0 +1,30 @@
|
||||
//
|
||||
// Created by lgv on 2025/8/25.
|
||||
//
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/api/hlc_service.grpc.pb.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
|
||||
namespace cmvr {
|
||||
namespace service {
|
||||
class GrpcSecurityGateway;
|
||||
class gRPCHlcServiceImpl final : public api::HlcService::Service {
|
||||
public:
|
||||
gRPCHlcServiceImpl();
|
||||
explicit gRPCHlcServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
~gRPCHlcServiceImpl() = default;
|
||||
grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override;
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
};
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,37 @@
|
||||
//
|
||||
// Created by linbo on 2025/6/13.
|
||||
// Created by xtkuang on 2025/6/13.
|
||||
//
|
||||
|
||||
#ifndef GRPC_MICROPHONE_SERVICE_H
|
||||
#define GRPC_MICROPHONE_SERVICE_H
|
||||
#include <memory>
|
||||
#include "cmvr/api/microphone_service.grpc.pb.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
#include "devices/microphone/abstract_microphone.h"
|
||||
#include "common/base/grpc_utils.h"
|
||||
|
||||
namespace cmvr::service
|
||||
{
|
||||
class GrpcSecurityGateway;
|
||||
|
||||
class gRPCMicroPhoneServiceImpl: public api::MicPhoneService::Service {
|
||||
public:
|
||||
gRPCMicroPhoneServiceImpl();
|
||||
explicit gRPCMicroPhoneServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
~gRPCMicroPhoneServiceImpl() override = default;
|
||||
grpc::Status GetStatus(grpc::ServerContext* context, const api::GetMicStateCommand_Request* request,api::GetMicStateCommand_Feedback* response) override;
|
||||
grpc::Status StartRecord(grpc::ServerContext* context, const api::StartMicRecordingCommand_Request* request,api::StartMicRecordingCommand_Feedback* response) override;
|
||||
grpc::Status StopRecord(grpc::ServerContext* context, const api::StopMicRecordingCommand_Request* request,api::StopMicRecordingCommand_Feedback* response) override;
|
||||
grpc::Status PauseRecord(grpc::ServerContext* context, const api::PauseMicRecordingCommand_Request* request,api::PauseMicRecordingCommand_Feedback* response) override;
|
||||
grpc::Status ResumeRecord(grpc::ServerContext* context, const api::ResumeMicRecordingCommand_Request* request,api::ResumeMicRecordingCommand_Feedback* response) override;
|
||||
grpc::Status StreamAudio(grpc::ServerContext* context, const api::StreamMicAudioCommand_Request* request, grpc::ServerWriter<api::StreamMicAudioCommand_Feedback>* writer) override;
|
||||
grpc::Status SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request,api::SetMicPhoneVolumeCommand_Feedback* response) override;
|
||||
grpc::Status GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request,api::GetMicPhoneVolumeCommand_Feedback* response) override;
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
};
|
||||
}
|
||||
#endif //GRPC_MICROPHONE_SERVICE_H
|
||||
222
cmvr-es/service/grpc/server/include/grpc_motor_service.h
Normal file
222
cmvr-es/service/grpc/server/include/grpc_motor_service.h
Normal file
@ -0,0 +1,222 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "cmvr/api/motor_service.grpc.pb.h"
|
||||
#include "devices/motor/abstract_motor.h"
|
||||
#include "service/grpc/server/include/motor_activity_coordinator.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
class DeviceManager;
|
||||
}
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
class gRPCMotorServiceImplTestAccess;
|
||||
class GrpcCommandTransaction;
|
||||
class GrpcSecurityGateway;
|
||||
struct GrpcRequestContext;
|
||||
|
||||
// A deliberately thin synchronous gRPC facade over AbstractMotor. It does not
|
||||
// schedule trajectories or retain asynchronous operations. The small amount of
|
||||
// state below only prevents two RPCs from owning one motor at the same time and
|
||||
// lets emergencyStop invalidate an already-running blocking RPC/stream.
|
||||
class gRPCMotorServiceImpl final : public api::MotorService::Service {
|
||||
public:
|
||||
gRPCMotorServiceImpl();
|
||||
explicit gRPCMotorServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
~gRPCMotorServiceImpl() override = default;
|
||||
|
||||
grpc::Status setZero(grpc::ServerContext* context,
|
||||
const api::SetMotorZeroRequest* request,
|
||||
api::MotorCommandResponse* response) override;
|
||||
grpc::Status moveToZero(grpc::ServerContext* context,
|
||||
const api::MoveMotorToZeroRequest* request,
|
||||
api::MotorCommandResponse* response) override;
|
||||
grpc::Status profilePosition(grpc::ServerContext* context,
|
||||
const api::ProfilePositionRequest* request,
|
||||
api::MotorCommandResponse* response) override;
|
||||
grpc::Status profileVelocity(grpc::ServerContext* context,
|
||||
const api::ProfileVelocityRequest* request,
|
||||
api::MotorCommandResponse* response) override;
|
||||
grpc::Status streamCyclicPosition(
|
||||
grpc::ServerContext* context,
|
||||
grpc::ServerReaderWriter<api::CyclicControlResponse,
|
||||
api::CyclicPositionRequest>* stream) override;
|
||||
grpc::Status streamCyclicVelocity(
|
||||
grpc::ServerContext* context,
|
||||
grpc::ServerReaderWriter<api::CyclicControlResponse,
|
||||
api::CyclicVelocityRequest>* stream) override;
|
||||
grpc::Status emergencyStop(grpc::ServerContext* context,
|
||||
const api::EmergencyStopRequest* request,
|
||||
api::MotorCommandResponse* response) override;
|
||||
grpc::Status getStatus(grpc::ServerContext* context,
|
||||
const api::GetMotorStatusRequest* request,
|
||||
api::GetMotorStatusResponse* response) override;
|
||||
grpc::Status setEnabled(grpc::ServerContext* context,
|
||||
const api::SetMotorEnabledRequest* request,
|
||||
api::MotorCommandResponse* response) override;
|
||||
|
||||
private:
|
||||
friend class gRPCMotorServiceImplTestAccess;
|
||||
|
||||
struct MotorControlState {
|
||||
std::mutex mutex;
|
||||
// Serializes all motion/enable writes with emergency quick-stop. The
|
||||
// cancel-generation check and the corresponding motor write must occur
|
||||
// while this mutex is held to prevent stale writes after an E-stop.
|
||||
std::mutex command_mutex;
|
||||
// Keeps the two-phase best-effort/final quick-stop sequence exclusive.
|
||||
// Without this, one concurrent E-stop could clear the shared
|
||||
// in-progress flag while another E-stop is still dispatching.
|
||||
std::mutex emergency_mutex;
|
||||
// Serializes exception cleanup from ownership inspection through the
|
||||
// final release. A second stale cleanup must re-check ownership only
|
||||
// after the first cleanup has fully completed.
|
||||
std::mutex exception_cleanup_mutex;
|
||||
bool busy{false};
|
||||
bool emergency_stopped{false};
|
||||
bool emergency_stop_in_progress{false};
|
||||
bool exception_cleanup_pending{false};
|
||||
std::uint64_t cancel_generation{0};
|
||||
api::MotorControlType active_control{api::MOTOR_CONTROL_NONE};
|
||||
std::string last_error;
|
||||
};
|
||||
|
||||
struct ResolvedMotor {
|
||||
std::shared_ptr<device::AbstractMotor> motor;
|
||||
std::shared_ptr<MotorControlState> control;
|
||||
};
|
||||
|
||||
enum class ResolveAccess {
|
||||
Control,
|
||||
Observe,
|
||||
};
|
||||
|
||||
struct MotorControlEntry {
|
||||
std::weak_ptr<device::AbstractMotor> owner;
|
||||
std::shared_ptr<MotorControlState> state;
|
||||
MotorActivityCoordinator::Registration stop_all_registration;
|
||||
};
|
||||
|
||||
class ControlLease {
|
||||
public:
|
||||
ControlLease(std::shared_ptr<MotorControlState> state,
|
||||
std::uint64_t generation);
|
||||
~ControlLease();
|
||||
ControlLease(const ControlLease&) = delete;
|
||||
ControlLease& operator=(const ControlLease&) = delete;
|
||||
|
||||
std::uint64_t generation() const noexcept { return generation_; }
|
||||
|
||||
private:
|
||||
std::shared_ptr<MotorControlState> state_;
|
||||
std::uint64_t generation_{0};
|
||||
int uncaught_on_entry_{0};
|
||||
};
|
||||
|
||||
grpc::Status resolveMotor(const api::MotorTarget& target,
|
||||
ResolvedMotor& resolved,
|
||||
ResolveAccess access = ResolveAccess::Control) const;
|
||||
std::shared_ptr<MotorControlState> stateFor(
|
||||
const std::shared_ptr<device::AbstractMotor>& motor) const;
|
||||
std::shared_ptr<MotorControlState> existingStateFor(
|
||||
const std::shared_ptr<device::AbstractMotor>& motor) const;
|
||||
std::unique_ptr<ControlLease> acquireControl(
|
||||
const ResolvedMotor& resolved,
|
||||
api::MotorControlType control,
|
||||
grpc::Status& failure,
|
||||
bool allow_emergency_stopped = false) const;
|
||||
|
||||
grpc::Status runProfilePosition(grpc::ServerContext* context,
|
||||
const ResolvedMotor& resolved,
|
||||
double target_position_rad,
|
||||
double max_velocity_rad_s,
|
||||
double acceleration_rad_s2,
|
||||
const api::MotorWaitOptions& wait,
|
||||
api::MotorCommandResponse* response,
|
||||
GrpcCommandTransaction& command);
|
||||
grpc::Status waitForPosition(grpc::ServerContext* context,
|
||||
const ResolvedMotor& resolved,
|
||||
std::uint64_t generation,
|
||||
double target_position_rad,
|
||||
const api::MotorWaitOptions& wait,
|
||||
api::MotorCommandResponse* response,
|
||||
std::chrono::steady_clock::time_point started);
|
||||
grpc::Status waitForVelocity(grpc::ServerContext* context,
|
||||
const ResolvedMotor& resolved,
|
||||
std::uint64_t generation,
|
||||
double target_velocity_rad_s,
|
||||
const api::MotorWaitOptions& wait,
|
||||
api::MotorCommandResponse* response,
|
||||
std::chrono::steady_clock::time_point started);
|
||||
|
||||
grpc::Status setZeroImpl(grpc::ServerContext* context,
|
||||
const api::SetMotorZeroRequest* request,
|
||||
api::MotorCommandResponse* response,
|
||||
GrpcCommandTransaction& command);
|
||||
grpc::Status moveToZeroImpl(grpc::ServerContext* context,
|
||||
const api::MoveMotorToZeroRequest* request,
|
||||
api::MotorCommandResponse* response,
|
||||
GrpcCommandTransaction& command);
|
||||
grpc::Status profilePositionImpl(
|
||||
grpc::ServerContext* context,
|
||||
const api::ProfilePositionRequest* request,
|
||||
api::MotorCommandResponse* response,
|
||||
GrpcCommandTransaction& command);
|
||||
grpc::Status profileVelocityImpl(
|
||||
grpc::ServerContext* context,
|
||||
const api::ProfileVelocityRequest* request,
|
||||
api::MotorCommandResponse* response,
|
||||
GrpcCommandTransaction& command);
|
||||
grpc::Status emergencyStopImpl(
|
||||
grpc::ServerContext* context,
|
||||
const api::EmergencyStopRequest* request,
|
||||
api::MotorCommandResponse* response,
|
||||
GrpcCommandTransaction& command);
|
||||
grpc::Status getStatusImpl(grpc::ServerContext* context,
|
||||
const api::GetMotorStatusRequest* request,
|
||||
api::GetMotorStatusResponse* response);
|
||||
grpc::Status setEnabledImpl(
|
||||
grpc::ServerContext* context,
|
||||
const api::SetMotorEnabledRequest* request,
|
||||
api::MotorCommandResponse* response,
|
||||
GrpcCommandTransaction& command);
|
||||
grpc::Status streamCyclicPositionImpl(
|
||||
grpc::ServerContext* context,
|
||||
grpc::ServerReaderWriter<api::CyclicControlResponse,
|
||||
api::CyclicPositionRequest>* stream,
|
||||
std::optional<api::MotorTarget>& cleanup_target,
|
||||
const GrpcRequestContext& request_context);
|
||||
grpc::Status streamCyclicVelocityImpl(
|
||||
grpc::ServerContext* context,
|
||||
grpc::ServerReaderWriter<api::CyclicControlResponse,
|
||||
api::CyclicVelocityRequest>* stream,
|
||||
std::optional<api::MotorTarget>& cleanup_target,
|
||||
const GrpcRequestContext& request_context);
|
||||
|
||||
void bestEffortQuickStop(const api::MotorTarget& target,
|
||||
const std::string& error) noexcept;
|
||||
void latchUnsafeAfterFailedStop(
|
||||
const std::shared_ptr<MotorControlState>& state,
|
||||
const std::string& error) const;
|
||||
void fillMotorStatus(const ResolvedMotor& resolved,
|
||||
api::MotorStatus* status) const;
|
||||
void setLastError(const std::shared_ptr<MotorControlState>& state,
|
||||
const std::string& error) const;
|
||||
|
||||
device::DeviceManager& dmgr_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
mutable std::mutex states_mutex_;
|
||||
mutable std::unordered_map<const device::AbstractMotor*,
|
||||
MotorControlEntry> states_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::service
|
||||
38
cmvr-es/service/grpc/server/include/grpc_recovery_audit.h
Normal file
38
cmvr-es/service/grpc/server/include/grpc_recovery_audit.h
Normal file
@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
struct RecoveryAuditRecord {
|
||||
std::uint64_t occurred_at_unix_ms{0};
|
||||
std::string stage;
|
||||
std::string correlation_id;
|
||||
std::string principal_id;
|
||||
std::string peer;
|
||||
std::string recovery_id;
|
||||
std::string reason;
|
||||
std::string mode;
|
||||
bool all_devices{false};
|
||||
std::vector<std::string> device_ids;
|
||||
std::uint64_t expected_safety_epoch{0};
|
||||
std::uint64_t previous_safety_epoch{0};
|
||||
std::uint64_t current_safety_epoch{0};
|
||||
std::string result;
|
||||
};
|
||||
|
||||
class RecoveryAuditSink {
|
||||
public:
|
||||
virtual ~RecoveryAuditSink() = default;
|
||||
virtual bool append(
|
||||
const RecoveryAuditRecord& record,
|
||||
std::string* error = nullptr) noexcept = 0;
|
||||
};
|
||||
|
||||
std::shared_ptr<RecoveryAuditSink> makeFileRecoveryAuditSink(
|
||||
std::string path);
|
||||
|
||||
} // namespace cmvr::service
|
||||
@ -0,0 +1,18 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h"
|
||||
#include "devices/arm/robot_arm.h"
|
||||
#include "service/grpc/server/include/grpc_arm_teleop_service.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
// Creates a fail-closed adapter from the process RobotArm abstraction to the
|
||||
// session-based ArmTeleop backend. available() remains false unless the config,
|
||||
// RobotModel and RobotArm capability all pass static validation.
|
||||
std::shared_ptr<ArmTeleopBackend> makeRobotArmTeleopBackend(
|
||||
std::shared_ptr<device::RobotArm> arm,
|
||||
const config::ArmTeleopBackendConfig& config);
|
||||
|
||||
} // namespace cmvr::service
|
||||
@ -0,0 +1,43 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
namespace cmvr::safety {
|
||||
class SafetyManager;
|
||||
}
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
class ActionQueueExecutor;
|
||||
class StopOperationDispatcher;
|
||||
|
||||
class GrpcSafetyParticipantRegistration final {
|
||||
public:
|
||||
~GrpcSafetyParticipantRegistration();
|
||||
GrpcSafetyParticipantRegistration(
|
||||
GrpcSafetyParticipantRegistration&&) noexcept;
|
||||
GrpcSafetyParticipantRegistration& operator=(
|
||||
GrpcSafetyParticipantRegistration&&) noexcept;
|
||||
GrpcSafetyParticipantRegistration(
|
||||
const GrpcSafetyParticipantRegistration&) = delete;
|
||||
GrpcSafetyParticipantRegistration& operator=(
|
||||
const GrpcSafetyParticipantRegistration&) = delete;
|
||||
|
||||
private:
|
||||
friend std::unique_ptr<GrpcSafetyParticipantRegistration>
|
||||
registerGrpcSafetyParticipants(
|
||||
safety::SafetyManager&,
|
||||
std::shared_ptr<ActionQueueExecutor>,
|
||||
std::shared_ptr<StopOperationDispatcher>);
|
||||
struct Impl;
|
||||
explicit GrpcSafetyParticipantRegistration(std::unique_ptr<Impl> impl);
|
||||
std::unique_ptr<Impl> impl_;
|
||||
};
|
||||
|
||||
std::unique_ptr<GrpcSafetyParticipantRegistration>
|
||||
registerGrpcSafetyParticipants(
|
||||
safety::SafetyManager& coordinator,
|
||||
std::shared_ptr<ActionQueueExecutor> action_queue,
|
||||
std::shared_ptr<StopOperationDispatcher> stop_dispatcher);
|
||||
|
||||
} // namespace cmvr::service
|
||||
37
cmvr-es/service/grpc/server/include/grpc_safety_proto.h
Normal file
37
cmvr-es/service/grpc/server/include/grpc_safety_proto.h
Normal file
@ -0,0 +1,37 @@
|
||||
#pragma once
|
||||
|
||||
#include "cmvr/api/safety_command.pb.h"
|
||||
#include "manager/safety_manager/include/safety_manager.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
api::CommandReasonCode toApiSafetyReason(
|
||||
safety::SafetyReason value) noexcept;
|
||||
safety::SafetyReason fromApiSafetyReason(
|
||||
api::CommandReasonCode value) noexcept;
|
||||
api::SafetyTriState toApiSafetyTriState(
|
||||
safety::TriState value) noexcept;
|
||||
api::SafetyCondition toApiSafetyCondition(
|
||||
safety::SafetyCondition value) noexcept;
|
||||
api::SystemAdmissionState toApiSystemAdmissionState(
|
||||
safety::SystemAdmissionState value) noexcept;
|
||||
api::DeviceAdmissionState toApiDeviceAdmissionState(
|
||||
safety::DeviceAdmissionState value) noexcept;
|
||||
api::SafetyBlockerScope toApiSafetyBlockerScope(
|
||||
safety::BlockerScope value) noexcept;
|
||||
api::SafetyRecoveryRequirement toApiRecoveryRequirement(
|
||||
safety::RecoveryRequirement value) noexcept;
|
||||
api::SafetyOperationResult toApiRecoveryResult(
|
||||
safety::RecoveryResultCode value) noexcept;
|
||||
|
||||
void populateDeviceSafetyState(
|
||||
const safety::DeviceSafetyStateView& source,
|
||||
api::DeviceSafetyStateInfo& destination);
|
||||
void populateSafetyTargetResult(
|
||||
const safety::SafetyTargetResult& source,
|
||||
api::SafetyOperationTargetResult& destination);
|
||||
void populateSafetyParticipantState(
|
||||
const safety::ParticipantSafetyStateView& source,
|
||||
api::SafetyParticipantStateInfo& destination);
|
||||
|
||||
} // namespace cmvr::service
|
||||
268
cmvr-es/service/grpc/server/include/grpc_security.h
Normal file
268
cmvr-es/service/grpc/server/include/grpc_security.h
Normal file
@ -0,0 +1,268 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <grpcpp/server_context.h>
|
||||
#include <grpcpp/support/status.h>
|
||||
|
||||
#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h"
|
||||
#include "manager/safety_manager/include/safety_types.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
enum class GrpcTransportSecurity {
|
||||
Insecure,
|
||||
ServerTls,
|
||||
MutualTls,
|
||||
};
|
||||
|
||||
enum class GrpcAuthenticationMethod {
|
||||
Disabled,
|
||||
StaticToken,
|
||||
Jwt,
|
||||
TlsClientCertificate,
|
||||
};
|
||||
|
||||
enum class GrpcRole {
|
||||
Anonymous,
|
||||
Observer,
|
||||
Operator,
|
||||
SafetyAdmin,
|
||||
};
|
||||
|
||||
enum class GrpcAccessClass {
|
||||
Read,
|
||||
Mutate,
|
||||
Stop,
|
||||
Recover,
|
||||
};
|
||||
|
||||
enum class GrpcRecoveryExposure {
|
||||
Disabled,
|
||||
LocalOnly,
|
||||
Authorized,
|
||||
};
|
||||
|
||||
struct GrpcPrincipal {
|
||||
std::string id{"anonymous"};
|
||||
GrpcAuthenticationMethod method{GrpcAuthenticationMethod::Disabled};
|
||||
bool authenticated{false};
|
||||
std::vector<GrpcRole> roles{GrpcRole::Anonymous};
|
||||
};
|
||||
|
||||
struct GrpcCallFacts {
|
||||
std::string correlation_id;
|
||||
std::string full_method_name;
|
||||
std::string peer;
|
||||
std::multimap<std::string, std::string> metadata;
|
||||
bool transport_encrypted{false};
|
||||
bool local_peer{false};
|
||||
std::chrono::steady_clock::time_point received_at;
|
||||
std::chrono::steady_clock::time_point deadline;
|
||||
};
|
||||
|
||||
struct GrpcRequestContext {
|
||||
std::string correlation_id;
|
||||
std::string full_method_name;
|
||||
std::string peer;
|
||||
GrpcPrincipal principal;
|
||||
bool transport_encrypted{false};
|
||||
bool local_peer{false};
|
||||
std::chrono::steady_clock::time_point received_at;
|
||||
std::chrono::steady_clock::time_point deadline;
|
||||
};
|
||||
|
||||
struct GrpcMethodPolicy {
|
||||
std::string full_method_name;
|
||||
GrpcAccessClass access{GrpcAccessClass::Read};
|
||||
GrpcRole minimum_role{GrpcRole::Observer};
|
||||
safety::CommandIntent command_intent{safety::CommandIntent::Observe};
|
||||
safety::SafetyPolicyFamily policy_family{
|
||||
safety::SafetyPolicyFamily::Sensor};
|
||||
bool mutating{false};
|
||||
bool safety_lane{false};
|
||||
|
||||
safety::CommandDescriptor commandDescriptor() const
|
||||
{
|
||||
return {
|
||||
full_method_name,
|
||||
command_intent,
|
||||
policy_family,
|
||||
mutating,
|
||||
safety_lane};
|
||||
}
|
||||
};
|
||||
|
||||
struct GrpcSecurityRuntimeConfig {
|
||||
GrpcTransportSecurity transport{GrpcTransportSecurity::Insecure};
|
||||
GrpcAuthenticationMethod authentication{
|
||||
GrpcAuthenticationMethod::Disabled};
|
||||
GrpcRecoveryExposure recovery_exposure{
|
||||
GrpcRecoveryExposure::Disabled};
|
||||
bool allow_insecure_non_loopback{false};
|
||||
bool insecure_non_loopback{false};
|
||||
bool legacy_compatibility{false};
|
||||
std::string recovery_audit_file;
|
||||
};
|
||||
|
||||
struct GrpcSecurityConfigResult {
|
||||
bool valid{false};
|
||||
GrpcSecurityRuntimeConfig config;
|
||||
std::string error;
|
||||
std::vector<std::string> warnings;
|
||||
};
|
||||
|
||||
GrpcSecurityConfigResult resolveGrpcSecurityConfig(
|
||||
const config::GRPCServerConfig& config,
|
||||
const std::string& effective_host);
|
||||
|
||||
bool isLocalGrpcPeer(const std::string& peer) noexcept;
|
||||
bool isLoopbackGrpcHost(const std::string& host) noexcept;
|
||||
|
||||
const char* toString(GrpcTransportSecurity value) noexcept;
|
||||
const char* toString(GrpcAuthenticationMethod value) noexcept;
|
||||
const char* toString(GrpcRecoveryExposure value) noexcept;
|
||||
const char* toString(GrpcRole value) noexcept;
|
||||
|
||||
struct GrpcAuthenticationResult {
|
||||
GrpcPrincipal principal;
|
||||
grpc::Status status;
|
||||
|
||||
bool ok() const noexcept { return status.ok(); }
|
||||
};
|
||||
|
||||
class GrpcAuthenticationProvider {
|
||||
public:
|
||||
virtual ~GrpcAuthenticationProvider() = default;
|
||||
virtual GrpcAuthenticationResult authenticate(
|
||||
const GrpcCallFacts& facts) const = 0;
|
||||
};
|
||||
|
||||
class DisabledGrpcAuthenticationProvider final
|
||||
: public GrpcAuthenticationProvider {
|
||||
public:
|
||||
GrpcAuthenticationResult authenticate(
|
||||
const GrpcCallFacts& facts) const override;
|
||||
};
|
||||
|
||||
struct GrpcAuthorizationDecision {
|
||||
bool allowed{false};
|
||||
grpc::Status status;
|
||||
};
|
||||
|
||||
class GrpcAuthorizationPolicy {
|
||||
public:
|
||||
virtual ~GrpcAuthorizationPolicy() = default;
|
||||
virtual GrpcAuthorizationDecision authorize(
|
||||
const GrpcRequestContext& context,
|
||||
const GrpcMethodPolicy& method) const = 0;
|
||||
};
|
||||
|
||||
class CompatibilityGrpcAuthorizationPolicy final
|
||||
: public GrpcAuthorizationPolicy {
|
||||
public:
|
||||
explicit CompatibilityGrpcAuthorizationPolicy(
|
||||
GrpcRecoveryExposure recovery_exposure);
|
||||
|
||||
GrpcAuthorizationDecision authorize(
|
||||
const GrpcRequestContext& context,
|
||||
const GrpcMethodPolicy& method) const override;
|
||||
|
||||
private:
|
||||
GrpcRecoveryExposure recovery_exposure_;
|
||||
};
|
||||
|
||||
class GrpcMethodPolicyRegistry final {
|
||||
public:
|
||||
bool registerPolicy(GrpcMethodPolicy policy);
|
||||
std::optional<GrpcMethodPolicy> find(
|
||||
const std::string& full_method_name) const;
|
||||
std::vector<GrpcMethodPolicy> snapshot() const;
|
||||
|
||||
private:
|
||||
std::unordered_map<std::string, GrpcMethodPolicy> policies_;
|
||||
};
|
||||
|
||||
const GrpcMethodPolicyRegistry& defaultGrpcMethodPolicyRegistry();
|
||||
|
||||
struct GrpcSecurityAuditRecord {
|
||||
std::string correlation_id;
|
||||
std::string full_method_name;
|
||||
std::string principal_id;
|
||||
std::string peer;
|
||||
GrpcAuthenticationMethod authentication{
|
||||
GrpcAuthenticationMethod::Disabled};
|
||||
GrpcAccessClass access{GrpcAccessClass::Read};
|
||||
bool authenticated{false};
|
||||
bool allowed{false};
|
||||
grpc::StatusCode status_code{grpc::StatusCode::OK};
|
||||
};
|
||||
|
||||
using GrpcSecurityAuditSink =
|
||||
std::function<void(const GrpcSecurityAuditRecord&)>;
|
||||
|
||||
class GrpcCallGuard final {
|
||||
public:
|
||||
GrpcCallGuard(GrpcRequestContext context,
|
||||
GrpcAuthorizationDecision decision);
|
||||
|
||||
bool allowed() const noexcept { return decision_.allowed; }
|
||||
const grpc::Status& status() const noexcept { return decision_.status; }
|
||||
const GrpcRequestContext& context() const noexcept { return context_; }
|
||||
|
||||
private:
|
||||
GrpcRequestContext context_;
|
||||
GrpcAuthorizationDecision decision_;
|
||||
};
|
||||
|
||||
class GrpcSecurityGateway final {
|
||||
public:
|
||||
GrpcSecurityGateway(
|
||||
GrpcSecurityRuntimeConfig config,
|
||||
std::shared_ptr<const GrpcAuthenticationProvider> authentication,
|
||||
std::shared_ptr<const GrpcAuthorizationPolicy> authorization,
|
||||
GrpcSecurityAuditSink audit_sink = {});
|
||||
|
||||
GrpcCallGuard beginCall(
|
||||
grpc::ServerContext* server_context,
|
||||
const GrpcMethodPolicy& method) const;
|
||||
GrpcCallGuard beginCall(
|
||||
GrpcCallFacts facts,
|
||||
const GrpcMethodPolicy& method) const;
|
||||
|
||||
const GrpcSecurityRuntimeConfig& config() const noexcept { return config_; }
|
||||
|
||||
private:
|
||||
GrpcSecurityRuntimeConfig config_;
|
||||
std::shared_ptr<const GrpcAuthenticationProvider> authentication_;
|
||||
std::shared_ptr<const GrpcAuthorizationPolicy> authorization_;
|
||||
GrpcSecurityAuditSink audit_sink_;
|
||||
};
|
||||
|
||||
std::shared_ptr<GrpcSecurityGateway> makeGrpcSecurityGateway(
|
||||
const GrpcSecurityRuntimeConfig& config,
|
||||
GrpcSecurityAuditSink audit_sink = {});
|
||||
|
||||
std::shared_ptr<GrpcSecurityGateway> makeDefaultGrpcSecurityGateway();
|
||||
|
||||
GrpcCallGuard beginRegisteredGrpcCall(
|
||||
const std::shared_ptr<GrpcSecurityGateway>& gateway,
|
||||
grpc::ServerContext* server_context,
|
||||
const std::string& full_method_name);
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
#define CMVR_GRPC_REQUIRE_REGISTERED_CALL(gateway, server_context, method) \
|
||||
const auto cmvr_grpc_call_guard = \
|
||||
::cmvr::service::beginRegisteredGrpcCall( \
|
||||
gateway, server_context, method); \
|
||||
if (!cmvr_grpc_call_guard.allowed()) { \
|
||||
return cmvr_grpc_call_guard.status(); \
|
||||
}
|
||||
38
cmvr-es/service/grpc/server/include/grpc_speaker_service.h
Normal file
38
cmvr-es/service/grpc/server/include/grpc_speaker_service.h
Normal file
@ -0,0 +1,38 @@
|
||||
//
|
||||
// Created by xtkuang on 2025/6/10.
|
||||
//
|
||||
|
||||
#ifndef GRPC_SPEAKER_SERVICE_H
|
||||
#define GRPC_SPEAKER_SERVICE_H
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/api/speaker_service.grpc.pb.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
#include "devices/speaker/abstract_speaker.h"
|
||||
#include "common/base/grpc_utils.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
class GrpcSecurityGateway;
|
||||
class gRPCSpeakerServiceImpl: public api::SpeakerService::Service {
|
||||
public:
|
||||
gRPCSpeakerServiceImpl();
|
||||
explicit gRPCSpeakerServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
~gRPCSpeakerServiceImpl() override = default;
|
||||
grpc::Status GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request,api::GetSpeakerStateCommand_Feedback* response) override;
|
||||
grpc::Status PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request,api::PlayAudioCommand_Feedback* response) override;
|
||||
grpc::Status StreamAudio(grpc::ServerContext* context, grpc::ServerReader<api::StreamSpeakerAudioCommand_Request>* reader, api::StreamSpeakerAudioCommand_Feedback* response) override;
|
||||
grpc::Status StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request,api::StopSpeakerCommand_Feedback* response) override;
|
||||
grpc::Status PausePlayback(grpc::ServerContext* context, const api::PauseSpeakerCommand_Request* request,api::PauseSpeakerCommand_Feedback* response) override;
|
||||
grpc::Status ResumePlayback(grpc::ServerContext* context, const api::ResumeSpeakerCommand_Request* request,api::ResumeSpeakerCommand_Feedback* response) override;
|
||||
grpc::Status SetVolume(grpc::ServerContext* context, const api::SetSpeakerVolumeCommand_Request* request,api::SetSpeakerVolumeCommand_Feedback* response) override;
|
||||
grpc::Status GetVolume(grpc::ServerContext* context, const api::GetSpeakerVolumeCommand_Request* request,api::GetSpeakerVolumeCommand_Feedback* response) override;
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
#endif //GRPC_SPEAKER_SERVICE_H
|
||||
68
cmvr-es/service/grpc/server/include/grpc_system_service.h
Normal file
68
cmvr-es/service/grpc/server/include/grpc_system_service.h
Normal file
@ -0,0 +1,68 @@
|
||||
//
|
||||
// Created by xtkuang on 2025/6/6.
|
||||
//
|
||||
|
||||
#ifndef GRPC_SYSTEM_SERVICE_H
|
||||
#define GRPC_SYSTEM_SERVICE_H
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/api/system_service.grpc.pb.h"
|
||||
#include "common/base/grpc_utils.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
|
||||
namespace cmvr::service
|
||||
{
|
||||
class ActionQueueExecutor;
|
||||
class RecoveryAuditSink;
|
||||
class GrpcSafetyParticipantRegistration;
|
||||
class GrpcSecurityGateway;
|
||||
class StopOperationDispatcher;
|
||||
|
||||
class gRPCSystemServiceImpl: public api::SystemService::Service {
|
||||
public:
|
||||
gRPCSystemServiceImpl();
|
||||
explicit gRPCSystemServiceImpl(
|
||||
std::chrono::milliseconds stop_timeout);
|
||||
explicit gRPCSystemServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
gRPCSystemServiceImpl(
|
||||
std::chrono::milliseconds stop_timeout,
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway);
|
||||
gRPCSystemServiceImpl(
|
||||
std::chrono::milliseconds stop_timeout,
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway,
|
||||
std::shared_ptr<RecoveryAuditSink> recovery_audit_sink);
|
||||
~gRPCSystemServiceImpl() override;
|
||||
// Exposed only to synchronize lifecycle concurrency tests.
|
||||
static bool waitForStopDispatcherDestructionForTesting(
|
||||
std::chrono::milliseconds timeout);
|
||||
// Called by GrpcServerTask before grpc::Server::Shutdown so accepted
|
||||
// ActionQueue handlers can reach a terminal result and do not hold the
|
||||
// synchronous server shutdown open indefinitely.
|
||||
void prepareForShutdown();
|
||||
grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override;
|
||||
grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override;
|
||||
grpc::Status GetDeviceList(grpc::ServerContext* context, const api::GetDeviceListCommand_Request* request, api::GetDeviceListCommand_Feedback* response) override;
|
||||
grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override;
|
||||
grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override;
|
||||
grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override;
|
||||
grpc::Status GetSafetyState(grpc::ServerContext* context, const cmvr::api::GetSafetyStateCommand_Request* request, cmvr::api::GetSafetyStateCommand_Feedback* response) override;
|
||||
grpc::Status RecoverSafetyState(grpc::ServerContext* context, const cmvr::api::RecoverSafetyStateCommand_Request* request, cmvr::api::RecoverSafetyStateCommand_Feedback* response) override;
|
||||
grpc::Status RestoreOperationalState(grpc::ServerContext* context, const cmvr::api::RestoreOperationalStateCommand_Request* request, cmvr::api::RestoreOperationalStateCommand_Feedback* response) override;
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
const std::chrono::milliseconds stop_timeout_;
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway_;
|
||||
std::shared_ptr<RecoveryAuditSink> recovery_audit_sink_;
|
||||
// Process instances share running jobs through a lifecycle registry.
|
||||
// The last service owner joins every worker before replacement.
|
||||
std::shared_ptr<StopOperationDispatcher> stop_dispatcher_;
|
||||
std::shared_ptr<ActionQueueExecutor> action_queue_;
|
||||
std::unique_ptr<GrpcSafetyParticipantRegistration>
|
||||
safety_participant_registration_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif //GRPC_SYSTEM_SERVICE_H
|
||||
133
cmvr-es/service/grpc/server/include/media_activity_coordinator.h
Normal file
133
cmvr-es/service/grpc/server/include/media_activity_coordinator.h
Normal file
@ -0,0 +1,133 @@
|
||||
#ifndef CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H
|
||||
#define CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "service/grpc/stop_all/include/deferred_stop_operation.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
// Coordinates in-process media RPC activity with SystemService::StopAll.
|
||||
// StopAll invalidates the current generation and waits for the affected RPCs
|
||||
// to release their own device leases; it does not stop device lifecycles.
|
||||
class MediaActivityCoordinator final {
|
||||
private:
|
||||
struct Impl;
|
||||
struct SessionState;
|
||||
|
||||
public:
|
||||
using CancelCallback = std::function<void()>;
|
||||
|
||||
struct StopAllTicket {
|
||||
std::uint64_t generation{0};
|
||||
std::uint64_t ticket_id{0};
|
||||
|
||||
bool valid() const noexcept
|
||||
{
|
||||
return generation != 0U && ticket_id != 0U;
|
||||
}
|
||||
};
|
||||
|
||||
struct FinishStopAllResult {
|
||||
bool ticket_consumed{false};
|
||||
bool participant_stopped{false};
|
||||
bool admission_resumed{false};
|
||||
};
|
||||
|
||||
class Session final {
|
||||
public:
|
||||
Session() = default;
|
||||
~Session();
|
||||
|
||||
Session(Session&& other) noexcept;
|
||||
Session& operator=(Session&& other) noexcept;
|
||||
|
||||
Session(const Session&) = delete;
|
||||
Session& operator=(const Session&) = delete;
|
||||
|
||||
explicit operator bool() const noexcept;
|
||||
bool cancelled() const noexcept;
|
||||
|
||||
// Linearizes a short device operation against beginStopAll(). If this
|
||||
// returns false, StopAll won the race and the operation was not run.
|
||||
bool runIfCurrent(const std::function<void()>& operation) const;
|
||||
|
||||
// Claims an optional process-wide resource for this session. This is
|
||||
// used by speaker input because one device cannot safely have two RPCs
|
||||
// feeding and independently stopping the same streaming pipeline.
|
||||
bool claimExclusiveResource(const std::string& resource_key);
|
||||
|
||||
void reset() noexcept;
|
||||
|
||||
private:
|
||||
friend class MediaActivityCoordinator;
|
||||
Session(
|
||||
std::shared_ptr<Impl> impl,
|
||||
std::shared_ptr<SessionState> state);
|
||||
|
||||
std::shared_ptr<Impl> impl_;
|
||||
std::shared_ptr<SessionState> state_;
|
||||
};
|
||||
|
||||
MediaActivityCoordinator();
|
||||
~MediaActivityCoordinator() = default;
|
||||
|
||||
MediaActivityCoordinator(const MediaActivityCoordinator&) = delete;
|
||||
MediaActivityCoordinator& operator=(const MediaActivityCoordinator&) = delete;
|
||||
|
||||
// Returns an invalid session while a StopAll round is in progress.
|
||||
Session beginSession(CancelCallback cancel = {});
|
||||
|
||||
// Pauses new sessions and invalidates all sessions from the previous
|
||||
// generation. Cancellation callbacks are normally invoked before this
|
||||
// returns. SystemService defers them until whole-machine motion stop
|
||||
// requests have been issued, so a callback cannot delay physical stops.
|
||||
// Concurrent callers join the same StopAll round.
|
||||
StopAllTicket beginStopAll(bool defer_cancellation = false);
|
||||
|
||||
// Collects one independently executable, at-most-once cancellation per
|
||||
// invalidated session. Each operation captures SessionState ownership and
|
||||
// can safely outlive this coordinator object without capturing `this`.
|
||||
bool collectCancellationOperations(
|
||||
const StopAllTicket& ticket,
|
||||
std::vector<DeferredStopOperation>& operations,
|
||||
std::string* error = nullptr) const;
|
||||
|
||||
// Legacy synchronous wrapper which serially executes the operations above.
|
||||
// Returns false for a stale ticket or when any callback throws.
|
||||
bool requestCancellation(const StopAllTicket& ticket);
|
||||
|
||||
// Waits until every session invalidated by this ticket has run its cleanup
|
||||
// and unregistered. A timeout leaves admission paused (fail closed).
|
||||
bool waitForStopped(
|
||||
const StopAllTicket& ticket,
|
||||
std::chrono::milliseconds timeout);
|
||||
|
||||
// Completes one StopAll participant. Admission resumes only after every
|
||||
// participant succeeds and all invalidated sessions have exited.
|
||||
bool finishStopAll(
|
||||
const StopAllTicket& ticket,
|
||||
bool all_media_stopped);
|
||||
|
||||
FinishStopAllResult finishStopAllDetailed(
|
||||
const StopAllTicket& ticket,
|
||||
bool all_media_stopped);
|
||||
|
||||
// Test/process teardown hook. Runtime recovery must use a new verified
|
||||
// StopAll or Recover transaction instead of bypassing this latch.
|
||||
void clearForTesting() noexcept;
|
||||
|
||||
private:
|
||||
std::shared_ptr<Impl> impl_;
|
||||
};
|
||||
|
||||
MediaActivityCoordinator& globalMediaActivityCoordinator();
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
#endif // CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H
|
||||
159
cmvr-es/service/grpc/server/include/motor_activity_coordinator.h
Normal file
159
cmvr-es/service/grpc/server/include/motor_activity_coordinator.h
Normal file
@ -0,0 +1,159 @@
|
||||
#ifndef CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H
|
||||
#define CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "service/grpc/stop_all/include/deferred_stop_operation.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
// Coordinates MotorService command dispatch with SystemService::StopAll.
|
||||
// Registered controls expose only operational cancellation and quick-stop;
|
||||
// this coordinator never invokes a MotorManager or device lifecycle method.
|
||||
class MotorActivityCoordinator final {
|
||||
private:
|
||||
struct Impl;
|
||||
|
||||
public:
|
||||
using CancelCallback = std::function<void()>;
|
||||
using QuickStopCallback = std::function<bool()>;
|
||||
using IdleCallback = std::function<bool()>;
|
||||
|
||||
struct StopAllTicket {
|
||||
std::uint64_t generation{0};
|
||||
std::uint64_t ticket_id{0};
|
||||
|
||||
bool valid() const noexcept
|
||||
{
|
||||
return generation != 0U && ticket_id != 0U;
|
||||
}
|
||||
};
|
||||
|
||||
struct FinishStopAllResult {
|
||||
bool ticket_consumed{false};
|
||||
bool participant_stopped{false};
|
||||
bool admission_resumed{false};
|
||||
};
|
||||
|
||||
class Registration final {
|
||||
public:
|
||||
Registration() = default;
|
||||
~Registration();
|
||||
|
||||
Registration(Registration&& other) noexcept;
|
||||
Registration& operator=(Registration&& other) noexcept;
|
||||
|
||||
Registration(const Registration&) = delete;
|
||||
Registration& operator=(const Registration&) = delete;
|
||||
|
||||
explicit operator bool() const noexcept;
|
||||
void reset() noexcept;
|
||||
|
||||
private:
|
||||
friend class MotorActivityCoordinator;
|
||||
Registration(std::shared_ptr<Impl> impl, std::uint64_t id) noexcept;
|
||||
|
||||
std::shared_ptr<Impl> impl_;
|
||||
std::uint64_t id_{0};
|
||||
};
|
||||
|
||||
class AdmissionGuard final {
|
||||
public:
|
||||
AdmissionGuard(AdmissionGuard&&) noexcept = default;
|
||||
AdmissionGuard& operator=(AdmissionGuard&&) noexcept = default;
|
||||
|
||||
AdmissionGuard(const AdmissionGuard&) = delete;
|
||||
AdmissionGuard& operator=(const AdmissionGuard&) = delete;
|
||||
|
||||
bool accepting() const noexcept { return accepting_; }
|
||||
|
||||
private:
|
||||
friend class MotorActivityCoordinator;
|
||||
AdmissionGuard(
|
||||
std::unique_lock<std::mutex>&& lock,
|
||||
bool accepting) noexcept;
|
||||
|
||||
std::unique_lock<std::mutex> lock_;
|
||||
bool accepting_{false};
|
||||
};
|
||||
|
||||
MotorActivityCoordinator();
|
||||
~MotorActivityCoordinator() = default;
|
||||
|
||||
MotorActivityCoordinator(const MotorActivityCoordinator&) = delete;
|
||||
MotorActivityCoordinator& operator=(const MotorActivityCoordinator&) = delete;
|
||||
|
||||
Registration registerControl(
|
||||
CancelCallback cancel,
|
||||
QuickStopCallback quick_stop,
|
||||
IdleCallback idle,
|
||||
std::string description = {});
|
||||
|
||||
// Hold this guard until the MotorControlState has been marked busy. This
|
||||
// makes final command admission atomic with beginStopAll().
|
||||
AdmissionGuard lockAdmission();
|
||||
|
||||
// Invalidates admission for every command from the preceding generation.
|
||||
// With defer_callbacks=false, cancellation callbacks retain their legacy
|
||||
// synchronous behavior. With true, callers must collect and execute every
|
||||
// target operation; each operation orders cancellation before quick-stop.
|
||||
StopAllTicket beginStopAll(bool defer_callbacks = false);
|
||||
|
||||
// Collects one independently executable operation per registration that
|
||||
// belonged to this StopAll round. Operations capture shared state rather
|
||||
// than this coordinator and are idempotent, including their failure result.
|
||||
bool collectStopOperations(
|
||||
const StopAllTicket& ticket,
|
||||
std::vector<DeferredStopOperation>& operations,
|
||||
std::string* error = nullptr) const;
|
||||
|
||||
// Legacy synchronous wrapper which serially executes the operations above.
|
||||
bool requestStop(
|
||||
const StopAllTicket& ticket,
|
||||
std::string* error = nullptr);
|
||||
|
||||
// Waits for in-flight RPC/stream ownership captured by the round to be
|
||||
// released after requestStop().
|
||||
bool waitForStopped(
|
||||
const StopAllTicket& ticket,
|
||||
std::chrono::milliseconds timeout,
|
||||
std::string* error = nullptr);
|
||||
|
||||
// Convenience operation for callers that do not need split-phase stop.
|
||||
bool stopAndWait(
|
||||
const StopAllTicket& ticket,
|
||||
std::chrono::milliseconds timeout,
|
||||
std::string* error = nullptr);
|
||||
|
||||
// Admission resumes only when all registered controls confirmed their stop
|
||||
// and every invalidated RPC/stream has exited. Failure remains fail-closed.
|
||||
bool finishStopAll(
|
||||
const StopAllTicket& ticket,
|
||||
bool all_motors_stopped);
|
||||
|
||||
FinishStopAllResult finishStopAllDetailed(
|
||||
const StopAllTicket& ticket,
|
||||
bool all_motors_stopped);
|
||||
|
||||
// Wakes StopAll after a MotorControlState releases or changes ownership.
|
||||
void notifyStateChanged() noexcept;
|
||||
|
||||
// Test/process teardown hook. Runtime recovery must use another successful
|
||||
// StopAll round instead of bypassing fail-closed state.
|
||||
void clearForTesting() noexcept;
|
||||
|
||||
private:
|
||||
std::shared_ptr<Impl> impl_;
|
||||
};
|
||||
|
||||
MotorActivityCoordinator& globalMotorActivityCoordinator();
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
#endif // CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H
|
||||
@ -0,0 +1,336 @@
|
||||
#include "service/grpc/server/include/camera_operational_activity_registry.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <exception>
|
||||
|
||||
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
namespace {
|
||||
|
||||
template <typename Operation>
|
||||
CameraOperationalActivityRegistry::DispatchResult dispatchIfAdmitted(
|
||||
std::mutex& device_mutex,
|
||||
const CameraOperationalActivityRegistry::DispatchFence& dispatch_fence,
|
||||
Operation&& operation)
|
||||
{
|
||||
std::uint64_t admitted_generation = 0U;
|
||||
{
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
if (!admission.accepting()) {
|
||||
return CameraOperationalActivityRegistry::DispatchResult::
|
||||
RejectedByStopAll;
|
||||
}
|
||||
admitted_generation = admission.generation();
|
||||
}
|
||||
|
||||
std::lock_guard dispatch_lock(device_mutex);
|
||||
{
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
if (!admission.accepting() ||
|
||||
admission.generation() != admitted_generation) {
|
||||
return CameraOperationalActivityRegistry::DispatchResult::
|
||||
RejectedByStopAll;
|
||||
}
|
||||
}
|
||||
|
||||
if (dispatch_fence && !dispatch_fence()) {
|
||||
return CameraOperationalActivityRegistry::DispatchResult::
|
||||
RejectedByDispatchFence;
|
||||
}
|
||||
|
||||
return operation();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
std::shared_ptr<CameraOperationalActivityRegistry::DeviceState>
|
||||
CameraOperationalActivityRegistry::stateForDevice(
|
||||
const std::string& device_id,
|
||||
const bool create)
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
const auto existing = states_.find(device_id);
|
||||
if (existing != states_.end()) {
|
||||
return existing->second;
|
||||
}
|
||||
if (!create) {
|
||||
return {};
|
||||
}
|
||||
|
||||
auto state = std::make_shared<DeviceState>();
|
||||
states_.emplace(device_id, state);
|
||||
return state;
|
||||
}
|
||||
|
||||
CameraOperationalActivityRegistry::DispatchResult
|
||||
CameraOperationalActivityRegistry::start(
|
||||
const std::string& device_id,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
ActivityToken* token,
|
||||
DispatchFence dispatch_fence)
|
||||
{
|
||||
if (token) {
|
||||
*token = {};
|
||||
}
|
||||
if (device_id.empty() || !camera) {
|
||||
return DispatchResult::DeviceFailure;
|
||||
}
|
||||
|
||||
const auto state = stateForDevice(device_id, true);
|
||||
return dispatchIfAdmitted(state->mutex, dispatch_fence, [&] {
|
||||
const auto previous_camera = state->active_camera;
|
||||
const bool was_active = state->active.load(std::memory_order_acquire);
|
||||
if (was_active && previous_camera != camera) {
|
||||
// A device id has one operational owner at a time. Replacing an
|
||||
// active instance would make a token from either instance unable
|
||||
// to roll back without risking the other camera.
|
||||
return DispatchResult::DeviceFailure;
|
||||
}
|
||||
if (!previous_camera) {
|
||||
// Allocate tracking before device I/O so a successful start always
|
||||
// has a StopAll-visible owner.
|
||||
state->active_camera = camera;
|
||||
}
|
||||
|
||||
bool started = false;
|
||||
try {
|
||||
started = camera->startOperationalActivity();
|
||||
} catch (...) {
|
||||
state->active_camera = previous_camera;
|
||||
state->active.store(was_active, std::memory_order_release);
|
||||
throw;
|
||||
}
|
||||
if (!started) {
|
||||
state->active_camera = previous_camera;
|
||||
state->active.store(was_active, std::memory_order_release);
|
||||
return DispatchResult::DeviceFailure;
|
||||
}
|
||||
|
||||
state->active_camera = camera;
|
||||
state->active.store(true, std::memory_order_release);
|
||||
++state->activity_generation;
|
||||
if (state->activity_generation == 0U) {
|
||||
++state->activity_generation;
|
||||
}
|
||||
if (token) {
|
||||
token->device_id = device_id;
|
||||
token->activity_generation = state->activity_generation;
|
||||
token->owns_start = !was_active;
|
||||
}
|
||||
return DispatchResult::Success;
|
||||
});
|
||||
}
|
||||
|
||||
bool CameraOperationalActivityRegistry::stopIfCurrent(
|
||||
const ActivityToken& token)
|
||||
{
|
||||
if (!token.valid() || !token.owns_start) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const auto state = stateForDevice(token.device_id, false);
|
||||
if (!state) {
|
||||
return true;
|
||||
}
|
||||
|
||||
std::lock_guard dispatch_lock(state->mutex);
|
||||
if (!state->active.load(std::memory_order_acquire) ||
|
||||
state->activity_generation != token.activity_generation ||
|
||||
!state->active_camera) {
|
||||
return true;
|
||||
}
|
||||
|
||||
bool stopped = false;
|
||||
try {
|
||||
stopped = state->active_camera->stopOperationalActivity();
|
||||
} catch (...) {
|
||||
stopped = false;
|
||||
}
|
||||
if (!stopped) {
|
||||
return false;
|
||||
}
|
||||
|
||||
state->last_stopped_camera = state->active_camera;
|
||||
state->active_camera.reset();
|
||||
state->active.store(false, std::memory_order_release);
|
||||
return true;
|
||||
}
|
||||
|
||||
CameraOperationalActivityRegistry::DispatchResult
|
||||
CameraOperationalActivityRegistry::stopLifecycle(
|
||||
const std::string& device_id,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
DispatchFence dispatch_fence)
|
||||
{
|
||||
if (device_id.empty() || !camera) {
|
||||
return DispatchResult::DeviceFailure;
|
||||
}
|
||||
|
||||
const auto state = stateForDevice(device_id, true);
|
||||
return dispatchIfAdmitted(state->mutex, dispatch_fence, [&] {
|
||||
if (!camera->stop()) {
|
||||
return DispatchResult::DeviceFailure;
|
||||
}
|
||||
state->active_camera.reset();
|
||||
state->active.store(false, std::memory_order_release);
|
||||
return DispatchResult::Success;
|
||||
});
|
||||
}
|
||||
|
||||
void CameraOperationalActivityRegistry::markCameraStopped(
|
||||
const std::string& device_id)
|
||||
{
|
||||
const auto state = stateForDevice(device_id, false);
|
||||
if (!state) {
|
||||
return;
|
||||
}
|
||||
std::lock_guard lock(state->mutex);
|
||||
state->active_camera.reset();
|
||||
state->active.store(false, std::memory_order_release);
|
||||
}
|
||||
|
||||
bool CameraOperationalActivityRegistry::stopActivitiesForDevice(
|
||||
const std::string& device_id,
|
||||
std::vector<std::string>* failures)
|
||||
{
|
||||
return stopActivitiesForDevice(device_id, {}, failures);
|
||||
}
|
||||
|
||||
bool CameraOperationalActivityRegistry::stopActivitiesForDevice(
|
||||
const std::string& device_id,
|
||||
const std::shared_ptr<device::AbstractCamera>& fallback_camera,
|
||||
std::vector<std::string>* failures)
|
||||
{
|
||||
const auto state = stateForDevice(
|
||||
device_id, static_cast<bool>(fallback_camera));
|
||||
if (!state) {
|
||||
return true;
|
||||
}
|
||||
|
||||
std::lock_guard dispatch_lock(state->mutex);
|
||||
const auto active_camera = state->active_camera;
|
||||
const auto camera = active_camera ? active_camera : fallback_camera;
|
||||
if (!camera) {
|
||||
return true;
|
||||
}
|
||||
|
||||
std::uint64_t stop_generation = 0U;
|
||||
{
|
||||
const auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
stop_generation = admission.generation();
|
||||
}
|
||||
if (!active_camera &&
|
||||
state->last_stopped_generation == stop_generation &&
|
||||
state->last_stopped_camera.lock() == camera) {
|
||||
return true;
|
||||
}
|
||||
|
||||
bool stopped = false;
|
||||
std::string detail;
|
||||
try {
|
||||
stopped = camera->stopOperationalActivity();
|
||||
if (!stopped) {
|
||||
detail = "operational camera stop was not confirmed";
|
||||
}
|
||||
} catch (const std::exception& error) {
|
||||
detail = std::string("operational camera stop threw: ") + error.what();
|
||||
} catch (...) {
|
||||
detail = "operational camera stop threw an unknown exception";
|
||||
}
|
||||
|
||||
if (stopped) {
|
||||
if (active_camera) {
|
||||
state->active_camera.reset();
|
||||
}
|
||||
state->last_stopped_camera = camera;
|
||||
state->last_stopped_generation = stop_generation;
|
||||
state->active.store(false, std::memory_order_release);
|
||||
return true;
|
||||
}
|
||||
if (failures) {
|
||||
failures->push_back(device_id + ": " + detail);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool CameraOperationalActivityRegistry::stopAllActivities(
|
||||
std::vector<std::string>* failures)
|
||||
{
|
||||
std::vector<std::string> device_ids;
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
device_ids.reserve(states_.size());
|
||||
for (const auto& [device_id, state] : states_) {
|
||||
(void)state;
|
||||
device_ids.push_back(device_id);
|
||||
}
|
||||
}
|
||||
|
||||
bool all_stopped = true;
|
||||
for (const auto& device_id : device_ids) {
|
||||
if (!stopActivitiesForDevice(device_id, failures)) {
|
||||
all_stopped = false;
|
||||
}
|
||||
}
|
||||
return all_stopped;
|
||||
}
|
||||
|
||||
std::size_t CameraOperationalActivityRegistry::activeCameraCount() const
|
||||
{
|
||||
return activeDeviceIds().size();
|
||||
}
|
||||
|
||||
std::vector<std::string>
|
||||
CameraOperationalActivityRegistry::trackedDeviceIds() const
|
||||
{
|
||||
std::vector<std::string> device_ids;
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
device_ids.reserve(states_.size());
|
||||
for (const auto& [device_id, state] : states_) {
|
||||
(void)state;
|
||||
device_ids.push_back(device_id);
|
||||
}
|
||||
}
|
||||
std::sort(device_ids.begin(), device_ids.end());
|
||||
return device_ids;
|
||||
}
|
||||
|
||||
std::vector<std::string>
|
||||
CameraOperationalActivityRegistry::activeDeviceIds() const
|
||||
{
|
||||
std::vector<std::pair<std::string, std::shared_ptr<DeviceState>>> states;
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
states.reserve(states_.size());
|
||||
for (const auto& entry : states_) {
|
||||
states.push_back(entry);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::string> device_ids;
|
||||
device_ids.reserve(states.size());
|
||||
for (const auto& [device_id, state] : states) {
|
||||
if (state->active.load(std::memory_order_acquire)) {
|
||||
device_ids.push_back(device_id);
|
||||
}
|
||||
}
|
||||
std::sort(device_ids.begin(), device_ids.end());
|
||||
return device_ids;
|
||||
}
|
||||
|
||||
void CameraOperationalActivityRegistry::clearForTesting()
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
states_.clear();
|
||||
}
|
||||
|
||||
CameraOperationalActivityRegistry&
|
||||
globalCameraOperationalActivityRegistry()
|
||||
{
|
||||
static CameraOperationalActivityRegistry registry;
|
||||
return registry;
|
||||
}
|
||||
|
||||
} // namespace cmvr::service
|
||||
233
cmvr-es/service/grpc/server/src/camera_ptz_activity_registry.cpp
Normal file
233
cmvr-es/service/grpc/server/src/camera_ptz_activity_registry.cpp
Normal file
@ -0,0 +1,233 @@
|
||||
#include "service/grpc/server/include/camera_ptz_activity_registry.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <exception>
|
||||
#include <utility>
|
||||
|
||||
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
std::shared_ptr<CameraPtzActivityRegistry::DeviceState>
|
||||
CameraPtzActivityRegistry::stateForDevice(
|
||||
const std::string& device_id,
|
||||
const bool create)
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
const auto existing = states_.find(device_id);
|
||||
if (existing != states_.end()) {
|
||||
return existing->second;
|
||||
}
|
||||
if (!create) {
|
||||
return {};
|
||||
}
|
||||
|
||||
auto state = std::make_shared<DeviceState>();
|
||||
states_.emplace(device_id, state);
|
||||
return state;
|
||||
}
|
||||
|
||||
CameraPtzActivityRegistry::DispatchResult
|
||||
CameraPtzActivityRegistry::control(
|
||||
const std::string& device_id,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
const device::PtzCommand command,
|
||||
const bool stop,
|
||||
const int speed,
|
||||
DispatchFence dispatch_fence)
|
||||
{
|
||||
if (device_id.empty() || !camera) {
|
||||
return DispatchResult::DeviceFailure;
|
||||
}
|
||||
|
||||
// START checks admission on both sides of the per-device dispatch queue. A
|
||||
// STOP is a safety-lane operation and remains available while StopAll is
|
||||
// latched; its Coordinator dispatch fence still runs under the same queue.
|
||||
std::uint64_t admitted_generation = 0U;
|
||||
if (!stop) {
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
if (!admission.accepting()) {
|
||||
return DispatchResult::RejectedByStopAll;
|
||||
}
|
||||
admitted_generation = admission.generation();
|
||||
}
|
||||
|
||||
const auto state = stateForDevice(device_id, true);
|
||||
std::lock_guard dispatch_lock(state->mutex);
|
||||
if (!stop) {
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
if (!admission.accepting() ||
|
||||
admission.generation() != admitted_generation) {
|
||||
return DispatchResult::RejectedByStopAll;
|
||||
}
|
||||
}
|
||||
|
||||
// The service-level safety transaction performs its final epoch,
|
||||
// generation, and hardware-state validation here. Keeping the fence under
|
||||
// the per-device lock prevents a request that waited in this queue from
|
||||
// dispatching with a stale admission permit.
|
||||
if (dispatch_fence && !dispatch_fence()) {
|
||||
return DispatchResult::RejectedByDispatchFence;
|
||||
}
|
||||
|
||||
if (!camera->controlPtz(command, stop, speed)) {
|
||||
return DispatchResult::DeviceFailure;
|
||||
}
|
||||
|
||||
if (!stop) {
|
||||
state->commands[command] = {camera, speed};
|
||||
} else {
|
||||
state->commands.erase(command);
|
||||
}
|
||||
state->active_command_count.store(
|
||||
state->commands.size(), std::memory_order_release);
|
||||
return DispatchResult::Success;
|
||||
}
|
||||
|
||||
void CameraPtzActivityRegistry::markCameraStopped(
|
||||
const std::string& device_id)
|
||||
{
|
||||
const auto state = stateForDevice(device_id, false);
|
||||
if (!state) {
|
||||
return;
|
||||
}
|
||||
std::lock_guard lock(state->mutex);
|
||||
state->commands.clear();
|
||||
state->active_command_count.store(0U, std::memory_order_release);
|
||||
}
|
||||
|
||||
bool CameraPtzActivityRegistry::stopActivitiesForDevice(
|
||||
const std::string& device_id,
|
||||
std::vector<std::string>* failures)
|
||||
{
|
||||
const auto state = stateForDevice(device_id, false);
|
||||
if (!state) {
|
||||
return true;
|
||||
}
|
||||
|
||||
std::lock_guard dispatch_lock(state->mutex);
|
||||
bool all_stopped = true;
|
||||
for (auto command_it = state->commands.begin();
|
||||
command_it != state->commands.end();) {
|
||||
bool stopped = false;
|
||||
std::string detail;
|
||||
try {
|
||||
const auto& activity = command_it->second;
|
||||
stopped = activity.camera && activity.camera->controlPtz(
|
||||
command_it->first, true, activity.speed);
|
||||
if (!stopped) {
|
||||
detail = "PTZ stop was rejected by the camera";
|
||||
}
|
||||
} catch (const std::exception& error) {
|
||||
detail = std::string("PTZ stop threw: ") + error.what();
|
||||
} catch (...) {
|
||||
detail = "PTZ stop threw an unknown exception";
|
||||
}
|
||||
|
||||
if (stopped) {
|
||||
command_it = state->commands.erase(command_it);
|
||||
continue;
|
||||
}
|
||||
|
||||
all_stopped = false;
|
||||
if (failures) {
|
||||
failures->push_back(device_id + ": " + detail);
|
||||
}
|
||||
++command_it;
|
||||
}
|
||||
state->active_command_count.store(
|
||||
state->commands.size(), std::memory_order_release);
|
||||
return all_stopped;
|
||||
}
|
||||
|
||||
bool CameraPtzActivityRegistry::stopAllActivities(
|
||||
std::vector<std::string>* failures)
|
||||
{
|
||||
std::vector<std::string> device_ids;
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
device_ids.reserve(states_.size());
|
||||
for (const auto& [device_id, state] : states_) {
|
||||
(void)state;
|
||||
device_ids.push_back(device_id);
|
||||
}
|
||||
}
|
||||
|
||||
bool all_stopped = true;
|
||||
for (const auto& device_id : device_ids) {
|
||||
if (!stopActivitiesForDevice(device_id, failures)) {
|
||||
all_stopped = false;
|
||||
}
|
||||
}
|
||||
return all_stopped;
|
||||
}
|
||||
|
||||
std::size_t CameraPtzActivityRegistry::activeCommandCount() const
|
||||
{
|
||||
std::vector<std::shared_ptr<DeviceState>> states;
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
states.reserve(states_.size());
|
||||
for (const auto& [device_id, state] : states_) {
|
||||
(void)device_id;
|
||||
states.push_back(state);
|
||||
}
|
||||
}
|
||||
|
||||
std::size_t count = 0U;
|
||||
for (const auto& state : states) {
|
||||
count += state->active_command_count.load(std::memory_order_acquire);
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
std::vector<std::string> CameraPtzActivityRegistry::trackedDeviceIds() const
|
||||
{
|
||||
std::vector<std::string> device_ids;
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
device_ids.reserve(states_.size());
|
||||
for (const auto& [device_id, state] : states_) {
|
||||
(void)state;
|
||||
device_ids.push_back(device_id);
|
||||
}
|
||||
}
|
||||
std::sort(device_ids.begin(), device_ids.end());
|
||||
return device_ids;
|
||||
}
|
||||
|
||||
std::vector<std::string> CameraPtzActivityRegistry::activeDeviceIds() const
|
||||
{
|
||||
std::vector<std::pair<std::string, std::shared_ptr<DeviceState>>> states;
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
states.reserve(states_.size());
|
||||
for (const auto& entry : states_) {
|
||||
states.push_back(entry);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::string> device_ids;
|
||||
device_ids.reserve(states.size());
|
||||
for (const auto& [device_id, state] : states) {
|
||||
if (state->active_command_count.load(std::memory_order_acquire) != 0U) {
|
||||
device_ids.push_back(device_id);
|
||||
}
|
||||
}
|
||||
std::sort(device_ids.begin(), device_ids.end());
|
||||
return device_ids;
|
||||
}
|
||||
|
||||
void CameraPtzActivityRegistry::clearForTesting()
|
||||
{
|
||||
std::lock_guard lock(states_mutex_);
|
||||
states_.clear();
|
||||
}
|
||||
|
||||
CameraPtzActivityRegistry& globalCameraPtzActivityRegistry()
|
||||
{
|
||||
static CameraPtzActivityRegistry registry;
|
||||
return registry;
|
||||
}
|
||||
|
||||
} // namespace cmvr::service
|
||||
1363
cmvr-es/service/grpc/server/src/grpc_agv_service.cpp
Normal file
1363
cmvr-es/service/grpc/server/src/grpc_agv_service.cpp
Normal file
File diff suppressed because it is too large
Load Diff
918
cmvr-es/service/grpc/server/src/grpc_arm_service.cpp
Normal file
918
cmvr-es/service/grpc/server/src/grpc_arm_service.cpp
Normal file
@ -0,0 +1,918 @@
|
||||
#include "service/grpc/server/include/grpc_arm_service.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <utility>
|
||||
|
||||
#include <google/protobuf/util/time_util.h>
|
||||
|
||||
#include "common/base/logging/logger.h"
|
||||
#include "manager/control_authority_manager/include/control_authority_manager.h"
|
||||
#include "service/grpc/server/include/grpc_command_transaction.h"
|
||||
#include "service/grpc/server/include/grpc_security.h"
|
||||
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
|
||||
|
||||
using google::protobuf::util::TimeUtil;
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
namespace {
|
||||
|
||||
void fillFeedback(api::CommandHeader_Feedback* feedback,
|
||||
const bool success,
|
||||
const std::string& message = {})
|
||||
{
|
||||
feedback->set_success(success);
|
||||
feedback->set_error_message(message);
|
||||
*feedback->mutable_timestamp() = TimeUtil::GetCurrentTime();
|
||||
}
|
||||
|
||||
grpc::Status resultToStatus(const device::Result& result)
|
||||
{
|
||||
if (result.ok()) {
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
return grpc::Status(grpc::StatusCode::INTERNAL, result.message);
|
||||
}
|
||||
|
||||
void logRpcSuccess(const char* rpc_name, const std::string& device_id)
|
||||
{
|
||||
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (" << rpc_name
|
||||
<< "): success, id=" << device_id;
|
||||
}
|
||||
|
||||
device::FrameType toFrameType(const api::ArmFrameType frame)
|
||||
{
|
||||
switch (frame) {
|
||||
case api::ARM_FRAME_TOOL:
|
||||
return device::FrameType::Tool;
|
||||
case api::ARM_FRAME_WORLD:
|
||||
return device::FrameType::World;
|
||||
case api::ARM_FRAME_USER:
|
||||
return device::FrameType::User;
|
||||
case api::ARM_FRAME_BASE:
|
||||
default:
|
||||
return device::FrameType::Base;
|
||||
}
|
||||
}
|
||||
|
||||
device::JointPositionCommand toJointPositionCommand(const api::JointPositionCommand& src)
|
||||
{
|
||||
device::JointPositionCommand dst;
|
||||
dst.position.assign(src.position().begin(), src.position().end());
|
||||
return dst;
|
||||
}
|
||||
|
||||
device::JointVelocityCommand toJointVelocityCommand(const api::JointVelocityCommand& src)
|
||||
{
|
||||
device::JointVelocityCommand dst;
|
||||
dst.velocity.assign(src.velocity().begin(), src.velocity().end());
|
||||
return dst;
|
||||
}
|
||||
|
||||
device::MotionOptions toMotionOptions(
|
||||
const api::MotionOptions& src,
|
||||
std::function<bool()> cancellation_requested = {})
|
||||
{
|
||||
device::MotionOptions dst;
|
||||
dst.velocity = src.velocity();
|
||||
dst.acceleration = src.acceleration();
|
||||
dst.blend_radius = src.blend_radius();
|
||||
dst.jerk = src.jerk() > 0.0 ? src.jerk() : 5.0;
|
||||
dst.joint_velocity_limits.assign(src.joint_velocity_limits().begin(),
|
||||
src.joint_velocity_limits().end());
|
||||
dst.asynchronous = src.asynchronous();
|
||||
dst.cancellation_requested = std::move(cancellation_requested);
|
||||
return dst;
|
||||
}
|
||||
|
||||
device::CartesianPose toCartesianPose(const api::CartesianPose& src)
|
||||
{
|
||||
return {src.x(), src.y(), src.z(), src.rx(), src.ry(), src.rz()};
|
||||
}
|
||||
|
||||
api::CartesianPose toApiCartesianPose(const device::CartesianPose& src)
|
||||
{
|
||||
api::CartesianPose dst;
|
||||
dst.set_x(src.x);
|
||||
dst.set_y(src.y);
|
||||
dst.set_z(src.z);
|
||||
dst.set_rx(src.rx);
|
||||
dst.set_ry(src.ry);
|
||||
dst.set_rz(src.rz);
|
||||
return dst;
|
||||
}
|
||||
|
||||
device::CartesianVelocity toCartesianVelocity(const api::CartesianVelocity& src)
|
||||
{
|
||||
return {src.vx(), src.vy(), src.vz(), src.wx(), src.wy(), src.wz()};
|
||||
}
|
||||
|
||||
template <typename Response>
|
||||
grpc::Status setResponseResult(Response* response, const device::Result& result)
|
||||
{
|
||||
fillFeedback(response->mutable_header(), result.ok(), result.ok() ? "" : result.message);
|
||||
return resultToStatus(result);
|
||||
}
|
||||
|
||||
grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std::string& device_id)
|
||||
{
|
||||
const std::string message = "RobotArm device not found: " + device_id;
|
||||
fillFeedback(response, false, message);
|
||||
return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
|
||||
}
|
||||
|
||||
template <typename Response>
|
||||
grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
|
||||
{
|
||||
const std::string message = "RobotArm device not found: " + device_id;
|
||||
fillFeedback(response->mutable_header(), false, message);
|
||||
return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
|
||||
}
|
||||
|
||||
grpc::Status setStopAllRejected(
|
||||
api::CommandHeader_Feedback* response,
|
||||
const std::string& device_id)
|
||||
{
|
||||
const std::string message =
|
||||
"RobotArm control is temporarily paused by StopAll: " + device_id;
|
||||
fillFeedback(response, false, message);
|
||||
return grpc::Status(grpc::StatusCode::UNAVAILABLE, message);
|
||||
}
|
||||
|
||||
template <typename Response>
|
||||
grpc::Status setStopAllRejected(
|
||||
Response* response,
|
||||
const std::string& device_id)
|
||||
{
|
||||
return setStopAllRejected(response->mutable_header(), device_id);
|
||||
}
|
||||
|
||||
grpc::Status setControlLeaseConflict(
|
||||
api::CommandHeader_Feedback* response,
|
||||
const std::string& device_id,
|
||||
const std::string& detail)
|
||||
{
|
||||
std::string message =
|
||||
"RobotArm control is leased by another active control operation: " +
|
||||
device_id;
|
||||
if (!detail.empty()) {
|
||||
CMVR_LOG(WARNING) << "[gRPCArmServiceImpl] control lease conflict, id="
|
||||
<< device_id << ", detail=" << detail;
|
||||
}
|
||||
fillFeedback(response, false, message);
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::FAILED_PRECONDITION, message);
|
||||
}
|
||||
|
||||
template <typename Response>
|
||||
grpc::Status setControlLeaseConflict(
|
||||
Response* response,
|
||||
const std::string& device_id,
|
||||
const std::string& detail)
|
||||
{
|
||||
return setControlLeaseConflict(
|
||||
response->mutable_header(), device_id, detail);
|
||||
}
|
||||
|
||||
grpc::Status setControlCancelled(
|
||||
api::CommandHeader_Feedback* response,
|
||||
const std::string& device_id,
|
||||
const std::string& reason)
|
||||
{
|
||||
std::string message = "RobotArm control was cancelled: " + device_id;
|
||||
if (!reason.empty()) {
|
||||
message += ", " + reason;
|
||||
}
|
||||
fillFeedback(response, false, message);
|
||||
return grpc::Status(grpc::StatusCode::CANCELLED, message);
|
||||
}
|
||||
|
||||
class ScopedUnaryControlLease final {
|
||||
public:
|
||||
ScopedUnaryControlLease(
|
||||
const std::string& device_id,
|
||||
const char* operation,
|
||||
const bool preemptive = false)
|
||||
: manager_(control::ControlAuthorityManager::instance())
|
||||
{
|
||||
static std::atomic<std::uint64_t> sequence{0};
|
||||
const std::string owner =
|
||||
std::string("grpc-arm-unary:") + operation + ":" +
|
||||
std::to_string(
|
||||
sequence.fetch_add(
|
||||
1U, std::memory_order_relaxed) +
|
||||
1U);
|
||||
const auto ttl = std::chrono::duration_cast<
|
||||
control::ControlAuthorityManager::Duration>(
|
||||
std::chrono::hours(24));
|
||||
control::ControlAcquireResult acquired;
|
||||
if (preemptive) {
|
||||
acquired = manager_.preemptAcquire(device_id, owner, ttl);
|
||||
} else {
|
||||
auto admission =
|
||||
globalStopAllAdmissionGate().lockAdmission();
|
||||
if (!admission.accepting()) {
|
||||
rejected_by_stop_all_ = true;
|
||||
detail_ = "System StopAll admission is closed";
|
||||
return;
|
||||
}
|
||||
admission_generation_ = admission.generation();
|
||||
acquired = manager_.tryAcquire(device_id, owner, ttl);
|
||||
}
|
||||
acquired_ = acquired.acquired;
|
||||
token_ = std::move(acquired.token);
|
||||
detail_ = std::move(acquired.detail);
|
||||
release_on_destroy_ = !preemptive;
|
||||
}
|
||||
|
||||
~ScopedUnaryControlLease()
|
||||
{
|
||||
if (release_on_destroy_) {
|
||||
manager_.release(token_);
|
||||
} else if (acquired_) {
|
||||
(void)manager_.retireSafetyHolder(token_);
|
||||
}
|
||||
}
|
||||
|
||||
bool acquired() const noexcept { return acquired_; }
|
||||
bool rejectedByStopAll() const noexcept
|
||||
{
|
||||
return rejected_by_stop_all_;
|
||||
}
|
||||
const std::string& detail() const noexcept { return detail_; }
|
||||
void confirmSafeToRelease() noexcept { release_on_destroy_ = true; }
|
||||
|
||||
bool waitForPreemptedRelease(
|
||||
const control::ControlAuthorityManager::Duration timeout)
|
||||
{
|
||||
return manager_.waitForPreemptedRelease(token_, timeout);
|
||||
}
|
||||
|
||||
bool admissionCurrent() const
|
||||
{
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
return admission.accepting() &&
|
||||
admission.generation() == admission_generation_;
|
||||
}
|
||||
|
||||
bool current() const
|
||||
{
|
||||
return manager_.validate(token_) && admissionCurrent();
|
||||
}
|
||||
|
||||
std::function<bool()> cancellationRequested(
|
||||
grpc::ServerContext* context) const
|
||||
{
|
||||
const auto token = token_;
|
||||
const auto admission_generation = admission_generation_;
|
||||
return [context, token, admission_generation]() {
|
||||
try {
|
||||
if ((context && context->IsCancelled()) ||
|
||||
!control::ControlAuthorityManager::instance()
|
||||
.validate(token)) {
|
||||
return true;
|
||||
}
|
||||
auto admission =
|
||||
globalStopAllAdmissionGate().lockAdmission();
|
||||
return !admission.accepting() ||
|
||||
admission.generation() != admission_generation;
|
||||
} catch (...) {
|
||||
return true;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
control::ControlDispatchGuard tryBeginDispatch()
|
||||
{
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
if (!admission.accepting() ||
|
||||
admission.generation() != admission_generation_) {
|
||||
return {};
|
||||
}
|
||||
return manager_.tryBeginDispatch(token_);
|
||||
}
|
||||
|
||||
private:
|
||||
control::ControlAuthorityManager& manager_;
|
||||
control::ControlLeaseToken token_;
|
||||
std::string detail_;
|
||||
bool acquired_{false};
|
||||
bool release_on_destroy_{true};
|
||||
bool rejected_by_stop_all_{false};
|
||||
std::uint64_t admission_generation_{0U};
|
||||
};
|
||||
|
||||
template <typename Operation>
|
||||
device::Result executeConfirmedArmStop(
|
||||
ScopedUnaryControlLease& control_barrier,
|
||||
const char* operation_name,
|
||||
Operation&& operation)
|
||||
{
|
||||
const auto initial_stop = operation();
|
||||
if (!initial_stop.ok()) {
|
||||
return initial_stop;
|
||||
}
|
||||
|
||||
constexpr auto handler_release_timeout = std::chrono::seconds(15);
|
||||
if (!control_barrier.waitForPreemptedRelease(
|
||||
std::chrono::duration_cast<
|
||||
control::ControlAuthorityManager::Duration>(
|
||||
handler_release_timeout))) {
|
||||
return device::Result::failure(
|
||||
device::ArmErrorCode::Timeout,
|
||||
std::string(operation_name) +
|
||||
" timed out waiting for the preempted control handler to exit");
|
||||
}
|
||||
|
||||
const auto final_stop = operation();
|
||||
if (final_stop.ok()) {
|
||||
control_barrier.confirmSafeToRelease();
|
||||
}
|
||||
return final_stop;
|
||||
}
|
||||
|
||||
template <typename Response>
|
||||
grpc::Status setControlAdmissionFailure(
|
||||
Response* response,
|
||||
const std::string& device_id,
|
||||
const ScopedUnaryControlLease& lease)
|
||||
{
|
||||
return lease.rejectedByStopAll()
|
||||
? setStopAllRejected(response, device_id)
|
||||
: setControlLeaseConflict(response, device_id, lease.detail());
|
||||
}
|
||||
|
||||
template <typename Response>
|
||||
grpc::Status setControlDispatchFailure(
|
||||
Response* response,
|
||||
const std::string& device_id,
|
||||
const ScopedUnaryControlLease& lease,
|
||||
const char* operation)
|
||||
{
|
||||
if (!lease.admissionCurrent()) {
|
||||
return setStopAllRejected(response, device_id);
|
||||
}
|
||||
return setControlLeaseConflict(
|
||||
response,
|
||||
device_id,
|
||||
std::string("control lease was preempted before ") + operation +
|
||||
" dispatch");
|
||||
}
|
||||
|
||||
grpc::Status setAlreadyEnabled(
|
||||
api::CommandHeader_Feedback* response,
|
||||
const std::string& device_id,
|
||||
const char* rpc_name)
|
||||
{
|
||||
constexpr const char* kAlreadyEnabledMessage =
|
||||
"RobotArm is already enabled";
|
||||
CMVR_LOG(WARNING) << "[gRPCArmServiceImpl] (" << rpc_name
|
||||
<< "): ignored because the device is already enabled, id="
|
||||
<< device_id;
|
||||
fillFeedback(response, true, kAlreadyEnabledMessage);
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
gRPCArmServiceImpl::gRPCArmServiceImpl()
|
||||
: gRPCArmServiceImpl(makeDefaultGrpcSecurityGateway())
|
||||
{
|
||||
}
|
||||
|
||||
gRPCArmServiceImpl::gRPCArmServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway)
|
||||
: dmgr_(device::DeviceManager::getInstance()),
|
||||
security_gateway_(security_gateway
|
||||
? std::move(security_gateway)
|
||||
: makeDefaultGrpcSecurityGateway())
|
||||
{
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/torqueOff", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
ScopedUnaryControlLease control_barrier(
|
||||
device_id, "torqueOff", true);
|
||||
if (!control_barrier.acquired()) {
|
||||
return setControlLeaseConflict(
|
||||
response, device_id, control_barrier.detail());
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
const auto result = executeConfirmedArmStop(
|
||||
control_barrier,
|
||||
"torqueOff",
|
||||
[&arm]() { return arm->torqueOff(); });
|
||||
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
|
||||
if (result.ok()) {
|
||||
logRpcSuccess("torqueOff", device_id);
|
||||
}
|
||||
return resultToStatus(result);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/torqueOn", request, response,
|
||||
[this, context, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
const auto state = arm->getRobotState();
|
||||
if (state.powered_on) {
|
||||
return setAlreadyEnabled(response, device_id, "torqueOn");
|
||||
}
|
||||
ScopedUnaryControlLease control_lease(
|
||||
device_id, "torqueOn");
|
||||
if (!control_lease.acquired()) {
|
||||
return setControlAdmissionFailure(
|
||||
response, device_id, control_lease);
|
||||
}
|
||||
auto dispatch = control_lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease, "torqueOn");
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
const auto cancellation_requested =
|
||||
control_lease.cancellationRequested(context);
|
||||
const auto result = arm->torqueOn(cancellation_requested);
|
||||
const bool control_current = control_lease.current();
|
||||
const bool cancellation_result =
|
||||
result.ok() ||
|
||||
result.code == device::ArmErrorCode::CommandRejected;
|
||||
if (cancellation_result &&
|
||||
!control_lease.admissionCurrent()) {
|
||||
return setStopAllRejected(response, device_id);
|
||||
}
|
||||
const bool rpc_cancelled = context && context->IsCancelled();
|
||||
if (cancellation_result &&
|
||||
(rpc_cancelled || !control_current)) {
|
||||
return setControlCancelled(
|
||||
response,
|
||||
device_id,
|
||||
rpc_cancelled
|
||||
? "the RPC was cancelled"
|
||||
: "control ownership was revoked");
|
||||
}
|
||||
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
|
||||
if (result.ok()) {
|
||||
logRpcSuccess("torqueOn", device_id);
|
||||
}
|
||||
return resultToStatus(result);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context,
|
||||
const api::MoveJ_Request* request,
|
||||
api::MoveJ_Response* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/moveJ", request, response,
|
||||
[this, context, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
ScopedUnaryControlLease control_lease(
|
||||
device_id, "moveJ");
|
||||
if (!control_lease.acquired()) {
|
||||
return setControlAdmissionFailure(
|
||||
response, device_id, control_lease);
|
||||
}
|
||||
auto dispatch = control_lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease, "moveJ");
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
auto options = toMotionOptions(
|
||||
request->options(),
|
||||
control_lease.cancellationRequested(context));
|
||||
const auto result = arm->moveJ(
|
||||
toJointPositionCommand(request->target()), options);
|
||||
if (result.ok()) {
|
||||
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id
|
||||
<< ", positions=" << request->target().position_size();
|
||||
}
|
||||
return setResponseResult(response, result);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context,
|
||||
const api::MoveL_Request* request,
|
||||
api::MoveL_Response* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/moveL", request, response,
|
||||
[this, context, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
ScopedUnaryControlLease control_lease(
|
||||
device_id, "moveL");
|
||||
if (!control_lease.acquired()) {
|
||||
return setControlAdmissionFailure(
|
||||
response, device_id, control_lease);
|
||||
}
|
||||
auto dispatch = control_lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease, "moveL");
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
auto options = toMotionOptions(
|
||||
request->options(),
|
||||
control_lease.cancellationRequested(context));
|
||||
const auto result = arm->moveL(
|
||||
toCartesianPose(request->target()),
|
||||
options,
|
||||
toFrameType(request->frame()));
|
||||
if (result.ok()) {
|
||||
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id
|
||||
<< ", frame=" << request->frame();
|
||||
}
|
||||
return setResponseResult(response, result);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext* context,
|
||||
const api::SpeedJ_Request* request,
|
||||
api::SpeedJ_Response* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/speedJ", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
ScopedUnaryControlLease control_lease(
|
||||
device_id, "speedJ");
|
||||
if (!control_lease.acquired()) {
|
||||
return setControlAdmissionFailure(
|
||||
response, device_id, control_lease);
|
||||
}
|
||||
auto dispatch = control_lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease, "speedJ");
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()),
|
||||
request->acceleration(),
|
||||
request->duration());
|
||||
if (result.ok()) {
|
||||
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedJ): success, id=" << device_id
|
||||
<< ", velocities=" << request->velocity().velocity_size()
|
||||
<< ", acceleration=" << request->acceleration()
|
||||
<< ", duration=" << request->duration();
|
||||
}
|
||||
return setResponseResult(response, result);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext* context,
|
||||
const api::SpeedL_Request* request,
|
||||
api::SpeedL_Response* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/speedL", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
ScopedUnaryControlLease control_lease(
|
||||
device_id, "speedL");
|
||||
if (!control_lease.acquired()) {
|
||||
return setControlAdmissionFailure(
|
||||
response, device_id, control_lease);
|
||||
}
|
||||
auto dispatch = control_lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease, "speedL");
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
|
||||
request->acceleration(),
|
||||
request->duration(),
|
||||
toFrameType(request->frame()));
|
||||
if (result.ok()) {
|
||||
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedL): success, id=" << device_id
|
||||
<< ", acceleration=" << request->acceleration()
|
||||
<< ", duration=" << request->duration()
|
||||
<< ", frame=" << request->frame();
|
||||
}
|
||||
return setResponseResult(response, result);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext* context,
|
||||
const api::ServoJ_Request* request,
|
||||
api::ServoJ_Response* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/servoJ", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
ScopedUnaryControlLease control_lease(
|
||||
device_id, "servoJ");
|
||||
if (!control_lease.acquired()) {
|
||||
return setControlAdmissionFailure(
|
||||
response, device_id, control_lease);
|
||||
}
|
||||
auto dispatch = control_lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease, "servoJ");
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
const auto result = arm->servoJ(toJointPositionCommand(request->target()));
|
||||
if (result.ok()) {
|
||||
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id
|
||||
<< ", positions=" << request->target().position_size();
|
||||
}
|
||||
return setResponseResult(response, result);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/stopMotion", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
ScopedUnaryControlLease control_barrier(
|
||||
device_id, "stopMotion", true);
|
||||
if (!control_barrier.acquired()) {
|
||||
return setControlLeaseConflict(
|
||||
response, device_id, control_barrier.detail());
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
const auto result = executeConfirmedArmStop(
|
||||
control_barrier,
|
||||
"stopMotion",
|
||||
[&arm]() { return arm->stopMotion(); });
|
||||
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
|
||||
if (result.ok()) {
|
||||
logRpcSuccess("stopMotion", device_id);
|
||||
}
|
||||
return resultToStatus(result);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext* context,
|
||||
const api::JointRequest* request,
|
||||
api::JointResponse* response)
|
||||
{
|
||||
CMVR_GRPC_REQUIRE_REGISTERED_CALL(
|
||||
security_gateway_, context, "/cmvr.api.ArmService/getJointState");
|
||||
try {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
const auto model = arm->getRobotModel();
|
||||
const auto state = arm->getJointState();
|
||||
auto* msg = response->mutable_state();
|
||||
for (const auto& name : model.joint_names) {
|
||||
msg->add_name(name);
|
||||
}
|
||||
for (double v : state.position) msg->add_position(v);
|
||||
for (double v : state.velocity) msg->add_velocity(v);
|
||||
for (double v : state.effort) msg->add_effort(v);
|
||||
fillFeedback(response->mutable_header(), true);
|
||||
// CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getJointState): success, id=" << device_id
|
||||
// << ", joints=" << msg->name_size()
|
||||
// << ", positions=" << msg->position_size();
|
||||
return grpc::Status::OK;
|
||||
} catch (const std::exception& e) {
|
||||
fillFeedback(response->mutable_header(), false, e.what());
|
||||
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||
}
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext* context,
|
||||
const api::GetPose_Request* request,
|
||||
api::GetPose_Response* response)
|
||||
{
|
||||
CMVR_GRPC_REQUIRE_REGISTERED_CALL(
|
||||
security_gateway_, context, "/cmvr.api.ArmService/getPose");
|
||||
try {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
const auto pose = request->base_link().empty() || request->ee_link().empty()
|
||||
? arm->fk(true)
|
||||
: arm->fk(request->base_link(), request->ee_link());
|
||||
*response->mutable_pose() = toApiCartesianPose(pose);
|
||||
fillFeedback(response->mutable_header(), true);
|
||||
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getPose): success, id=" << device_id
|
||||
<< ", pose=(" << pose.x << ", " << pose.y << ", " << pose.z
|
||||
<< ", " << pose.rx << ", " << pose.ry << ", " << pose.rz << ")";
|
||||
return grpc::Status::OK;
|
||||
} catch (const std::exception& e) {
|
||||
fillFeedback(response->mutable_header(), false, e.what());
|
||||
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||
}
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext* context,
|
||||
const api::CalibrateZeroQ_Request* request,
|
||||
api::CalibrateZeroQ_Response* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/calibrateZeroQ", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
ScopedUnaryControlLease control_lease(
|
||||
device_id, "calibrateZeroQ");
|
||||
if (!control_lease.acquired()) {
|
||||
return setControlAdmissionFailure(
|
||||
response, device_id, control_lease);
|
||||
}
|
||||
auto dispatch = control_lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease, "calibrateZeroQ");
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
const auto result = arm->calibrateZeroQ(request->joint_name());
|
||||
if (result.ok()) {
|
||||
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id
|
||||
<< ", joint=" << request->joint_name();
|
||||
}
|
||||
return setResponseResult(response, result);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::getPoseMatrix(grpc::ServerContext* context,
|
||||
const api::GetPoseMatrix_Request*,
|
||||
api::GetPoseMatrix_Response* response)
|
||||
{
|
||||
CMVR_GRPC_REQUIRE_REGISTERED_CALL(
|
||||
security_gateway_, context, "/cmvr.api.ArmService/getPoseMatrix");
|
||||
fillFeedback(response->mutable_header(), false, "getPoseMatrix is not implemented");
|
||||
return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "getPoseMatrix is not implemented");
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::computeForwardKinematics(grpc::ServerContext* context,
|
||||
const api::ComputeForwardKinematics_Request*,
|
||||
api::ComputeForwardKinematics_Response* response)
|
||||
{
|
||||
CMVR_GRPC_REQUIRE_REGISTERED_CALL(
|
||||
security_gateway_, context,
|
||||
"/cmvr.api.ArmService/computeForwardKinematics");
|
||||
fillFeedback(response->mutable_header(), false, "computeForwardKinematics is not implemented");
|
||||
return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented");
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::ExecuteJsonCommand(
|
||||
grpc::ServerContext* context,
|
||||
const api::JsonDeviceCommand_Request* request,
|
||||
api::JsonDeviceCommand_Feedback* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/ExecuteJsonCommand", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
fillFeedback(
|
||||
response->mutable_header(),
|
||||
false,
|
||||
"Device not found: " + device_id);
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
ScopedUnaryControlLease control_lease(
|
||||
device_id, "ExecuteJsonCommand");
|
||||
if (!control_lease.acquired()) {
|
||||
return setControlAdmissionFailure(
|
||||
response, device_id, control_lease);
|
||||
}
|
||||
auto dispatch = control_lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease,
|
||||
"ExecuteJsonCommand");
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
|
||||
std::string response_json;
|
||||
const bool success = arm->executeJsonCommand(
|
||||
request->request_json(), response_json);
|
||||
fillFeedback(
|
||||
response->mutable_header(),
|
||||
success,
|
||||
success ? "" : response_json);
|
||||
response->set_response_json(response_json);
|
||||
if (success) {
|
||||
logRpcSuccess("ExecuteJsonCommand", device_id);
|
||||
}
|
||||
return grpc::Status::OK;
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
|
||||
const cmvr::api::CommandHeader_Request *request,
|
||||
cmvr::api::CommandHeader_Feedback *response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.ArmService/clearFault", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const std::string device_id = request->device_id();
|
||||
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||
if (!arm) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
ScopedUnaryControlLease control_lease(
|
||||
device_id, "clearFault");
|
||||
if (!control_lease.acquired()) {
|
||||
return setControlAdmissionFailure(
|
||||
response, device_id, control_lease);
|
||||
}
|
||||
auto dispatch = control_lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease, "clearFault");
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
const auto result = arm->clearFault();
|
||||
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
|
||||
return resultToStatus(result);
|
||||
});
|
||||
}
|
||||
} // namespace cmvr::service
|
||||
1227
cmvr-es/service/grpc/server/src/grpc_arm_teleop_service.cpp
Normal file
1227
cmvr-es/service/grpc/server/src/grpc_arm_teleop_service.cpp
Normal file
File diff suppressed because it is too large
Load Diff
1087
cmvr-es/service/grpc/server/src/grpc_camera_service.cpp
Normal file
1087
cmvr-es/service/grpc/server/src/grpc_camera_service.cpp
Normal file
File diff suppressed because it is too large
Load Diff
1271
cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp
Normal file
1271
cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp
Normal file
File diff suppressed because it is too large
Load Diff
764
cmvr-es/service/grpc/server/src/grpc_dexhand_service.cpp
Normal file
764
cmvr-es/service/grpc/server/src/grpc_dexhand_service.cpp
Normal file
@ -0,0 +1,764 @@
|
||||
#include "common/base/logging/logger.h"
|
||||
//
|
||||
// Created by linbo on 2025/7/3.
|
||||
//
|
||||
|
||||
#include "../include/grpc_dexhand_service.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <cmath>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
|
||||
#include "manager/control_authority_manager/include/control_authority_manager.h"
|
||||
#include "service/grpc/server/include/grpc_command_transaction.h"
|
||||
#include "service/grpc/server/include/media_activity_coordinator.h"
|
||||
#include "service/grpc/server/include/grpc_security.h"
|
||||
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace cmvr::service;
|
||||
using namespace cmvr::device;
|
||||
|
||||
#define DEXHAND_MAX_POSITION 2000
|
||||
#define DEXHAND_MAX_ANGLE 1000
|
||||
#define DEXHAND_MAX_FORCE 3000
|
||||
#define DEXHAND_MAX_SPEED 1000
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr int kDexHandDofCount = 6;
|
||||
|
||||
cmvr::api::SensorData::FingerType toProtoFingerType(const AbstractDexHand::FingerType finger) {
|
||||
switch (finger) {
|
||||
case AbstractDexHand::FingerType::PINKY:
|
||||
return cmvr::api::SensorData::PINKY;
|
||||
case AbstractDexHand::FingerType::RING:
|
||||
return cmvr::api::SensorData::RING;
|
||||
case AbstractDexHand::FingerType::MIDDLE:
|
||||
return cmvr::api::SensorData::MIDDLE_FINGER;
|
||||
case AbstractDexHand::FingerType::INDEX:
|
||||
return cmvr::api::SensorData::INDEX;
|
||||
case AbstractDexHand::FingerType::THUMB:
|
||||
return cmvr::api::SensorData::THUMB;
|
||||
case AbstractDexHand::FingerType::PALM:
|
||||
return cmvr::api::SensorData::PALM;
|
||||
}
|
||||
return cmvr::api::SensorData::PINKY;
|
||||
}
|
||||
|
||||
cmvr::api::SensorData::PartType toProtoPartType(const AbstractDexHand::TactileRegion region) {
|
||||
switch (region) {
|
||||
case AbstractDexHand::TactileRegion::TIP:
|
||||
return cmvr::api::SensorData::TIP;
|
||||
case AbstractDexHand::TactileRegion::FINGER:
|
||||
return cmvr::api::SensorData::FINGER;
|
||||
case AbstractDexHand::TactileRegion::PAD:
|
||||
return cmvr::api::SensorData::PAD;
|
||||
case AbstractDexHand::TactileRegion::THUMB_MIDDLE:
|
||||
return cmvr::api::SensorData::THUMB_MIDDLE;
|
||||
case AbstractDexHand::TactileRegion::PALM_PAD:
|
||||
return cmvr::api::SensorData::PALM_PAD;
|
||||
}
|
||||
return cmvr::api::SensorData::TIP;
|
||||
}
|
||||
|
||||
void fillSensorData(const AbstractDexHand::TactileRegionData& tactile_data,
|
||||
cmvr::api::SensorData* sensor_data) {
|
||||
sensor_data->set_rows(tactile_data.view.rows);
|
||||
sensor_data->set_cols(tactile_data.view.cols);
|
||||
sensor_data->set_finger_type(toProtoFingerType(tactile_data.finger));
|
||||
sensor_data->set_part_type(toProtoPartType(tactile_data.region));
|
||||
sensor_data->set_sensor_name(tactile_data.name == nullptr ? "" : tactile_data.name);
|
||||
|
||||
for (int row = 0; row < tactile_data.view.rows; ++row) {
|
||||
auto* row_data = sensor_data->add_data();
|
||||
const auto* values = tactile_data.view.rowData(row);
|
||||
for (int col = 0; col < tactile_data.view.cols; ++col) {
|
||||
// Keep the existing scalar wire format by exposing the normal-force projection.
|
||||
row_data->add_values(static_cast<int32_t>(values[col].fz));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename ResponseT>
|
||||
void appendSensorData(const std::vector<AbstractDexHand::TactileRegionData>& tactile_regions,
|
||||
ResponseT* response) {
|
||||
for (const auto& tactile_region : tactile_regions) {
|
||||
if (!tactile_region.valid()) {
|
||||
continue;
|
||||
}
|
||||
fillSensorData(tactile_region, response->add_sensor());
|
||||
}
|
||||
}
|
||||
|
||||
template <typename FreedomCollection>
|
||||
bool applyFreedomValues(const FreedomCollection& freedoms,
|
||||
const int scale,
|
||||
std::vector<int>& targets,
|
||||
std::string* error_message) {
|
||||
for (const auto& freedom : freedoms) {
|
||||
if (freedom.id() < 0 || freedom.id() >= static_cast<int>(targets.size())) {
|
||||
if (error_message) {
|
||||
*error_message = "Invalid dexhand DOF id: " + std::to_string(freedom.id());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (!std::isfinite(freedom.value())) {
|
||||
if (error_message) {
|
||||
*error_message = "Invalid dexhand command value: not finite.";
|
||||
}
|
||||
return false;
|
||||
}
|
||||
targets[static_cast<size_t>(freedom.id())] = static_cast<int>(freedom.value() * scale);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename ResponseT>
|
||||
grpc::Status failResponse(ResponseT* response, const std::string& message) {
|
||||
response->mutable_header()->set_success(false);
|
||||
response->mutable_header()->set_error_message(message);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
class ScopedDexHandControlLease final {
|
||||
public:
|
||||
ScopedDexHandControlLease(const std::string& device_id,
|
||||
const char* operation)
|
||||
: manager_(cmvr::control::ControlAuthorityManager::instance()) {
|
||||
static std::atomic<std::uint64_t> sequence{0U};
|
||||
const std::string owner =
|
||||
std::string("grpc-dexhand-unary:") + operation + ":" +
|
||||
std::to_string(
|
||||
sequence.fetch_add(1U, std::memory_order_relaxed) + 1U);
|
||||
const auto ttl = std::chrono::duration_cast<
|
||||
cmvr::control::ControlAuthorityManager::Duration>(
|
||||
std::chrono::hours(24));
|
||||
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
if (!admission.accepting()) {
|
||||
rejected_by_stop_all_ = true;
|
||||
detail_ = "System StopAll admission is closed";
|
||||
return;
|
||||
}
|
||||
admission_generation_ = admission.generation();
|
||||
|
||||
auto acquired = manager_.tryAcquire(device_id, owner, ttl);
|
||||
acquired_ = acquired.acquired;
|
||||
token_ = std::move(acquired.token);
|
||||
detail_ = std::move(acquired.detail);
|
||||
}
|
||||
|
||||
~ScopedDexHandControlLease() {
|
||||
manager_.release(token_);
|
||||
}
|
||||
|
||||
bool acquired() const noexcept { return acquired_; }
|
||||
bool rejectedByStopAll() const noexcept {
|
||||
return rejected_by_stop_all_;
|
||||
}
|
||||
const std::string& detail() const noexcept { return detail_; }
|
||||
|
||||
bool admissionCurrent() const {
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
return admission.accepting() &&
|
||||
admission.generation() == admission_generation_;
|
||||
}
|
||||
|
||||
cmvr::control::ControlDispatchGuard tryBeginDispatch() {
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
if (!admission.accepting() ||
|
||||
admission.generation() != admission_generation_) {
|
||||
return {};
|
||||
}
|
||||
return manager_.tryBeginDispatch(token_);
|
||||
}
|
||||
|
||||
private:
|
||||
cmvr::control::ControlAuthorityManager& manager_;
|
||||
cmvr::control::ControlLeaseToken token_;
|
||||
std::string detail_;
|
||||
bool acquired_{false};
|
||||
bool rejected_by_stop_all_{false};
|
||||
std::uint64_t admission_generation_{0U};
|
||||
};
|
||||
|
||||
template <typename ResponseT>
|
||||
grpc::Status failControlAdmission(
|
||||
ResponseT* response,
|
||||
const std::string& device_id,
|
||||
const ScopedDexHandControlLease& lease) {
|
||||
if (lease.rejectedByStopAll()) {
|
||||
return failResponse(
|
||||
response,
|
||||
"DexHand control is temporarily paused by StopAll: " + device_id);
|
||||
}
|
||||
return failResponse(
|
||||
response,
|
||||
"DexHand control is leased by another active operation: " +
|
||||
device_id +
|
||||
(lease.detail().empty() ? "" : " (" + lease.detail() + ")"));
|
||||
}
|
||||
|
||||
template <typename ResponseT>
|
||||
grpc::Status failControlDispatch(
|
||||
ResponseT* response,
|
||||
const std::string& device_id,
|
||||
const ScopedDexHandControlLease& lease) {
|
||||
if (!lease.admissionCurrent()) {
|
||||
return failResponse(
|
||||
response,
|
||||
"DexHand control was preempted by StopAll: " + device_id);
|
||||
}
|
||||
return failResponse(
|
||||
response,
|
||||
"DexHand control lease was preempted before device dispatch: " +
|
||||
device_id);
|
||||
}
|
||||
|
||||
template <typename ResponseT, typename Operation>
|
||||
bool dispatchDexHandCommand(
|
||||
ResponseT* response,
|
||||
const std::string& device_id,
|
||||
const std::shared_ptr<AbstractDexHand>& dev,
|
||||
ScopedDexHandControlLease& lease,
|
||||
GrpcCommandTransaction& command,
|
||||
Operation&& operation) {
|
||||
auto dispatch = lease.tryBeginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
(void)failControlDispatch(response, device_id, lease);
|
||||
return false;
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return false;
|
||||
}
|
||||
if (!dev->resumeOperationalActivity()) {
|
||||
(void)failResponse(
|
||||
response,
|
||||
"DexHand operational activity could not be resumed: " +
|
||||
device_id);
|
||||
return false;
|
||||
}
|
||||
std::forward<Operation>(operation)();
|
||||
return true;
|
||||
}
|
||||
|
||||
std::vector<int> readCurrentAngles(const std::shared_ptr<AbstractDexHand>& dev) {
|
||||
DexHandState state{};
|
||||
dev->getState(state);
|
||||
|
||||
std::vector<int> current_angles(static_cast<size_t>(kDexHandDofCount), 0);
|
||||
for (int i = 0; i < kDexHandDofCount; ++i) {
|
||||
current_angles[static_cast<size_t>(i)] = state.hands[i].angle;
|
||||
}
|
||||
return current_angles;
|
||||
}
|
||||
|
||||
bool respondUnsupportedForRh56(const std::shared_ptr<AbstractDexHand>& dev,
|
||||
const char* rpc_name,
|
||||
const char* hint,
|
||||
cmvr::api::CommandHeader_Feedback* header) {
|
||||
if (std::dynamic_pointer_cast<RH56DFTPDexhand>(dev) == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
header->set_success(false);
|
||||
header->set_error_message(std::string(rpc_name) + " is not supported by RH56DFTPDexhand. " + hint);
|
||||
setCurrentTimestamp(header->mutable_timestamp());
|
||||
return true;
|
||||
}
|
||||
|
||||
std::vector<AbstractDexHand::TactileRegionKey> buildRh56AllTactileRegions() {
|
||||
using FingerType = AbstractDexHand::FingerType;
|
||||
using TactileRegion = AbstractDexHand::TactileRegion;
|
||||
|
||||
return {
|
||||
{FingerType::PINKY, TactileRegion::TIP},
|
||||
{FingerType::PINKY, TactileRegion::FINGER},
|
||||
{FingerType::PINKY, TactileRegion::PAD},
|
||||
{FingerType::RING, TactileRegion::TIP},
|
||||
{FingerType::RING, TactileRegion::FINGER},
|
||||
{FingerType::RING, TactileRegion::PAD},
|
||||
{FingerType::MIDDLE, TactileRegion::TIP},
|
||||
{FingerType::MIDDLE, TactileRegion::FINGER},
|
||||
{FingerType::MIDDLE, TactileRegion::PAD},
|
||||
{FingerType::INDEX, TactileRegion::TIP},
|
||||
{FingerType::INDEX, TactileRegion::FINGER},
|
||||
{FingerType::INDEX, TactileRegion::PAD},
|
||||
{FingerType::THUMB, TactileRegion::TIP},
|
||||
{FingerType::THUMB, TactileRegion::FINGER},
|
||||
{FingerType::THUMB, TactileRegion::THUMB_MIDDLE},
|
||||
{FingerType::THUMB, TactileRegion::PAD},
|
||||
{FingerType::PALM, TactileRegion::PALM_PAD}
|
||||
};
|
||||
}
|
||||
|
||||
void maybeConfigureRh56FullTactilePolling(const std::shared_ptr<AbstractDexHand>& dev) {
|
||||
auto rh56 = std::dynamic_pointer_cast<RH56DFTPDexhand>(dev);
|
||||
if (!rh56) {
|
||||
return;
|
||||
}
|
||||
rh56->setTactilePollingRegions(buildRh56AllTactileRegions());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
gRPCDexHandServiceImpl::gRPCDexHandServiceImpl()
|
||||
: gRPCDexHandServiceImpl(makeDefaultGrpcSecurityGateway()) {}
|
||||
|
||||
gRPCDexHandServiceImpl::gRPCDexHandServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway)
|
||||
: dmgr_(DeviceManager::getInstance()),
|
||||
security_gateway_(security_gateway
|
||||
? std::move(security_gateway)
|
||||
: makeDefaultGrpcSecurityGateway()) {}
|
||||
|
||||
grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
|
||||
const api::GetDexHandStateCommand_Request* request, api::GetDexHandStateCommand_Feedback* response) {
|
||||
CMVR_GRPC_REQUIRE_REGISTERED_CALL(
|
||||
security_gateway_, context, "/cmvr.api.DexHandService/GetStatus");
|
||||
try {
|
||||
string dev_id = request->header().device_id();
|
||||
CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetStatus): id=" << dev_id;
|
||||
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
|
||||
if (!dev) {
|
||||
return failResponse(response, "DexHand device not found: " + dev_id);
|
||||
}
|
||||
DexHandState state{};
|
||||
dev->getState(state);
|
||||
response->mutable_state()->set_is_initialized(state.is_initialized);
|
||||
for (int i = 0; i < kDexHandDofCount; i++) {
|
||||
auto hand = response->mutable_state()->add_hands();
|
||||
hand->set_dof_id(i);
|
||||
hand->set_angle(state.hands[i].angle);
|
||||
hand->set_current(state.hands[i].current);
|
||||
hand->set_force(state.hands[i].force);
|
||||
hand->set_position(state.hands[i].position);
|
||||
hand->set_speed(state.hands[i].speed);
|
||||
hand->set_temperature(state.hands[i].temperature);
|
||||
hand->set_error(state.hands[i].error);
|
||||
for (auto& errormessage : state.hands[i].error_message) {
|
||||
hand->add_error_message(errormessage);
|
||||
}
|
||||
}
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetStatus): success, id=" << dev_id
|
||||
<< ", initialized=" << state.is_initialized
|
||||
<< ", dof=" << response->state().hands_size();
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
catch (const std::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 gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
|
||||
, const cmvr::api::SetDexHandPositionsCommand_Request* request
|
||||
, cmvr::api::SetDexHandPositionsCommand_Feedback* response) {
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.DexHandService/SetDexHandPos", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
string dev_id = request->header().device_id();
|
||||
CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPos): id=" << dev_id;
|
||||
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
|
||||
if (!dev) {
|
||||
return failResponse(response, "DexHand device not found: " + dev_id);
|
||||
}
|
||||
ScopedDexHandControlLease control_lease(dev_id, "SetDexHandPos");
|
||||
if (!control_lease.acquired()) {
|
||||
return failControlAdmission(response, dev_id, control_lease);
|
||||
}
|
||||
if (respondUnsupportedForRh56(dev,
|
||||
"SetDexHandPos",
|
||||
"Use SetDexHandAngle for RH56 joint commands.",
|
||||
response->mutable_header())) {
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
|
||||
std::string error_message;
|
||||
if (!applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets, &error_message)) {
|
||||
return failResponse(response, error_message);
|
||||
}
|
||||
if (!dispatchDexHandCommand(
|
||||
response,
|
||||
dev_id,
|
||||
dev,
|
||||
control_lease,
|
||||
command,
|
||||
[&] { dev->setPositions(finger_joint_targets); })) {
|
||||
return command.dispatchStatus().ok()
|
||||
? grpc::Status::OK
|
||||
: command.dispatchStatus();
|
||||
}
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPos): success, id=" << dev_id
|
||||
<< ", values=" << request->values_size();
|
||||
return grpc::Status::OK;
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* context
|
||||
, const cmvr::api::SetDexHandAnglesCommand_Request* request
|
||||
, cmvr::api::SetDexHandAnglesCommand_Feedback* response) {
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.DexHandService/SetDexHandAngle", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
string dev_id = request->header().device_id();
|
||||
CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): id=" << dev_id;
|
||||
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
|
||||
if (!dev) {
|
||||
return failResponse(response, "DexHand device not found: " + dev_id);
|
||||
}
|
||||
ScopedDexHandControlLease control_lease(dev_id, "SetDexHandAngle");
|
||||
if (!control_lease.acquired()) {
|
||||
return failControlAdmission(response, dev_id, control_lease);
|
||||
}
|
||||
|
||||
if (const auto rh56 = std::dynamic_pointer_cast<RH56DFTPDexhand>(dev)) {
|
||||
std::vector<int> finger_joint_targets = readCurrentAngles(dev);
|
||||
std::string error_message;
|
||||
if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) {
|
||||
return failResponse(response, error_message);
|
||||
}
|
||||
if (!dispatchDexHandCommand(
|
||||
response,
|
||||
dev_id,
|
||||
dev,
|
||||
control_lease,
|
||||
command,
|
||||
[&] { rh56->setAngles(finger_joint_targets); })) {
|
||||
return command.dispatchStatus().ok()
|
||||
? grpc::Status::OK
|
||||
: command.dispatchStatus();
|
||||
}
|
||||
} else {
|
||||
std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
|
||||
std::string error_message;
|
||||
if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) {
|
||||
return failResponse(response, error_message);
|
||||
}
|
||||
if (!dispatchDexHandCommand(
|
||||
response,
|
||||
dev_id,
|
||||
dev,
|
||||
control_lease,
|
||||
command,
|
||||
[&] { dev->setAngles(finger_joint_targets); })) {
|
||||
return command.dispatchStatus().ok()
|
||||
? grpc::Status::OK
|
||||
: command.dispatchStatus();
|
||||
}
|
||||
}
|
||||
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): success, id=" << dev_id
|
||||
<< ", values=" << request->values_size();
|
||||
return grpc::Status::OK;
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* context
|
||||
, const cmvr::api::SetDexHandForceCommand_Request* request
|
||||
, cmvr::api::SetDexHandForceCommand_Feedback* response) {
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.DexHandService/SetDexHandForce", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
string dev_id = request->header().device_id();
|
||||
CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandForce): id=" << dev_id;
|
||||
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
|
||||
if (!dev) {
|
||||
return failResponse(response, "DexHand device not found: " + dev_id);
|
||||
}
|
||||
ScopedDexHandControlLease control_lease(dev_id, "SetDexHandForce");
|
||||
if (!control_lease.acquired()) {
|
||||
return failControlAdmission(response, dev_id, control_lease);
|
||||
}
|
||||
if (respondUnsupportedForRh56(dev,
|
||||
"SetDexHandForce",
|
||||
"RH56DFTPDexhand currently exposes angle and tactile APIs only.",
|
||||
response->mutable_header())) {
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
|
||||
std::string error_message;
|
||||
if (!applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets, &error_message)) {
|
||||
return failResponse(response, error_message);
|
||||
}
|
||||
if (!dispatchDexHandCommand(
|
||||
response,
|
||||
dev_id,
|
||||
dev,
|
||||
control_lease,
|
||||
command,
|
||||
[&] { dev->setForce(finger_joint_targets); })) {
|
||||
return command.dispatchStatus().ok()
|
||||
? grpc::Status::OK
|
||||
: command.dispatchStatus();
|
||||
}
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandForce): success, id=" << dev_id
|
||||
<< ", values=" << request->values_size();
|
||||
return grpc::Status::OK;
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* context
|
||||
, const cmvr::api::SetDexHandSpeedCommand_Request* request
|
||||
, cmvr::api::SetDexHandSpeedCommand_Feedback* response) {
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.DexHandService/SetDexHandSpeed", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
string dev_id = request->header().device_id();
|
||||
CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): id=" << dev_id;
|
||||
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
|
||||
if (!dev) {
|
||||
return failResponse(response, "DexHand device not found: " + dev_id);
|
||||
}
|
||||
ScopedDexHandControlLease control_lease(dev_id, "SetDexHandSpeed");
|
||||
if (!control_lease.acquired()) {
|
||||
return failControlAdmission(response, dev_id, control_lease);
|
||||
}
|
||||
if (respondUnsupportedForRh56(dev,
|
||||
"SetDexHandSpeed",
|
||||
"RH56DFTPDexhand currently exposes angle and tactile APIs only.",
|
||||
response->mutable_header())) {
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
|
||||
std::string error_message;
|
||||
if (!applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets, &error_message)) {
|
||||
return failResponse(response, error_message);
|
||||
}
|
||||
if (!dispatchDexHandCommand(
|
||||
response,
|
||||
dev_id,
|
||||
dev,
|
||||
control_lease,
|
||||
command,
|
||||
[&] { dev->setVelocities(finger_joint_targets); })) {
|
||||
return command.dispatchStatus().ok()
|
||||
? grpc::Status::OK
|
||||
: command.dispatchStatus();
|
||||
}
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): success, id=" << dev_id
|
||||
<< ", values=" << request->values_size();
|
||||
return grpc::Status::OK;
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* context
|
||||
, const cmvr::api::SetDexHandPresetActCommand_Request* request
|
||||
, cmvr::api::SetDexHandPresetActCommand_Feedback* response) {
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.DexHandService/SetDexHandPresetAct", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
string dev_id = request->header().device_id();
|
||||
CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): id=" << dev_id;
|
||||
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
|
||||
if (!dev) {
|
||||
return failResponse(response, "DexHand device not found: " + dev_id);
|
||||
}
|
||||
ScopedDexHandControlLease control_lease(
|
||||
dev_id, "SetDexHandPresetAct");
|
||||
if (!control_lease.acquired()) {
|
||||
return failControlAdmission(response, dev_id, control_lease);
|
||||
}
|
||||
if (respondUnsupportedForRh56(dev,
|
||||
"SetDexHandPresetAct",
|
||||
"RH56DFTPDexhand currently exposes angle and tactile APIs only.",
|
||||
response->mutable_header())) {
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
auto presetActId = request->presetactid();
|
||||
if (!dispatchDexHandCommand(
|
||||
response,
|
||||
dev_id,
|
||||
dev,
|
||||
control_lease,
|
||||
command,
|
||||
[&] { dev->setPresetAct(presetActId); })) {
|
||||
return command.dispatchStatus().ok()
|
||||
? grpc::Status::OK
|
||||
: command.dispatchStatus();
|
||||
}
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): success, id=" << dev_id
|
||||
<< ", preset_act_id=" << presetActId;
|
||||
return grpc::Status::OK;
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
|
||||
, const cmvr::api::GetSensorDataCommand_Request* request
|
||||
, cmvr::api::GetSensorDataCommand_Feedback* response) {
|
||||
CMVR_GRPC_REQUIRE_REGISTERED_CALL(
|
||||
security_gateway_, context,
|
||||
"/cmvr.api.DexHandService/GetSensorData");
|
||||
|
||||
auto media_session = globalMediaActivityCoordinator().beginSession();
|
||||
if (!media_session) {
|
||||
return failResponse(
|
||||
response,
|
||||
"DexHand sensor activity is temporarily paused by StopAll");
|
||||
}
|
||||
|
||||
try {
|
||||
string dev_id = request->header().device_id();
|
||||
CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id;
|
||||
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
|
||||
if (!dev) {
|
||||
return failResponse(response, "DexHand device not found: " + dev_id);
|
||||
}
|
||||
|
||||
bool resumed = false;
|
||||
std::vector<AbstractDexHand::TactileRegionData> sensor_data;
|
||||
if (!media_session.runIfCurrent([&] {
|
||||
resumed = dev->resumeOperationalActivity();
|
||||
if (!resumed) {
|
||||
return;
|
||||
}
|
||||
maybeConfigureRh56FullTactilePolling(dev);
|
||||
sensor_data = dev->getSensorData();
|
||||
})) {
|
||||
return failResponse(
|
||||
response,
|
||||
"DexHand sensor activity was preempted by StopAll: " +
|
||||
dev_id);
|
||||
}
|
||||
if (!resumed) {
|
||||
return failResponse(
|
||||
response,
|
||||
"DexHand sensor activity could not be resumed: " + dev_id);
|
||||
}
|
||||
appendSensorData(sensor_data, response);
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorData): success, id=" << dev_id
|
||||
<< ", sensors=" << response->sensor_size();
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
catch (const std::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 gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* context
|
||||
, grpc::ServerReaderWriter<cmvr::api::GetSensorDataStreamCommand_Feedback, cmvr::api::GetSensorDataStreamCommand_Request>* stream)
|
||||
{
|
||||
CMVR_GRPC_REQUIRE_REGISTERED_CALL(
|
||||
security_gateway_, context,
|
||||
"/cmvr.api.DexHandService/GetSensorDataStream");
|
||||
auto media_session = globalMediaActivityCoordinator().beginSession(
|
||||
[context] {
|
||||
if (context) {
|
||||
context->TryCancel();
|
||||
}
|
||||
});
|
||||
if (!media_session) {
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::UNAVAILABLE,
|
||||
"DexHand sensor stream is temporarily paused by StopAll");
|
||||
}
|
||||
|
||||
try {
|
||||
api::GetSensorDataStreamCommand_Request request;
|
||||
if (!stream->Read(&request)) {
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
string dev_id = request.header().device_id();
|
||||
CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): start,id=" << dev_id;
|
||||
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
|
||||
if (!dev) {
|
||||
api::GetSensorDataStreamCommand_Feedback response;
|
||||
response.mutable_header()->set_success(false);
|
||||
response.mutable_header()->set_error_message("DexHand device not found: " + dev_id);
|
||||
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||
stream->Write(response);
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
bool resumed = false;
|
||||
if (!media_session.runIfCurrent([&] {
|
||||
resumed = dev->resumeOperationalActivity();
|
||||
if (resumed) {
|
||||
maybeConfigureRh56FullTactilePolling(dev);
|
||||
}
|
||||
})) {
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::CANCELLED,
|
||||
"DexHand sensor stream was preempted by StopAll");
|
||||
}
|
||||
if (!resumed) {
|
||||
api::GetSensorDataStreamCommand_Feedback response;
|
||||
response.mutable_header()->set_success(false);
|
||||
response.mutable_header()->set_error_message(
|
||||
"DexHand sensor activity could not be resumed: " + dev_id);
|
||||
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||
stream->Write(response);
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): streaming success, id=" << dev_id;
|
||||
|
||||
while (!media_session.cancelled() &&
|
||||
!(context && context->IsCancelled()))
|
||||
{
|
||||
api::GetSensorDataStreamCommand_Feedback response;
|
||||
std::vector<AbstractDexHand::TactileRegionData> sensor_data;
|
||||
if (!media_session.runIfCurrent(
|
||||
[&] { sensor_data = dev->getSensorData(); })) {
|
||||
break;
|
||||
}
|
||||
appendSensorData(sensor_data, &response);
|
||||
response.mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||
|
||||
if (media_session.cancelled() ||
|
||||
(context && context->IsCancelled())) {
|
||||
break;
|
||||
}
|
||||
if (!stream->Write(response)) {
|
||||
CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id;
|
||||
break;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(33));
|
||||
}
|
||||
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): finished, id=" << dev_id;
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
catch (const std::exception& e) {
|
||||
CMVR_LOG(ERROR) << "[gRPCDexHandServiceImpl] (GetSensorDataStream) exception: " << e.what();
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,504 @@
|
||||
#include "service/grpc/server/include/grpc_error_logging_interceptor.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <sstream>
|
||||
#include <string_view>
|
||||
#include <utility>
|
||||
|
||||
#include <google/protobuf/descriptor.h>
|
||||
#include <google/protobuf/io/coded_stream.h>
|
||||
#include <google/protobuf/wire_format_lite.h>
|
||||
#include <grpcpp/server_context.h>
|
||||
#include <grpcpp/support/interceptor.h>
|
||||
#include <grpcpp/support/proto_buffer_reader.h>
|
||||
#include <grpcpp/support/status.h>
|
||||
|
||||
#include "cmvr/api/common.pb.h"
|
||||
#include "common/base/logging/logger.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
|
||||
namespace {
|
||||
|
||||
using Hook = grpc::experimental::InterceptionHookPoints;
|
||||
using CodedInputStream = google::protobuf::io::CodedInputStream;
|
||||
using WireFormatLite = google::protobuf::internal::WireFormatLite;
|
||||
|
||||
constexpr std::string_view kFeedbackType =
|
||||
"cmvr.api.CommandHeader.Feedback";
|
||||
constexpr std::size_t kMaxLogDetailBytes = 1024;
|
||||
constexpr int kMaxFeedbackBytes = 64 * 1024;
|
||||
|
||||
enum class ResponseShape {
|
||||
NONE,
|
||||
DIRECT_FEEDBACK,
|
||||
ENVELOPE,
|
||||
};
|
||||
|
||||
struct ApplicationFeedback {
|
||||
bool present{false};
|
||||
bool parsed{false};
|
||||
bool success{false};
|
||||
std::string error_message;
|
||||
};
|
||||
|
||||
ApplicationFeedback feedbackFromProto(
|
||||
const api::CommandHeader_Feedback& message)
|
||||
{
|
||||
return ApplicationFeedback{
|
||||
true, true, message.success(), message.error_message()};
|
||||
}
|
||||
|
||||
std::string boundedDetail(const std::string& detail)
|
||||
{
|
||||
if (detail.size() <= kMaxLogDetailBytes) {
|
||||
return detail;
|
||||
}
|
||||
|
||||
constexpr std::string_view suffix = "... [truncated]";
|
||||
std::size_t length = kMaxLogDetailBytes - suffix.size();
|
||||
while (length > 0 &&
|
||||
(static_cast<unsigned char>(detail[length]) & 0xc0U) == 0x80U) {
|
||||
--length;
|
||||
}
|
||||
return detail.substr(0, length) + std::string(suffix);
|
||||
}
|
||||
|
||||
const char* failureKindName(const GrpcFailureKind kind)
|
||||
{
|
||||
switch (kind) {
|
||||
case GrpcFailureKind::APPLICATION:
|
||||
return "application";
|
||||
case GrpcFailureKind::GRPC_STATUS:
|
||||
return "grpc_status";
|
||||
case GrpcFailureKind::MALFORMED_RESPONSE:
|
||||
return "malformed_response";
|
||||
}
|
||||
return "unknown";
|
||||
}
|
||||
|
||||
const char* statusCodeName(const grpc::StatusCode code)
|
||||
{
|
||||
switch (code) {
|
||||
case grpc::StatusCode::OK: return "OK";
|
||||
case grpc::StatusCode::CANCELLED: return "CANCELLED";
|
||||
case grpc::StatusCode::UNKNOWN: return "UNKNOWN";
|
||||
case grpc::StatusCode::INVALID_ARGUMENT: return "INVALID_ARGUMENT";
|
||||
case grpc::StatusCode::DEADLINE_EXCEEDED: return "DEADLINE_EXCEEDED";
|
||||
case grpc::StatusCode::NOT_FOUND: return "NOT_FOUND";
|
||||
case grpc::StatusCode::ALREADY_EXISTS: return "ALREADY_EXISTS";
|
||||
case grpc::StatusCode::PERMISSION_DENIED: return "PERMISSION_DENIED";
|
||||
case grpc::StatusCode::RESOURCE_EXHAUSTED: return "RESOURCE_EXHAUSTED";
|
||||
case grpc::StatusCode::FAILED_PRECONDITION: return "FAILED_PRECONDITION";
|
||||
case grpc::StatusCode::ABORTED: return "ABORTED";
|
||||
case grpc::StatusCode::OUT_OF_RANGE: return "OUT_OF_RANGE";
|
||||
case grpc::StatusCode::UNIMPLEMENTED: return "UNIMPLEMENTED";
|
||||
case grpc::StatusCode::INTERNAL: return "INTERNAL";
|
||||
case grpc::StatusCode::UNAVAILABLE: return "UNAVAILABLE";
|
||||
case grpc::StatusCode::DATA_LOSS: return "DATA_LOSS";
|
||||
case grpc::StatusCode::UNAUTHENTICATED: return "UNAUTHENTICATED";
|
||||
case grpc::StatusCode::DO_NOT_USE: break;
|
||||
}
|
||||
return "UNKNOWN_CODE";
|
||||
}
|
||||
|
||||
void logFailure(const GrpcFailureRecord& record)
|
||||
{
|
||||
std::ostringstream message;
|
||||
message << "[gRPC] request failed, method=" << record.method
|
||||
<< ", peer=" << (record.peer.empty() ? "unknown" : record.peer)
|
||||
<< ", kind=" << failureKindName(record.kind)
|
||||
<< ", code=" << static_cast<int>(record.status_code)
|
||||
<< '(' << statusCodeName(record.status_code) << ')'
|
||||
<< ", detail=" << boundedDetail(record.detail);
|
||||
switch (record.severity) {
|
||||
case GrpcFailureSeverity::INFO:
|
||||
CMVR_LOG(INFO) << message.str();
|
||||
break;
|
||||
case GrpcFailureSeverity::WARNING:
|
||||
CMVR_LOG(WARNING) << message.str();
|
||||
logging::Logger::instance().flush();
|
||||
break;
|
||||
case GrpcFailureSeverity::ERROR:
|
||||
CMVR_LOG(ERROR) << message.str();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
GrpcFailureSeverity severityForStatus(const grpc::StatusCode code)
|
||||
{
|
||||
switch (code) {
|
||||
case grpc::StatusCode::CANCELLED:
|
||||
case grpc::StatusCode::INVALID_ARGUMENT:
|
||||
case grpc::StatusCode::DEADLINE_EXCEEDED:
|
||||
case grpc::StatusCode::NOT_FOUND:
|
||||
case grpc::StatusCode::ALREADY_EXISTS:
|
||||
case grpc::StatusCode::PERMISSION_DENIED:
|
||||
case grpc::StatusCode::RESOURCE_EXHAUSTED:
|
||||
case grpc::StatusCode::FAILED_PRECONDITION:
|
||||
case grpc::StatusCode::ABORTED:
|
||||
case grpc::StatusCode::OUT_OF_RANGE:
|
||||
case grpc::StatusCode::UNIMPLEMENTED:
|
||||
case grpc::StatusCode::UNAVAILABLE:
|
||||
case grpc::StatusCode::UNAUTHENTICATED:
|
||||
return GrpcFailureSeverity::WARNING;
|
||||
case grpc::StatusCode::OK:
|
||||
case grpc::StatusCode::UNKNOWN:
|
||||
case grpc::StatusCode::INTERNAL:
|
||||
case grpc::StatusCode::DATA_LOSS:
|
||||
case grpc::StatusCode::DO_NOT_USE:
|
||||
return GrpcFailureSeverity::ERROR;
|
||||
}
|
||||
return GrpcFailureSeverity::ERROR;
|
||||
}
|
||||
|
||||
bool splitMethodName(const std::string_view full_method,
|
||||
std::string_view& service_name,
|
||||
std::string_view& method_name)
|
||||
{
|
||||
if (full_method.empty()) {
|
||||
return false;
|
||||
}
|
||||
const std::size_t service_begin = full_method.front() == '/' ? 1 : 0;
|
||||
const std::size_t separator = full_method.find('/', service_begin);
|
||||
if (separator == std::string_view::npos ||
|
||||
separator == service_begin || separator + 1 >= full_method.size()) {
|
||||
return false;
|
||||
}
|
||||
service_name = full_method.substr(service_begin, separator - service_begin);
|
||||
method_name = full_method.substr(separator + 1);
|
||||
return true;
|
||||
}
|
||||
|
||||
ResponseShape responseShapeForMethod(const std::string_view full_method)
|
||||
{
|
||||
std::string_view service_name;
|
||||
std::string_view method_name;
|
||||
if (!splitMethodName(full_method, service_name, method_name)) {
|
||||
return ResponseShape::NONE;
|
||||
}
|
||||
|
||||
const auto* pool = google::protobuf::DescriptorPool::generated_pool();
|
||||
const auto* service = pool->FindServiceByName(std::string(service_name));
|
||||
if (service == nullptr) {
|
||||
return ResponseShape::NONE;
|
||||
}
|
||||
const auto* method = service->FindMethodByName(std::string(method_name));
|
||||
if (method == nullptr || method->output_type() == nullptr) {
|
||||
return ResponseShape::NONE;
|
||||
}
|
||||
|
||||
const auto* output = method->output_type();
|
||||
if (output->full_name() == kFeedbackType) {
|
||||
return ResponseShape::DIRECT_FEEDBACK;
|
||||
}
|
||||
|
||||
const auto* header = output->FindFieldByNumber(1);
|
||||
if (header == nullptr || header->name() != "header" ||
|
||||
header->is_repeated() ||
|
||||
header->cpp_type() != google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE ||
|
||||
header->message_type() == nullptr ||
|
||||
header->message_type()->full_name() != kFeedbackType) {
|
||||
return ResponseShape::NONE;
|
||||
}
|
||||
return ResponseShape::ENVELOPE;
|
||||
}
|
||||
|
||||
bool mergeFeedback(CodedInputStream& input,
|
||||
api::CommandHeader_Feedback& feedback)
|
||||
{
|
||||
return feedback.MergePartialFromCodedStream(&input) &&
|
||||
input.ConsumedEntireMessage();
|
||||
}
|
||||
|
||||
bool mergeBoundedFeedback(CodedInputStream& input,
|
||||
api::CommandHeader_Feedback& feedback)
|
||||
{
|
||||
std::uint32_t length = 0;
|
||||
if (!input.ReadVarint32(&length) ||
|
||||
length > static_cast<std::uint32_t>(kMaxFeedbackBytes) ||
|
||||
length > static_cast<std::uint32_t>(std::numeric_limits<int>::max())) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string payload;
|
||||
if (!input.ReadString(&payload, static_cast<int>(length))) {
|
||||
return false;
|
||||
}
|
||||
return feedback.MergeFromString(payload);
|
||||
}
|
||||
|
||||
ApplicationFeedback parseDirectFeedback(grpc::ByteBuffer& buffer)
|
||||
{
|
||||
ApplicationFeedback feedback;
|
||||
grpc::ProtoBufferReader reader(&buffer);
|
||||
if (!reader.status().ok()) {
|
||||
return feedback;
|
||||
}
|
||||
CodedInputStream input(&reader);
|
||||
input.SetTotalBytesLimit(kMaxFeedbackBytes);
|
||||
api::CommandHeader_Feedback message;
|
||||
if (!mergeFeedback(input, message)) {
|
||||
feedback.present = true;
|
||||
return feedback;
|
||||
}
|
||||
return feedbackFromProto(message);
|
||||
}
|
||||
|
||||
ApplicationFeedback parseEnvelopeFeedback(grpc::ByteBuffer& buffer)
|
||||
{
|
||||
ApplicationFeedback feedback;
|
||||
grpc::ProtoBufferReader reader(&buffer);
|
||||
if (!reader.status().ok()) {
|
||||
return feedback;
|
||||
}
|
||||
CodedInputStream input(&reader);
|
||||
const std::uint32_t tag = input.ReadTag();
|
||||
if (tag == 0 || WireFormatLite::GetTagFieldNumber(tag) != 1) {
|
||||
feedback.parsed = true;
|
||||
return feedback;
|
||||
}
|
||||
|
||||
feedback.present = true;
|
||||
if (WireFormatLite::GetTagWireType(tag) !=
|
||||
WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
|
||||
return feedback;
|
||||
}
|
||||
|
||||
api::CommandHeader_Feedback message;
|
||||
if (!mergeBoundedFeedback(input, message)) {
|
||||
return feedback;
|
||||
}
|
||||
return feedbackFromProto(message);
|
||||
}
|
||||
|
||||
class GrpcErrorLoggingInterceptor final
|
||||
: public grpc::experimental::Interceptor {
|
||||
public:
|
||||
GrpcErrorLoggingInterceptor(std::string method,
|
||||
grpc::ServerContextBase* context,
|
||||
const ResponseShape response_shape,
|
||||
const bool server_streaming,
|
||||
const bool client_streaming,
|
||||
GrpcFailureSink sink)
|
||||
: method_(std::move(method)),
|
||||
context_(context),
|
||||
peer_(context == nullptr ? std::string{} : context->peer()),
|
||||
response_shape_(response_shape),
|
||||
server_streaming_(server_streaming),
|
||||
client_streaming_(client_streaming),
|
||||
sink_(std::move(sink))
|
||||
{
|
||||
}
|
||||
|
||||
void Intercept(grpc::experimental::InterceptorBatchMethods* methods) override
|
||||
{
|
||||
if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_CANCEL)) {
|
||||
// gRPC forbids delaying this hook. Only publish the signal here;
|
||||
// the final status hook performs any logging.
|
||||
server_cancel_requested_.store(true, std::memory_order_release);
|
||||
return;
|
||||
}
|
||||
if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_MESSAGE)) {
|
||||
inspectResponse(methods->GetSerializedSendMessage());
|
||||
}
|
||||
if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_STATUS)) {
|
||||
inspectStatus(methods->GetSendStatus());
|
||||
}
|
||||
methods->Proceed();
|
||||
}
|
||||
|
||||
private:
|
||||
void emit(GrpcFailureRecord record)
|
||||
{
|
||||
record.method = method_;
|
||||
record.peer = peer_;
|
||||
record.detail = boundedDetail(record.detail);
|
||||
if (sink_) {
|
||||
std::lock_guard lock(sink_mutex_);
|
||||
sink_(record);
|
||||
} else {
|
||||
logFailure(record);
|
||||
}
|
||||
}
|
||||
|
||||
void inspectResponse(grpc::ByteBuffer* buffer)
|
||||
{
|
||||
if (response_shape_ == ResponseShape::NONE ||
|
||||
buffer == nullptr || !buffer->Valid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
ApplicationFeedback feedback =
|
||||
response_shape_ == ResponseShape::DIRECT_FEEDBACK
|
||||
? parseDirectFeedback(*buffer)
|
||||
: parseEnvelopeFeedback(*buffer);
|
||||
std::optional<GrpcFailureRecord> failure;
|
||||
if (!feedback.present) {
|
||||
failure = GrpcFailureRecord{
|
||||
GrpcFailureKind::MALFORMED_RESPONSE,
|
||||
{}, {}, grpc::StatusCode::INTERNAL,
|
||||
GrpcFailureSeverity::ERROR,
|
||||
"response is missing CommandHeader.Feedback"};
|
||||
} else if (!feedback.parsed) {
|
||||
failure = GrpcFailureRecord{
|
||||
GrpcFailureKind::MALFORMED_RESPONSE,
|
||||
{}, {}, grpc::StatusCode::INTERNAL,
|
||||
GrpcFailureSeverity::ERROR,
|
||||
"response contains an invalid CommandHeader.Feedback"};
|
||||
} else if (!feedback.success) {
|
||||
failure = GrpcFailureRecord{
|
||||
GrpcFailureKind::APPLICATION,
|
||||
{}, {}, grpc::StatusCode::OK,
|
||||
GrpcFailureSeverity::ERROR,
|
||||
feedback.error_message.empty()
|
||||
? "operation failed without an error message"
|
||||
: feedback.error_message};
|
||||
}
|
||||
|
||||
if (!failure.has_value()) {
|
||||
return;
|
||||
}
|
||||
if (server_streaming_) {
|
||||
emit(std::move(*failure));
|
||||
return;
|
||||
}
|
||||
pending_failure_ = std::move(*failure);
|
||||
}
|
||||
|
||||
void inspectStatus(const grpc::Status& status)
|
||||
{
|
||||
const grpc::StatusCode status_code = status.error_code();
|
||||
std::string status_detail = status.error_message();
|
||||
if (!status.ok() && status_detail.empty()) {
|
||||
status_detail = "RPC completed with a non-OK status";
|
||||
}
|
||||
|
||||
if (!status.ok()) {
|
||||
// A non-OK unary/client-streaming status suppresses the response
|
||||
// body, so only the status is visible to the platform.
|
||||
pending_failure_.reset();
|
||||
if (status_code == grpc::StatusCode::CANCELLED) {
|
||||
emitCancellationOnce(status_code, std::move(status_detail));
|
||||
return;
|
||||
}
|
||||
emit({GrpcFailureKind::GRPC_STATUS,
|
||||
{}, {}, status_code,
|
||||
severityForStatus(status_code),
|
||||
status_detail});
|
||||
return;
|
||||
}
|
||||
|
||||
const bool deadline_expired =
|
||||
context_ != nullptr &&
|
||||
context_->deadline() <= std::chrono::system_clock::now();
|
||||
// IsCancelled() waits for the client close on the synchronous API.
|
||||
// Unary requests have already sent that close, but querying it here
|
||||
// could block an early client-streaming/bidi response indefinitely.
|
||||
if (deadline_expired ||
|
||||
(context_ != nullptr && !client_streaming_ &&
|
||||
context_->IsCancelled())) {
|
||||
pending_failure_.reset();
|
||||
const grpc::StatusCode code = deadline_expired
|
||||
? grpc::StatusCode::DEADLINE_EXCEEDED
|
||||
: grpc::StatusCode::CANCELLED;
|
||||
std::string detail;
|
||||
if (deadline_expired) {
|
||||
detail = "RPC deadline expired before completion";
|
||||
} else if (server_cancel_requested_.load(
|
||||
std::memory_order_acquire)) {
|
||||
detail =
|
||||
"RPC cancellation was requested by the edge server before completion";
|
||||
} else {
|
||||
detail =
|
||||
"RPC was cancelled by the client or transport before completion";
|
||||
}
|
||||
emitCancellationOnce(code, std::move(detail));
|
||||
return;
|
||||
}
|
||||
|
||||
if (server_cancel_requested_.load(std::memory_order_acquire)) {
|
||||
pending_failure_.reset();
|
||||
emitCancellationOnce(
|
||||
grpc::StatusCode::CANCELLED,
|
||||
"RPC cancellation was requested by the edge server before completion");
|
||||
return;
|
||||
}
|
||||
|
||||
if (pending_failure_.has_value()) {
|
||||
emit(std::move(*pending_failure_));
|
||||
pending_failure_.reset();
|
||||
}
|
||||
}
|
||||
|
||||
void emitCancellationOnce(const grpc::StatusCode code, std::string detail)
|
||||
{
|
||||
if (cancellation_recorded_.exchange(true, std::memory_order_acq_rel)) {
|
||||
return;
|
||||
}
|
||||
emit({GrpcFailureKind::GRPC_STATUS,
|
||||
{}, {}, code, severityForStatus(code), std::move(detail)});
|
||||
}
|
||||
|
||||
std::string method_;
|
||||
grpc::ServerContextBase* context_{nullptr};
|
||||
std::string peer_;
|
||||
ResponseShape response_shape_{ResponseShape::NONE};
|
||||
bool server_streaming_{false};
|
||||
bool client_streaming_{false};
|
||||
GrpcFailureSink sink_;
|
||||
std::optional<GrpcFailureRecord> pending_failure_;
|
||||
std::mutex sink_mutex_;
|
||||
std::atomic<bool> server_cancel_requested_{false};
|
||||
std::atomic<bool> cancellation_recorded_{false};
|
||||
};
|
||||
|
||||
class GrpcErrorLoggingInterceptorFactory final
|
||||
: public grpc::experimental::ServerInterceptorFactoryInterface {
|
||||
public:
|
||||
explicit GrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink)
|
||||
: sink_(std::move(sink))
|
||||
{
|
||||
}
|
||||
|
||||
grpc::experimental::Interceptor* CreateServerInterceptor(
|
||||
grpc::experimental::ServerRpcInfo* info) override
|
||||
{
|
||||
const std::string method =
|
||||
info != nullptr && info->method() != nullptr
|
||||
? info->method()
|
||||
: "<unknown>";
|
||||
const bool server_streaming =
|
||||
info != nullptr &&
|
||||
(info->type() == grpc::experimental::ServerRpcInfo::Type::SERVER_STREAMING ||
|
||||
info->type() == grpc::experimental::ServerRpcInfo::Type::BIDI_STREAMING);
|
||||
const bool client_streaming =
|
||||
info != nullptr &&
|
||||
(info->type() == grpc::experimental::ServerRpcInfo::Type::CLIENT_STREAMING ||
|
||||
info->type() == grpc::experimental::ServerRpcInfo::Type::BIDI_STREAMING);
|
||||
return new GrpcErrorLoggingInterceptor(
|
||||
method,
|
||||
info == nullptr ? nullptr : info->server_context(),
|
||||
responseShapeForMethod(method), server_streaming,
|
||||
client_streaming, sink_);
|
||||
}
|
||||
|
||||
private:
|
||||
GrpcFailureSink sink_;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
std::unique_ptr<grpc::experimental::ServerInterceptorFactoryInterface>
|
||||
makeGrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink)
|
||||
{
|
||||
return std::make_unique<GrpcErrorLoggingInterceptorFactory>(
|
||||
std::move(sink));
|
||||
}
|
||||
|
||||
} // namespace cmvr::service
|
||||
561
cmvr-es/service/grpc/server/src/grpc_head_service.cpp
Normal file
561
cmvr-es/service/grpc/server/src/grpc_head_service.cpp
Normal file
@ -0,0 +1,561 @@
|
||||
#include "common/base/logging/logger.h"
|
||||
#include "../include/grpc_head_service.h"
|
||||
|
||||
#include "cmvr/api/biohead_service.grpc.pb.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
#include "common/base/grpc_utils.h"
|
||||
#include "biohead/biohead_esp32/include/biohead_esp32.h"
|
||||
#include "service/grpc/server/include/grpc_command_transaction.h"
|
||||
#include "service/grpc/server/include/media_activity_coordinator.h"
|
||||
#include "service/grpc/server/include/grpc_security.h"
|
||||
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
|
||||
#include <chrono>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <optional>
|
||||
#include <utility>
|
||||
|
||||
using namespace std;
|
||||
using namespace cmvr::service;
|
||||
using namespace cmvr::device;
|
||||
using namespace cmvr::api;
|
||||
|
||||
namespace {
|
||||
template <typename ResponseT>
|
||||
grpc::Status failResponse(ResponseT* response, const std::string& message) {
|
||||
response->mutable_header()->set_success(false);
|
||||
response->mutable_header()->set_error_message(message);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
void logSuccess(const char* rpc_name, const std::string& device_id) {
|
||||
CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (" << rpc_name
|
||||
<< "): success, id=" << device_id;
|
||||
}
|
||||
|
||||
std::optional<AbstractBiohead::OperationalToken> admitHeadCommand(
|
||||
const std::shared_ptr<AbstractBiohead>& robot)
|
||||
{
|
||||
auto admission = globalStopAllAdmissionGate().lockAdmission();
|
||||
if (!admission.accepting() || !robot) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return robot->beginOperationalActivity();
|
||||
}
|
||||
|
||||
template <typename ResponseT>
|
||||
grpc::Status failStoppedCommand(ResponseT* response)
|
||||
{
|
||||
return failResponse(
|
||||
response,
|
||||
"Biohead command was rejected because StopAll is in progress or "
|
||||
"the command was preempted");
|
||||
}
|
||||
|
||||
template <typename RequestT, typename ResponseT, typename Operation>
|
||||
grpc::Status executeHeadOperationalCommand(
|
||||
const std::shared_ptr<GrpcSecurityGateway>& security_gateway,
|
||||
grpc::ServerContext* context,
|
||||
DeviceManager& device_manager,
|
||||
const char* full_method_name,
|
||||
const char* rpc_name,
|
||||
const RequestT* request,
|
||||
ResponseT* response,
|
||||
Operation&& operation)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway, context, device_manager.safetyManager(),
|
||||
full_method_name, request, response,
|
||||
[&device_manager, request, response, rpc_name,
|
||||
operation = std::forward<Operation>(operation)](
|
||||
GrpcCommandTransaction& command) mutable {
|
||||
const std::string device_id = request->header().device_id();
|
||||
const auto robot =
|
||||
device_manager.getDevice<AbstractBiohead>(device_id);
|
||||
if (!robot) {
|
||||
return failResponse(
|
||||
response, "Biohead device not found: " + device_id);
|
||||
}
|
||||
|
||||
const auto activity = admitHeadCommand(robot);
|
||||
if (!activity) {
|
||||
return failStoppedCommand(response);
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
if (!operation(*robot, *activity)) {
|
||||
return failStoppedCommand(response);
|
||||
}
|
||||
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(
|
||||
response->mutable_header()->mutable_timestamp());
|
||||
logSuccess(rpc_name, device_id);
|
||||
return grpc::Status::OK;
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl()
|
||||
: gRPCMBioHeadServiceImpl(makeDefaultGrpcSecurityGateway()) {}
|
||||
|
||||
gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl(
|
||||
std::shared_ptr<GrpcSecurityGateway> security_gateway)
|
||||
: dmgr_(DeviceManager::getInstance()),
|
||||
security_gateway_(security_gateway
|
||||
? std::move(security_gateway)
|
||||
: makeDefaultGrpcSecurityGateway()) {}
|
||||
|
||||
|
||||
// 设置表情（一次性）
|
||||
grpc::Status gRPCMBioHeadServiceImpl::SetExpression(
|
||||
grpc::ServerContext* context,
|
||||
const SetFacialExpression_Request* request,
|
||||
SetFacialExpression_Feedback* response) {
|
||||
return executeHeadOperationalCommand(
|
||||
security_gateway_, context, dmgr_,
|
||||
"/cmvr.api.BioHeadService/SetExpression", "SetExpression",
|
||||
request, response,
|
||||
[request](AbstractBiohead& robot,
|
||||
const AbstractBiohead::OperationalToken activity) {
|
||||
FacialExpressionState expression_state;
|
||||
expression_state.left_eyebrow_outside_y =
|
||||
request->expression().eyebrow().left_outside_y();
|
||||
expression_state.left_eyebrow_inside_y =
|
||||
request->expression().eyebrow().left_inside_y();
|
||||
expression_state.right_eyebrow_outside_y =
|
||||
request->expression().eyebrow().right_outside_y();
|
||||
expression_state.right_eyebrow_inside_y =
|
||||
request->expression().eyebrow().right_inside_y();
|
||||
expression_state.left_eye_upper_lid_y =
|
||||
request->expression().eyelid().left_upper_y();
|
||||
expression_state.left_eye_lower_lid_y =
|
||||
request->expression().eyelid().left_lower_y();
|
||||
expression_state.right_eye_upper_lid_y =
|
||||
request->expression().eyelid().right_upper_y();
|
||||
expression_state.right_eye_lower_lid_y =
|
||||
request->expression().eyelid().right_lower_y();
|
||||
expression_state.left_eye_ball_x =
|
||||
request->expression().eyeball().left_x();
|
||||
expression_state.left_eye_ball_y =
|
||||
request->expression().eyeball().left_y();
|
||||
expression_state.right_eye_ball_x =
|
||||
request->expression().eyeball().right_x();
|
||||
expression_state.right_eye_ball_y =
|
||||
request->expression().eyeball().right_y();
|
||||
expression_state.left_nose_y =
|
||||
request->expression().nose().left_y();
|
||||
expression_state.right_nose_y =
|
||||
request->expression().nose().right_y();
|
||||
expression_state.upper_lip_y =
|
||||
request->expression().mouth().upper_lip_y();
|
||||
expression_state.lower_lip_y =
|
||||
request->expression().mouth().lower_lip_y();
|
||||
return robot.setExpressionPoseIfCurrent(
|
||||
activity, expression_state);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
// 流式控制接口
|
||||
grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
|
||||
grpc::ServerContext* context,
|
||||
grpc::ServerReaderWriter<StreamFacialExpression_Feedback, StreamFacialExpression_Request>* stream)
|
||||
{
|
||||
CMVR_GRPC_REQUIRE_REGISTERED_CALL(
|
||||
security_gateway_, context,
|
||||
"/cmvr.api.BioHeadService/StreamExpression");
|
||||
StreamFacialExpression_Feedback feedback_msg;
|
||||
std::string dev_id;
|
||||
std::shared_ptr<AbstractBiohead> robot;
|
||||
bool first_message = true;
|
||||
AbstractBiohead::OperationalToken activity{0U};
|
||||
std::optional<GrpcStreamingSafetySession> safety_session;
|
||||
auto media_session = globalMediaActivityCoordinator().beginSession(
|
||||
[context] {
|
||||
if (context) {
|
||||
context->TryCancel();
|
||||
}
|
||||
});
|
||||
if (!media_session) {
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::UNAVAILABLE,
|
||||
"Biohead stream rejected because StopAll is in progress");
|
||||
}
|
||||
|
||||
try {
|
||||
StreamFacialExpression_Request request_msg;
|
||||
constexpr float control_frequency = 10;
|
||||
const auto time_interval = std::chrono::milliseconds(static_cast<int>(1000 / control_frequency));
|
||||
auto last_control_time =
|
||||
std::chrono::steady_clock::now() - time_interval;
|
||||
|
||||
CMVR_LOG(INFO) << "StreamExpression started.";
|
||||
|
||||
while (stream->Read(&request_msg)) {
|
||||
if (first_message) {
|
||||
dev_id = request_msg.header().device_id();
|
||||
if (dev_id.empty()) {
|
||||
feedback_msg.mutable_header()->set_success(false);
|
||||
feedback_msg.mutable_header()->set_error_message("Device ID is empty in first message");
|
||||
setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
|
||||
stream->Write(feedback_msg);
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
|
||||
if (!robot) {
|
||||
const std::string message = "Biohead device not found: " + dev_id;
|
||||
feedback_msg.mutable_header()->set_success(false);
|
||||
feedback_msg.mutable_header()->set_error_message(message);
|
||||
setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
|
||||
stream->Write(feedback_msg);
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
|
||||
const auto admitted = admitHeadCommand(robot);
|
||||
if (!admitted) {
|
||||
feedback_msg.mutable_header()->set_success(false);
|
||||
feedback_msg.mutable_header()->set_error_message(
|
||||
"Biohead stream rejected because StopAll is in "
|
||||
"progress");
|
||||
setCurrentTimestamp(
|
||||
feedback_msg.mutable_header()->mutable_timestamp());
|
||||
stream->Write(feedback_msg);
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
activity = *admitted;
|
||||
|
||||
const auto& header = request_msg.header();
|
||||
GrpcStreamingSafetyOpen safety_open;
|
||||
safety_open.full_method_name =
|
||||
"/cmvr.api.BioHeadService/StreamExpression";
|
||||
safety_open.device_id = dev_id;
|
||||
safety_open.session_id = header.command_id().empty()
|
||||
? cmvr_grpc_call_guard.context().correlation_id
|
||||
: header.command_id();
|
||||
safety_open.expected_service_instance_id =
|
||||
header.expected_service_instance_id();
|
||||
if (header.has_expected_device_generation()) {
|
||||
safety_open.expected_device_generation =
|
||||
header.expected_device_generation();
|
||||
}
|
||||
safety_open.authority_generation = activity;
|
||||
safety_open.deadline =
|
||||
cmvr_grpc_call_guard.context().deadline;
|
||||
safety_session.emplace(
|
||||
dmgr_.safetyManager(),
|
||||
cmvr_grpc_call_guard.context(),
|
||||
std::move(safety_open));
|
||||
if (!safety_session->admitted()) {
|
||||
feedback_msg.mutable_header()->set_success(false);
|
||||
feedback_msg.mutable_header()->set_error_message(
|
||||
safety_session->status().error_message());
|
||||
setCurrentTimestamp(
|
||||
feedback_msg.mutable_header()->mutable_timestamp());
|
||||
stream->Write(feedback_msg);
|
||||
return safety_session->status();
|
||||
}
|
||||
|
||||
first_message = false;
|
||||
CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): streaming success, id=" << dev_id;
|
||||
} else if (!request_msg.header().device_id().empty() &&
|
||||
request_msg.header().device_id() != dev_id) {
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::INVALID_ARGUMENT,
|
||||
"biohead stream cannot change device_id after its first frame");
|
||||
}
|
||||
|
||||
// ✅ 如果紧急停止触发，直接退出
|
||||
if (media_session.cancelled() ||
|
||||
(context && context->IsCancelled())) {
|
||||
CMVR_LOG(WARNING) << "[Stream] Emergency stop requested. Terminating stream for device: " << dev_id;
|
||||
break;
|
||||
}
|
||||
|
||||
if (!safety_session || !safety_session->revalidate()) {
|
||||
const auto status = safety_session
|
||||
? safety_session->status()
|
||||
: grpc::Status(
|
||||
grpc::StatusCode::INTERNAL,
|
||||
"biohead stream safety session was not initialized");
|
||||
feedback_msg.mutable_header()->set_success(false);
|
||||
feedback_msg.mutable_header()->set_error_message(
|
||||
status.error_message());
|
||||
setCurrentTimestamp(
|
||||
feedback_msg.mutable_header()->mutable_timestamp());
|
||||
stream->Write(feedback_msg);
|
||||
return status;
|
||||
}
|
||||
|
||||
auto current_time = std::chrono::steady_clock::now();
|
||||
auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_control_time);
|
||||
if (elapsed_time < time_interval) continue;
|
||||
|
||||
FacialExpressionState expression_state;
|
||||
|
||||
// 眉毛
|
||||
expression_state.left_eyebrow_outside_y = request_msg.expr().eyebrow().left_outside_y();
|
||||
expression_state.left_eyebrow_inside_y = request_msg.expr().eyebrow().left_inside_y();
|
||||
expression_state.right_eyebrow_outside_y = request_msg.expr().eyebrow().right_outside_y();
|
||||
expression_state.right_eyebrow_inside_y = request_msg.expr().eyebrow().right_inside_y();
|
||||
|
||||
// 眼睑
|
||||
expression_state.left_eye_upper_lid_y = request_msg.expr().eyelid().left_upper_y();
|
||||
expression_state.left_eye_lower_lid_y = request_msg.expr().eyelid().left_lower_y();
|
||||
expression_state.right_eye_upper_lid_y = request_msg.expr().eyelid().right_upper_y();
|
||||
expression_state.right_eye_lower_lid_y = request_msg.expr().eyelid().right_lower_y();
|
||||
|
||||
// 眼球
|
||||
expression_state.left_eye_ball_y = request_msg.expr().eyeball().left_y();
|
||||
expression_state.right_eye_ball_y = request_msg.expr().eyeball().right_y();
|
||||
|
||||
// 鼻子
|
||||
expression_state.left_nose_y = request_msg.expr().nose().left_y();
|
||||
expression_state.right_nose_y = request_msg.expr().nose().right_y();
|
||||
|
||||
// 嘴部
|
||||
expression_state.upper_lip_y = request_msg.expr().mouth().upper_lip_y();
|
||||
expression_state.lower_lip_y = request_msg.expr().mouth().lower_lip_y();
|
||||
|
||||
// 嘴角
|
||||
expression_state.left_corner_lip_x = request_msg.expr().mouth().left_lip().upper_y();
|
||||
expression_state.left_corner_lip_y = request_msg.expr().mouth().left_lip().corner_y();
|
||||
expression_state.lower_left_lip_y = request_msg.expr().mouth().left_lip().lower_y();
|
||||
|
||||
expression_state.right_corner_lip_x = request_msg.expr().mouth().right_lip().upper_y();
|
||||
expression_state.lower_right_lip_y = request_msg.expr().mouth().right_lip().corner_y();
|
||||
expression_state.right_corner_lip_y = request_msg.expr().mouth().right_lip().lower_y();
|
||||
|
||||
// 下巴
|
||||
expression_state.jaw_x = request_msg.expr().jaw().x();
|
||||
expression_state.jaw_y = request_msg.expr().jaw().y();
|
||||
|
||||
bool dispatched = false;
|
||||
grpc::Status dispatch_status = grpc::Status::OK;
|
||||
const bool current_session = media_session.runIfCurrent([&] {
|
||||
auto dispatch = safety_session->beginDispatch();
|
||||
if (!dispatch.acquired()) {
|
||||
dispatch_status = safety_session->status();
|
||||
return;
|
||||
}
|
||||
dispatched = robot->streamFacialPoseIfCurrent(
|
||||
activity, expression_state, 0, 0);
|
||||
});
|
||||
if (!dispatch_status.ok()) {
|
||||
feedback_msg.mutable_header()->set_success(false);
|
||||
feedback_msg.mutable_header()->set_error_message(
|
||||
dispatch_status.error_message());
|
||||
setCurrentTimestamp(
|
||||
feedback_msg.mutable_header()->mutable_timestamp());
|
||||
stream->Write(feedback_msg);
|
||||
return dispatch_status;
|
||||
}
|
||||
if (!current_session || !dispatched) {
|
||||
CMVR_LOG(WARNING)
|
||||
<< "[gRPCMBioHeadServiceImpl] StreamExpression was "
|
||||
"preempted, id="
|
||||
<< dev_id;
|
||||
break;
|
||||
}
|
||||
last_control_time = current_time;
|
||||
|
||||
feedback_msg.mutable_header()->set_success(true);
|
||||
feedback_msg.mutable_header()->clear_error_message();
|
||||
setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
|
||||
if (!stream->Write(feedback_msg)) break;
|
||||
}
|
||||
|
||||
CMVR_LOG(INFO) << "StreamExpression finished for device: " << dev_id;
|
||||
CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): finished, id=" << dev_id;
|
||||
return grpc::Status::OK;
|
||||
} catch (const std::exception& e) {
|
||||
feedback_msg.mutable_header()->set_success(false);
|
||||
feedback_msg.mutable_header()->set_error_message(e.what());
|
||||
setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
|
||||
if (stream) stream->Write(feedback_msg);
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 获取设备状态
|
||||
grpc::Status gRPCMBioHeadServiceImpl::GetSystemStatus(
|
||||
grpc::ServerContext* context,
|
||||
const GetStatus_Request* request,
|
||||
GetStatus_Feedback* response)
|
||||
{
|
||||
CMVR_GRPC_REQUIRE_REGISTERED_CALL(
|
||||
security_gateway_, context,
|
||||
"/cmvr.api.BioHeadService/GetSystemStatus");
|
||||
try {
|
||||
string dev_id = request->header().device_id();
|
||||
auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
|
||||
if (!robot) {
|
||||
return failResponse(response, "Biohead device not found: " + dev_id);
|
||||
}
|
||||
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||
logSuccess("GetSystemStatus", dev_id);
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
catch (const 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 gRPCMBioHeadServiceImpl::EmergencyStop(
|
||||
grpc::ServerContext* context,
|
||||
const EmergencyStop_Request* request,
|
||||
EmergencyStop_Feedback* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.BioHeadService/EmergencyStop", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const string dev_id = request->header().device_id();
|
||||
const auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
|
||||
if (!robot) {
|
||||
return failResponse(
|
||||
response, "Biohead device not found: " + dev_id);
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
if (!robot->stopOperationalActivity()) {
|
||||
return failResponse(
|
||||
response,
|
||||
"Biohead could not confirm that operational activity "
|
||||
"stopped: " + dev_id);
|
||||
}
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(
|
||||
response->mutable_header()->mutable_timestamp());
|
||||
logSuccess("EmergencyStop", dev_id);
|
||||
return grpc::Status::OK;
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response)
|
||||
{
|
||||
return executeHeadOperationalCommand(
|
||||
security_gateway_, context, dmgr_,
|
||||
"/cmvr.api.BioHeadService/SpeakStart", "SpeakStart",
|
||||
request, response,
|
||||
[](AbstractBiohead& robot,
|
||||
const AbstractBiohead::OperationalToken activity) {
|
||||
return robot.speakStartIfCurrent(activity);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
|
||||
grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response)
|
||||
{
|
||||
return executeRegisteredGrpcCommand(
|
||||
security_gateway_, context, dmgr_.safetyManager(),
|
||||
"/cmvr.api.BioHeadService/SpeakStop", request, response,
|
||||
[this, request, response](GrpcCommandTransaction& command) {
|
||||
const string dev_id = request->header().device_id();
|
||||
const auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
|
||||
if (!robot) {
|
||||
return failResponse(
|
||||
response, "Biohead device not found: " + dev_id);
|
||||
}
|
||||
if (!command.beginDispatch()) {
|
||||
return command.dispatchStatus();
|
||||
}
|
||||
robot->speakstop();
|
||||
response->mutable_header()->set_success(true);
|
||||
setCurrentTimestamp(
|
||||
response->mutable_header()->mutable_timestamp());
|
||||
logSuccess("SpeakStop", dev_id);
|
||||
return grpc::Status::OK;
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
|
||||
grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response)
|
||||
{
|
||||
return executeHeadOperationalCommand(
|
||||
security_gateway_, context, dmgr_,
|
||||
"/cmvr.api.BioHeadService/Happy", "Happy", request, response,
|
||||
[](AbstractBiohead& robot,
|
||||
const AbstractBiohead::OperationalToken activity) {
|
||||
return robot.expressionHappyIfCurrent(activity);
|
||||
});
|
||||
}
|
||||
|
||||
grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response)
|
||||
{
|
||||
return executeHeadOperationalCommand(
|
||||
security_gateway_, context, dmgr_,
|
||||
"/cmvr.api.BioHeadService/Surprise", "Surprise", request,
|
||||
response,
|
||||
[](AbstractBiohead& robot,
|
||||
const AbstractBiohead::OperationalToken activity) {
|
||||
return robot.expressionSurprisedIfCurrent(activity);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response)
|
||||
{
|
||||
return executeHeadOperationalCommand(
|
||||
security_gateway_, context, dmgr_,
|
||||
"/cmvr.api.BioHeadService/ExpressionTired", "ExpressionTired",
|
||||
request, response,
|
||||
[](AbstractBiohead& robot,
|
||||
const AbstractBiohead::OperationalToken activity) {
|
||||
return robot.expressionTiredIfCurrent(activity);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
|
||||
grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response)
|
||||
{
|
||||
return executeHeadOperationalCommand(
|
||||
security_gateway_, context, dmgr_,
|
||||
"/cmvr.api.BioHeadService/ExpressionAngry", "ExpressionAngry",
|
||||
request, response,
|
||||
[](AbstractBiohead& robot,
|
||||
const AbstractBiohead::OperationalToken activity) {
|
||||
return robot.expressionAngryIfCurrent(activity);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
|
||||
grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response)
|
||||
{
|
||||
return executeHeadOperationalCommand(
|
||||
security_gateway_, context, dmgr_,
|
||||
"/cmvr.api.BioHeadService/ExpressionSadness",
|
||||
"ExpressionSadness", request, response,
|
||||
[](AbstractBiohead& robot,
|
||||
const AbstractBiohead::OperationalToken activity) {
|
||||
return robot.expressionSadnessIfCurrent(activity);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response)
|
||||
{
|
||||
return executeHeadOperationalCommand(
|
||||
security_gateway_, context, dmgr_,
|
||||
"/cmvr.api.BioHeadService/ExpressionYawn", "ExpressionYawn",
|
||||
request, response,
|
||||
[](AbstractBiohead& robot,
|
||||
const AbstractBiohead::OperationalToken activity) {
|
||||
return robot.expressionYawnIfCurrent(activity);
|
||||
});
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user