SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'dev' of http://192.168.0.100:18088/smart_bench/cmvr-es into dev

Browse Source
This commit is contained in:
lgv 2026-03-17 16:19:58 +08:00
commit 4629de3d36
14 changed files with 850 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cmvr-es/common/CMakeLists.txt
View File

@ -4,6 +4,7 @@
file(GLOB SRC file(GLOB SRC
${CMAKE_CURRENT_SOURCE_DIR}/utils/config_helper/src/config_setting.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/config_helper/src/config_setting.cpp
${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/CameraCapture.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/CameraCapture.cpp
${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/RealSenseCapture.cpp
${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoFrameEncoder.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoFrameEncoder.cpp
${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoWriter.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoWriter.cpp
${CMAKE_CURRENT_SOURCE_DIR}/curve/src/s_curve.cpp ${CMAKE_CURRENT_SOURCE_DIR}/curve/src/s_curve.cpp
@ -26,6 +27,10 @@ target_link_libraries(common PUBLIC
avutil avutil
avcodec avcodec
swscale swscale
realsense2
opencv_core
opencv_imgproc
opencv_videoio
) )
add_library(cmvr_es::common ALIAS common) add_library(cmvr_es::common ALIAS common)

74
cmvr-es/common/config/cabin_robot.xml
View File

@ -32,44 +32,44 @@
<!-- <LeftArm id="left_arm" devtype="ti5Robot" />--> <!-- <LeftArm id="left_arm" devtype="ti5Robot" />-->
<!-- <RightArm />--> <!-- <RightArm />-->
<!-- <Neck/>--> <!-- <Neck/>-->
<Humanoid id="hc01" dof="14" <!-- <Humanoid id="hc01" dof="14"-->
urdf="/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf" <!-- urdf="/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"-->
baseLink="PELVIS_S" <!-- baseLink="PELVIS_S"-->
jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R" <!-- jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"-->
linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM" <!-- linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"-->
bufferSize="50" <!-- bufferSize="50"-->
verbose="false"> <!-- verbose="false">-->
<CanManger id="" devId=""> <!-- <CanManger id="" devId="">-->
<LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP"> <!-- <LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP">-->
<Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
</LeftArmCan> <!-- </LeftArmCan>-->
<RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP"> <!-- <RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">-->
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/> <!-- <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>-->
<Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/> <!-- <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>-->
</RightArmCan> <!-- </RightArmCan>-->
<HeadCan id = " " devId = " " channelId ="2" enable="false"> <!-- <HeadCan id = " " devId = " " channelId ="2" enable="false">-->
<Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
</HeadCan> <!-- </HeadCan>-->
<WaistCan id = " " devId = " " channelId ="3" enable="false"> <!-- <WaistCan id = " " devId = " " channelId ="3" enable="false">-->
<Motor id="4" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="4" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
</WaistCan> <!-- </WaistCan>-->
</CanManger> <!-- </CanManger>-->
</Humanoid> <!-- </Humanoid>-->
</Robot> </Robot>
<BioHead> <BioHead>

18
cmvr-es/common/utils/config_helper/src/config_setting.cpp
View File

@ -2,18 +2,18 @@
#include <unistd.h> #include <unistd.h>
// static std::string basePath()
// {
// char* cwd = ::getcwd(nullptr, 0);
// if (!cwd) return {};
// std::string s(cwd);
// std::free(cwd);
// return s + "/config/";
// }
static std::string basePath() static std::string basePath()
{ {
return "/home/lgv/cmvr/cmvr-es/output/bin/config/"; char* cwd = ::getcwd(nullptr, 0);
if (!cwd) return {};
std::string s(cwd);
std::free(cwd);
return s + "/config/";
} }
// static std::string basePath()
// {
// return "/home/lgv/cmvr/cmvr-es/output/bin/config/";
// }
DEFINE_string(pinocchio_qp_ik_solver_config_file, DEFINE_string(pinocchio_qp_ik_solver_config_file,
basePath() + "ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt", basePath() + "ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt",

18
cmvr-es/common/utils/ffmpeg/include/CameraCapture.h
View File

@ -4,26 +4,29 @@
#include <string> #include <string>
#include <memory> #include <memory>
#include <functional> #include <functional>
#include <opencv2/opencv.hpp>
extern "C"{ extern "C"{
#include <libavformat/avformat.h> #include <libavformat/avformat.h>
#include <libavcodec/avcodec.h> #include <libavcodec/avcodec.h>
#include <libavdevice/avdevice.h> #include <libavdevice/avdevice.h>
#include <libswscale/swscale.h>
} }
namespace ffmpeg { namespace ffmpeg {
struct CameraConfig { struct CameraConfig {
std::string device_name = "0"; // Windows: "0" (dshow), Linux: "0" (对应 /dev/video0) std::string device_name = "0"; // Windows: "0" (dshow), Linux: "/dev/video0"
int width = 1280; int width = 1280;
int height = 720; int height = 720;
int fps = 30; int fps = 30;
AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P; AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P; // 保持原有的像素格式
}; };
class CameraCapture { class CameraCapture {
public: public:
using FrameCallback = std::function<void(AVFrame* frame)>; // 回调函数：同时返回AVFrame和cv::Mat
using FrameCallback = std::function<void(AVFrame* frame, cv::Mat rgb_image, int64_t timestamp)>;
CameraCapture(); CameraCapture();
~CameraCapture(); ~CameraCapture();
@ -54,6 +57,9 @@ namespace ffmpeg {
bool is_capturing_ = false; bool is_capturing_ = false;
AVPacket* packet_ = nullptr; AVPacket* packet_ = nullptr;
AVFrame* frame_ = nullptr; AVFrame* frame_ = nullptr;
AVFrame* rgb_frame_ = nullptr; // 用于转换的RGB帧
SwsContext* sws_ctx_ = nullptr; // 格式转换上下文
int64_t frame_count_ = 0;
// 初始化设备（平台相关） // 初始化设备（平台相关）
int init_device(); int init_device();
@ -61,6 +67,12 @@ namespace ffmpeg {
// 初始化解码器 // 初始化解码器
int init_decoder(); int init_decoder();
// 初始化SWS上下文
int init_sws_context();
// 将AVFrame转换为cv::Mat
cv::Mat avframe_to_cvmat(AVFrame* frame);
// 释放资源 // 释放资源
void cleanup(); void cleanup();
}; };

94
cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h Normal file
View File

@ -0,0 +1,94 @@
// RealSenseCapture.h
#pragma once
#include <string>
#include <memory>
#include <functional>
#include <mutex>
#include <thread>
#include <atomic>
#include <opencv2/opencv.hpp>
#include <librealsense2/rs.hpp>
extern "C" {
#include <libavformat/avformat.h>
#include <libavcodec/avcodec.h>
#include <libswscale/swscale.h>
}
namespace ffmpeg {
struct RealSenseConfig {
std::string serial_number = ""; // 设备序列号，为空则使用第一个设备
int width = 1280;
int height = 720;
int fps = 30;
AVPixelFormat pixel_format = AV_PIX_FMT_BGR24; // RealSense输出BGR格式
};
class RealSenseCapture {
public:
// 回调函数：同时返回AVFrame和cv::Mat
using FrameCallback = std::function<void(AVFrame* frame, cv::Mat rgb_image, int64_t timestamp)>;
RealSenseCapture();
~RealSenseCapture();
// 初始化摄像头
int initialize(const RealSenseConfig& config);
// 开始捕获
int start_capture(FrameCallback callback);
// 停止捕获
void stop_capture();
// 获取当前配置
RealSenseConfig get_config() const { return config_; }
// 获取设备信息
std::string get_device_info() const;
// 获取内参
rs2_intrinsics get_intrinsics() const { return intrinsics_; }
private:
RealSenseConfig config_;
// RealSense相关
rs2::context ctx_;
rs2::pipeline pipe_;
rs2::config rs_cfg_;
rs2::pipeline_profile profile_;
rs2_intrinsics intrinsics_;
// FFmpeg相关
AVFrame* frame_ = nullptr; // 用于编码的AVFrame
AVFrame* rgb_frame_ = nullptr; // 用于OpenCV的AVFrame
// 状态控制
std::atomic<bool> is_capturing_{false};
std::unique_ptr<std::thread> capture_thread_;
mutable std::mutex mutex_;
// 帧计数
int64_t frame_count_ = 0;
// 初始化设备
int init_device();
// 创建AVFrame
AVFrame* create_avframe(int width, int height, AVPixelFormat pix_fmt);
// 捕获线程函数
void capture_thread_func(FrameCallback callback);
// 将RealSense帧转换为AVFrame
AVFrame* realsense_to_avframe(rs2::frame frame);
// 将RealSense帧转换为cv::Mat
cv::Mat realsense_to_cvmat(rs2::frame frame);
};
} // namespace ffmpeg

87
cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp
View File

@ -9,6 +9,7 @@ namespace ffmpeg {
CameraCapture::CameraCapture() { CameraCapture::CameraCapture() {
packet_ = av_packet_alloc(); packet_ = av_packet_alloc();
frame_ = av_frame_alloc(); frame_ = av_frame_alloc();
rgb_frame_ = av_frame_alloc();
} }
CameraCapture::~CameraCapture() { CameraCapture::~CameraCapture() {
@ -17,6 +18,8 @@ CameraCapture::~CameraCapture() {
if (packet_) av_packet_free(&packet_); if (packet_) av_packet_free(&packet_);
if (frame_) av_frame_free(&frame_); if (frame_) av_frame_free(&frame_);
if (rgb_frame_) av_frame_free(&rgb_frame_);
if (sws_ctx_) sws_freeContext(sws_ctx_);
} }
int CameraCapture::initialize(const CameraConfig& config) { int CameraCapture::initialize(const CameraConfig& config) {
@ -36,6 +39,25 @@ int CameraCapture::initialize(const CameraConfig& config) {
return ret; return ret;
} }
// 初始化SWS上下文
ret = init_sws_context();
if (ret < 0) {
std::cerr << "初始化SWS上下文失败" << std::endl;
return ret;
}
// 分配RGB帧缓冲区（用于转换）
rgb_frame_->width = config_.width;
rgb_frame_->height = config_.height;
rgb_frame_->format = AV_PIX_FMT_BGR24; // OpenCV使用BGR格式
ret = av_frame_get_buffer(rgb_frame_, 0);
if (ret < 0) {
std::cerr << "分配RGB帧缓冲区失败" << std::endl;
return ret;
}
std::cout << "摄像头初始化成功: " << config_.width << "x" << config_.height
<< "@" << config_.fps << "fps" << std::endl;
return 0; return 0;
} }
@ -56,8 +78,15 @@ int CameraCapture::init_device() {
return -1; return -1;
} }
// 设置摄像头参数
AVDictionary* options = nullptr;
av_dict_set(&options, "framerate", std::to_string(config_.fps).c_str(), 0);
av_dict_set(&options, "video_size", (std::to_string(config_.width) + "x" + std::to_string(config_.height)).c_str(), 0);
// 打开摄像头 // 打开摄像头
int ret = avformat_open_input(&fmt_ctx_, device_path.c_str(), input_fmt, nullptr); int ret = avformat_open_input(&fmt_ctx_, device_path.c_str(), input_fmt, &options);
av_dict_free(&options);
if (ret < 0) { if (ret < 0) {
char err_buf[1024]; char err_buf[1024];
av_strerror(ret, err_buf, sizeof(err_buf)); av_strerror(ret, err_buf, sizeof(err_buf));
@ -114,6 +143,46 @@ int CameraCapture::init_decoder() {
return 0; return 0;
} }
int CameraCapture::init_sws_context() {
// 获取解码器输出的像素格式
AVPixelFormat src_format = decoder_ctx_->pix_fmt;
sws_ctx_ = sws_getContext(
decoder_ctx_->width, decoder_ctx_->height, src_format,
config_.width, config_.height, AV_PIX_FMT_BGR24, // 转换为BGR24给OpenCV
SWS_BILINEAR, nullptr, nullptr, nullptr
);
if (!sws_ctx_) {
std::cerr << "创建SWS上下文失败" << std::endl;
return -1;
}
return 0;
}
cv::Mat CameraCapture::avframe_to_cvmat(AVFrame* frame) {
// 确保RGB帧可写
av_frame_make_writable(rgb_frame_);
// 转换到BGR24
sws_scale(sws_ctx_,
frame->data, frame->linesize, 0, frame->height,
rgb_frame_->data, rgb_frame_->linesize);
// 创建cv::Mat（BGR格式）
cv::Mat image(config_.height, config_.width, CV_8UC3);
// 复制数据（由于linesize可能不等于width*3，需要逐行复制）
for (int i = 0; i < config_.height; i++) {
memcpy(image.data + i * image.step,
rgb_frame_->data[0] + i * rgb_frame_->linesize[0],
config_.width * 3);
}
return image;
}
int CameraCapture::start_capture(FrameCallback callback) { int CameraCapture::start_capture(FrameCallback callback) {
if (!callback || !fmt_ctx_ || !decoder_ctx_) { if (!callback || !fmt_ctx_ || !decoder_ctx_) {
std::cerr << "参数无效或未初始化" << std::endl; std::cerr << "参数无效或未初始化" << std::endl;
@ -121,6 +190,9 @@ int CameraCapture::start_capture(FrameCallback callback) {
} }
is_capturing_ = true; is_capturing_ = true;
frame_count_ = 0;
std::cout << "开始采集..." << std::endl;
while (is_capturing_) { while (is_capturing_) {
// 读取数据包 // 读取数据包
@ -154,15 +226,18 @@ int CameraCapture::start_capture(FrameCallback callback) {
break; break;
} }
// 回调处理帧 // 设置PTS
callback(frame_); frame_->pts = frame_count_;
// 转换为cv::Mat（用于显示或OpenCV处理）
cv::Mat rgb_image = avframe_to_cvmat(frame_);
// 回调：同时返回AVFrame和cv::Mat
callback(frame_, rgb_image, frame_count_++);
} }
} }
av_packet_unref(packet_); av_packet_unref(packet_);
// 控制帧率
std::this_thread::sleep_for(std::chrono::milliseconds(1000 / config_.fps));
} }
return 0; return 0;

301
cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp Normal file
View File

@ -0,0 +1,301 @@
// RealSenseCapture.cpp
#include "../include/RealSenseCapture.h"
#include <iostream>
#include <chrono>
namespace ffmpeg {
RealSenseCapture::RealSenseCapture() {
frame_ = av_frame_alloc();
rgb_frame_ = av_frame_alloc();
}
RealSenseCapture::~RealSenseCapture() {
stop_capture();
if (frame_) av_frame_free(&frame_);
if (rgb_frame_) av_frame_free(&rgb_frame_);
}
int RealSenseCapture::initialize(const RealSenseConfig& config) {
std::lock_guard<std::mutex> lock(mutex_);
config_ = config;
try {
// 初始化设备
int ret = init_device();
if (ret < 0) {
std::cerr << "初始化RealSense设备失败" << std::endl;
return ret;
}
// 创建AVFrame
if (frame_) av_frame_free(&frame_);
if (rgb_frame_) av_frame_free(&rgb_frame_);
frame_ = create_avframe(config_.width, config_.height, config_.pixel_format);
rgb_frame_ = create_avframe(config_.width, config_.height, AV_PIX_FMT_BGR24);
if (!frame_ || !rgb_frame_) {
std::cerr << "创建AVFrame失败" << std::endl;
return -1;
}
std::cout << "RealSense摄像头初始化成功: "
<< config_.width << "x" << config_.height << "@" << config_.fps << "fps" << std::endl;
return 0;
} catch (const rs2::error& e) {
std::cerr << "RealSense错误: " << e.what() << std::endl;
return -1;
} catch (const std::exception& e) {
std::cerr << "常规错误: " << e.what() << std::endl;
return -1;
}
}
int RealSenseCapture::init_device() {
try {
// 查询设备
rs2::device_list devices = ctx_.query_devices();
size_t device_count = devices.size();
if (device_count == 0) {
std::cerr << "未检测到RealSense设备" << std::endl;
return -1;
}
std::cout << "检测到 " << device_count << " 个RealSense设备" << std::endl;
// 如果指定了序列号，查找对应设备
if (!config_.serial_number.empty()) {
bool found = false;
for (size_t i = 0; i < device_count; i++) {
rs2::device dev = devices[i];
std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
if (serial == config_.serial_number) {
std::cout << "找到指定序列号的设备: " << serial << std::endl;
found = true;
rs_cfg_.enable_device(serial);
break;
}
}
if (!found) {
std::cerr << "未找到序列号为 " << config_.serial_number << " 的设备" << std::endl;
return -1;
}
} else {
// 使用第一个设备
rs2::device dev = devices[0];
std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
std::cout << "使用第一个设备: " << dev.get_info(RS2_CAMERA_INFO_NAME)
<< " (序列号: " << serial << ")" << std::endl;
rs_cfg_.enable_device(serial);
}
// 配置彩色视频流
rs_cfg_.enable_stream(RS2_STREAM_COLOR,
config_.width,
config_.height,
RS2_FORMAT_BGR8, // OpenCV使用BGR格式
config_.fps);
return 0;
} catch (const rs2::error& e) {
std::cerr << "初始化设备失败: " << e.what() << std::endl;
return -1;
}
}
AVFrame* RealSenseCapture::create_avframe(int width, int height, AVPixelFormat pix_fmt) {
AVFrame* frame = av_frame_alloc();
if (!frame) {
return nullptr;
}
frame->width = width;
frame->height = height;
frame->format = pix_fmt;
// 分配缓冲区
int ret = av_frame_get_buffer(frame, 0);
if (ret < 0) {
av_frame_free(&frame);
return nullptr;
}
return frame;
}
AVFrame* RealSenseCapture::realsense_to_avframe(rs2::frame frame) {
auto vf = frame.as<rs2::video_frame>();
const int width = vf.get_width();
const int height = vf.get_height();
const uint8_t* data = reinterpret_cast<const uint8_t*>(vf.get_data());
// 确保帧可写
av_frame_make_writable(frame_);
// RealSense的BGR8格式，linesize = width * 3
int src_linesize = width * 3;
// 复制数据到AVFrame
for (int i = 0; i < height; i++) {
memcpy(frame_->data[0] + i * frame_->linesize[0],
data + i * src_linesize,
src_linesize);
}
return frame_;
}
cv::Mat RealSenseCapture::realsense_to_cvmat(rs2::frame frame) {
auto vf = frame.as<rs2::video_frame>();
const int width = vf.get_width();
const int height = vf.get_height();
// RealSense的BGR8格式直接对应OpenCV的CV_8UC3
cv::Mat image(cv::Size(width, height), CV_8UC3, (void*)vf.get_data(), cv::Mat::AUTO_STEP);
// 返回深拷贝，确保图像数据独立
return image.clone();
}
int RealSenseCapture::start_capture(FrameCallback callback) {
std::lock_guard<std::mutex> lock(mutex_);
if (!callback) {
std::cerr << "回调函数为空" << std::endl;
return -1;
}
if (is_capturing_) {
std::cerr << "已经在捕获中" << std::endl;
return -1;
}
try {
// 启动pipeline
profile_ = pipe_.start(rs_cfg_);
// 获取内参
auto stream = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
intrinsics_ = stream.get_intrinsics();
// 等待第一帧，确保设备正常工作
rs2::frameset frames = pipe_.wait_for_frames(2000); // 2秒超时
if (!frames.get_color_frame()) {
std::cerr << "无法获取第一帧" << std::endl;
pipe_.stop();
return -1;
}
is_capturing_ = true;
frame_count_ = 0;
// 启动捕获线程
capture_thread_ = std::make_unique<std::thread>(&RealSenseCapture::capture_thread_func,
this, callback);
std::cout << "RealSense开始采集" << std::endl;
return 0;
} catch (const rs2::error& e) {
std::cerr << "启动采集失败: " << e.what() << std::endl;
return -1;
}
}
void RealSenseCapture::capture_thread_func(FrameCallback callback) {
const int frame_interval = 1000 / config_.fps; // 毫秒
while (is_capturing_) {
auto frame_start = std::chrono::steady_clock::now();
try {
// 等待帧数据
rs2::frameset frames = pipe_.wait_for_frames(1000); // 1秒超时
rs2::frame color_frame = frames.get_color_frame();
if (!color_frame) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
continue;
}
// 转换为AVFrame（用于编码）
AVFrame* av_frame = realsense_to_avframe(color_frame);
av_frame->pts = frame_count_;
// 转换为cv::Mat（用于显示或OpenCV处理）
cv::Mat rgb_image = realsense_to_cvmat(color_frame);
// 回调：同时返回AVFrame和cv::Mat
callback(av_frame, rgb_image, frame_count_++);
} catch (const rs2::error& e) {
std::cerr << "采集错误: " << e.what() << std::endl;
if (!is_capturing_) break;
}
// 控制帧率
auto frame_end = std::chrono::steady_clock::now();
auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
frame_end - frame_start).count();
if (elapsed < frame_interval) {
std::this_thread::sleep_for(std::chrono::milliseconds(frame_interval - elapsed));
}
}
}
void RealSenseCapture::stop_capture() {
std::lock_guard<std::mutex> lock(mutex_);
if (!is_capturing_) {
return;
}
is_capturing_ = false;
if (capture_thread_ && capture_thread_->joinable()) {
capture_thread_->join();
capture_thread_.reset();
}
try {
pipe_.stop();
} catch (const std::exception& e) {
std::cerr << "停止pipeline时出错: " << e.what() << std::endl;
}
std::cout << "RealSense停止采集" << std::endl;
}
std::string RealSenseCapture::get_device_info() const {
try {
rs2::device_list devices = ctx_.query_devices();
if (devices.size() == 0) {
return "No RealSense device found";
}
std::string info;
for (size_t i = 0; i < devices.size(); i++) {
rs2::device dev = devices[i];
info += "Device " + std::to_string(i + 1) + ": ";
info += dev.get_info(RS2_CAMERA_INFO_NAME);
info += " (SN: " + std::string(dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER)) + ")";
if (i < devices.size() - 1) info += "\n";
}
return info;
} catch (const std::exception& e) {
return "Failed to get device info: " + std::string(e.what());
}
}
} // namespace ffmpeg

39
cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp
View File

@ -218,10 +218,10 @@ void RealsenseCamera::init() {
} }
// 初始化深度图编码器（示例参数：640x480，15fps，H.265） // 初始化深度图编码器（示例参数：640x480，15fps，H.265）
if (!initSingleEncoder(depthEncoder_, codec_, width_, height_, fps_)) { // if (!initSingleEncoder(depthEncoder_, codec_, width_, height_, fps_)) {
LOG(ERROR) << "[RealsenseCamera] (start): Failed to init depth encoder!"; // LOG(ERROR) << "[RealsenseCamera] (start): Failed to init depth encoder!";
throw runtime_error("Failed to init depth encoder!"); // throw runtime_error("Failed to init depth encoder!");
} // }
LOG(INFO) << "[RealsenseCamera] (init): realsense camera initialized at index " << serial_; // 记录初始化成功日志 LOG(INFO) << "[RealsenseCamera] (init): realsense camera initialized at index " << serial_; // 记录初始化成功日志
} }
catch (const exception &error){ catch (const exception &error){
@ -884,7 +884,7 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& enco
ctx->time_base = {1, fps}; // 时间基（1/fps） ctx->time_base = {1, fps}; // 时间基（1/fps）
ctx->framerate = {fps, 1}; // 帧率 ctx->framerate = {fps, 1}; // 帧率
ctx->max_b_frames = 0; // 禁用B帧（降低延迟） ctx->max_b_frames = 0; // 禁用B帧（降低延迟）
ctx->gop_size = 10;//I帧间隔1，每一帧都是I帧 ctx->gop_size = 10;//I帧间隔
// 5. 设置编码器私有参数 // 5. 设置编码器私有参数
if (codec->id == AV_CODEC_ID_H264) { if (codec->id == AV_CODEC_ID_H264) {
av_opt_set(ctx->priv_data, "preset", "ultrafast", 0); av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
@ -895,6 +895,24 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& enco
} else if (codec->id == AV_CODEC_ID_HEVC) { } else if (codec->id == AV_CODEC_ID_HEVC) {
// 降低 x265 控制台日志噪声（如 "encoded 0 frames"）。 // 降低 x265 控制台日志噪声（如 "encoded 0 frames"）。
av_opt_set(ctx->priv_data, "x265-params", "log-level=none", 0); av_opt_set(ctx->priv_data, "x265-params", "log-level=none", 0);
// H.265 的强制设置
// 使用 x265-params 字符串设置所有参数
char x265_params[256];
snprintf(x265_params, sizeof(x265_params),
"keyint=%d:" // 关键帧间隔
"min-keyint=%d:" // 最小关键帧间隔
"no-open-gop=1:" // 禁用开放GOP
"bframes=0:" // 禁用B帧
"lookahead=0:"
"log-level=none", // 日志级别
10, 10); // 设置keyint和min-keyint为10
av_opt_set(ctx->priv_data, "x265-params", x265_params, 0);
// 或者分开设置（如果支持）
av_opt_set_int(ctx->priv_data, "keyint", 10, 0);
av_opt_set_int(ctx->priv_data, "min-keyint", 10, 0);
av_opt_set(ctx->priv_data, "no-open-gop", "1", 0);
} }
// 6. 设置像素格式（不变） // 6. 设置像素格式（不变）
@ -938,6 +956,17 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& enco
return false; return false;
} }
// 添加调试信息，确认设置生效
std::cout << "Encoder settings:" << std::endl;
std::cout << " GOP size: " << ctx->gop_size << std::endl;
if (codec->id == AV_CODEC_ID_HEVC) {
char* params = nullptr;
if (av_opt_get(ctx->priv_data, "x265-params", 0, (uint8_t**)&params) >= 0) {
std::cout << " x265-params: " << params << std::endl;
av_free(params);
}
}
std::cout << "Successfully initialized " << codec_name << " encoder ( " std::cout << "Successfully initialized " << codec_name << " encoder ( "
<< width << "x" << height << "@" << fps << "fps )" << std::endl; << width << "x" << height << "@" << fps << "fps )" << std::endl;
return true; return true;

3
cmvr-es/devices/robot/abstract_robot.h
View File

@ -176,6 +176,9 @@ namespace cmvr::device{
virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0; virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0;
virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0; virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0;
virtual bool getPoseMatrix(std::string &bask_link, std::string &ee_link, Eigen::Matrix4d& cur_pose) = 0;
virtual bool fk(const std::vector<double>& joints_angle, Eigen::Matrix4d& cur_pose, bool is_tcp = true) = 0;
protected: protected:
int dof_{}; int dof_{};
RobotState state_{}; RobotState state_{};

13
cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h
View File

@ -6,7 +6,7 @@
#include "device_manager/include/device_manager.h" #include "device_manager/include/device_manager.h"
#include "cmvr/api/humanoid_robot_service.grpc.pb.h" #include "cmvr/api/humanoid_robot_service.grpc.pb.h"
#include "ik_solver/include/pinocchio_qp_ik_solver.h"
namespace cmvr { namespace cmvr {
namespace service { namespace service {
class gRPCHumanoidRobotServiceImpl final : public api::HumanoidRobotService::Service { class gRPCHumanoidRobotServiceImpl final : public api::HumanoidRobotService::Service {
@ -31,8 +31,19 @@ namespace cmvr {
grpc::Status getPose(grpc::ServerContext *context, const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) override; grpc::Status getPose(grpc::ServerContext *context, const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) override;
grpc::Status calibrateZeroQ(grpc::ServerContext* context, const cmvr::api::CalibrateZeroQ_Request* request, cmvr::api::CalibrateZeroQ_Response* response) override; grpc::Status calibrateZeroQ(grpc::ServerContext* context, const cmvr::api::CalibrateZeroQ_Request* request, cmvr::api::CalibrateZeroQ_Response* response) override;
grpc::Status servoJ(grpc::ServerContext* context, const cmvr::api::ServoJ_Request* request, cmvr::api::ServoJ_Response* response) override;
grpc::Status getPoseMatrix(grpc::ServerContext* context, const cmvr::api::GetPoseMatrix_Request* request, cmvr::api::GetPoseMatrix_Response* response) override;
grpc::Status computeForwardKinematics(grpc::ServerContext* context, const cmvr::api::ComputeForwardKinematics_Request* request, cmvr::api::ComputeForwardKinematics_Response* response) override;
private: private:
device::DeviceManager& dmgr_; device::DeviceManager& dmgr_;
//矩阵转换
std::shared_ptr<PinocchioQpIKSolver> pinocchio_ik_{nullptr};
}; };

24
cmvr-es/service/grpc/src/grpc_camera_service.cpp
View File

@ -95,9 +95,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
response->mutable_header()->set_success(true); response->mutable_header()->set_success(true);
response->mutable_intrinsics()->set_fx(intrinsics.fx); response->mutable_intrinsics()->set_fx(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fx); response->mutable_intrinsics()->set_fy(intrinsics.fy);
response->mutable_intrinsics()->set_cx(intrinsics.fx); response->mutable_intrinsics()->set_cx(intrinsics.cx);
response->mutable_intrinsics()->set_cy(intrinsics.fx); response->mutable_intrinsics()->set_cy(intrinsics.cy);
for (int i = 0; i < 5 ; i++) { for (int i = 0; i < 5 ; i++) {
response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]); response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
} }
@ -146,9 +146,9 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
response->mutable_header()->set_success(true); response->mutable_header()->set_success(true);
response->mutable_intrinsics()->set_fx(intrinsics.fx); response->mutable_intrinsics()->set_fx(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fx); response->mutable_intrinsics()->set_fy(intrinsics.fy);
response->mutable_intrinsics()->set_cx(intrinsics.fx); response->mutable_intrinsics()->set_cx(intrinsics.cx);
response->mutable_intrinsics()->set_cy(intrinsics.fx); response->mutable_intrinsics()->set_cy(intrinsics.cy);
for (int i = 0; i < 5 ; i++) { for (int i = 0; i < 5 ; i++) {
response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]); response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
} }
@ -200,9 +200,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
response->mutable_header()->set_success(true); response->mutable_header()->set_success(true);
response->mutable_intrinsics()->set_fx(intrinsics.fx); response->mutable_intrinsics()->set_fx(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fx); response->mutable_intrinsics()->set_fy(intrinsics.fy);
response->mutable_intrinsics()->set_cx(intrinsics.fx); response->mutable_intrinsics()->set_cx(intrinsics.cx);
response->mutable_intrinsics()->set_cy(intrinsics.fx); response->mutable_intrinsics()->set_cy(intrinsics.cy);
for (int i = 0; i < 5 ; i++) { for (int i = 0; i < 5 ; i++) {
response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]); response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
} }
@ -454,9 +454,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
response.mutable_color_frame()->set_height(frame_data.height); response.mutable_color_frame()->set_height(frame_data.height);
response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy);
for (int i = 0; i < 5 ; i++) { for (int i = 0; i < 5 ; i++) {
response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]); response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
} }

188
cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
View File

@ -14,7 +14,14 @@ using namespace cmvr::api;
using google::protobuf::util::TimeUtil; using google::protobuf::util::TimeUtil;
gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()){} gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance())
{
pinocchio_ik_ = std::make_shared<PinocchioQpIKSolver>("/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
,"PELVIS_S"
,"R_WRIST_R_S");
pinocchio_ik_->init();
}
grpc::Status gRPCHumanoidRobotServiceImpl::torqueOff(grpc::ServerContext *context, grpc::Status gRPCHumanoidRobotServiceImpl::torqueOff(grpc::ServerContext *context,
@ -244,3 +251,182 @@ grpc::Status gRPCHumanoidRobotServiceImpl::calibrateZeroQ(grpc::ServerContext* c
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret; return ret;
} }
grpc::Status gRPCHumanoidRobotServiceImpl::servoJ(grpc::ServerContext* context
, const cmvr::api::ServoJ_Request* request
, cmvr::api::ServoJ_Response* response)
{
grpc::Status ret = grpc::Status::OK;
try {
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
std::vector<JointPoint> cmds{};
for (const auto& jc : request->cmds()) {
cmds.emplace_back(jc.joint_name(),jc.rad(),jc.vel());
}
robot->servoJ(cmds,request->vel(),0);
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
}catch (const std::exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
}
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret;
}
// 在 gRPC 服务实现文件中添加以下代码
grpc::Status gRPCHumanoidRobotServiceImpl::getPoseMatrix(grpc::ServerContext* context
, const cmvr::api::GetPoseMatrix_Request* request
, cmvr::api::GetPoseMatrix_Response* response)
{
grpc::Status ret = grpc::Status::OK;
try {
// 获取机械臂设备
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
// 获取请求参数
std::string base_link = request->base_link();
std::string ee_link = request->ee_link();
// 调用机械臂的 getPoseMatrix 方法获取当前位姿矩阵
Eigen::Matrix4d cur_pose;
bool success = robot->getPoseMatrix(base_link, ee_link, cur_pose);
if (success) {
// 设置成功状态
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
// 填充4x4矩阵到响应
auto* matrix = response->mutable_matrix();
matrix->set_m00(cur_pose(0, 0));
matrix->set_m01(cur_pose(0, 1));
matrix->set_m02(cur_pose(0, 2));
matrix->set_m03(cur_pose(0, 3));
matrix->set_m10(cur_pose(1, 0));
matrix->set_m11(cur_pose(1, 1));
matrix->set_m12(cur_pose(1, 2));
matrix->set_m13(cur_pose(1, 3));
matrix->set_m20(cur_pose(2, 0));
matrix->set_m21(cur_pose(2, 1));
matrix->set_m22(cur_pose(2, 2));
matrix->set_m23(cur_pose(2, 3));
matrix->set_m30(cur_pose(3, 0));
matrix->set_m31(cur_pose(3, 1));
matrix->set_m32(cur_pose(3, 2));
matrix->set_m33(cur_pose(3, 3));
// 可选：记录日志
// LOG(INFO) << "getPoseMatrix success for device: " << request->header().device_id();
} else {
// 获取位姿失败
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message("Failed to get pose matrix");
ret = grpc::Status(grpc::StatusCode::INTERNAL, "Failed to get pose matrix");
}
} catch (const std::exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
}
// 设置时间戳
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret;
}
grpc::Status gRPCHumanoidRobotServiceImpl::computeForwardKinematics(grpc::ServerContext* context
, const cmvr::api::ComputeForwardKinematics_Request* request
, cmvr::api::ComputeForwardKinematics_Response* response)
{
grpc::Status ret = grpc::Status::OK;
try {
// // 获取机械臂设备
// auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
// 解析关节命令
std::vector<JointPoint> cmds{};
for (const auto& jc : request->joints()) {
cmds.emplace_back(jc.joint_name(), jc.rad(), jc.vel());
}
// 如果没有关节命令，返回错误
if (cmds.empty()) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message("No joint commands provided");
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, "No joint commands provided");
}
// 获取请求参数
std::string base_link = request->base_link();
std::string ee_link = request->ee_link();
bool is_tcp = true; // 默认使用TCP
// 从关节命令中提取关节角度
std::vector<double> joints_angle;
for (const auto& cmd : cmds) {
joints_angle.push_back(cmd.rad); // 使用 rad 作为关节角度
}
// 调用机械臂的 fk 方法计算正运动学
Eigen::Matrix4d cur_pose;
bool success = pinocchio_ik_->fk(joints_angle, cur_pose, is_tcp);
if (success) {
// 设置成功状态
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
// 填充4x4矩阵到响应
auto* matrix = response->mutable_matrix();
matrix->set_m00(cur_pose(0, 0));
matrix->set_m01(cur_pose(0, 1));
matrix->set_m02(cur_pose(0, 2));
matrix->set_m03(cur_pose(0, 3));
matrix->set_m10(cur_pose(1, 0));
matrix->set_m11(cur_pose(1, 1));
matrix->set_m12(cur_pose(1, 2));
matrix->set_m13(cur_pose(1, 3));
matrix->set_m20(cur_pose(2, 0));
matrix->set_m21(cur_pose(2, 1));
matrix->set_m22(cur_pose(2, 2));
matrix->set_m23(cur_pose(2, 3));
matrix->set_m30(cur_pose(3, 0));
matrix->set_m31(cur_pose(3, 1));
matrix->set_m32(cur_pose(3, 2));
matrix->set_m33(cur_pose(3, 3));
// 可选：记录日志
// LOG(INFO) << "computeForwardKinematics success for device: " << request->header().device_id();
} else {
// 正运动学计算失败
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message("Forward kinematics calculation failed");
ret = grpc::Status(grpc::StatusCode::INTERNAL, "Forward kinematics calculation failed");
}
} catch (const std::exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
}
// 设置时间戳
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret;
}

56
protos/cmvr/api/humanoid_robot_command.proto
View File

@ -40,6 +40,17 @@ enum RobotJointIndexDirection{
ROTATE_Y = 9; // 绕Y轴旋转 ROTATE_Y = 9; // 绕Y轴旋转
ROTATE_Z = 10; // 绕Z轴旋转 ROTATE_Z = 10; // 绕Z轴旋转
} }
// 4x4 齐次变换矩阵
message TransformMatrix4x4 {
double m00 = 1; double m01 = 2; double m02 = 3; double m03 = 4;
double m10 = 5; double m11 = 6; double m12 = 7; double m13 = 8;
double m20 = 9; double m21 = 10; double m22 = 11; double m23 = 12;
double m30 = 13; double m31 = 14; double m32 = 15; double m33 = 16;
}
message MoveJ{ message MoveJ{
message Request{ message Request{
CommandHeader.Request header = 1; CommandHeader.Request header = 1;
@ -144,3 +155,48 @@ message CalibrateZeroQ {
CommandHeader.Feedback header= 1; CommandHeader.Feedback header= 1;
} }
} }
message ServoJ{
message Request{
CommandHeader.Request header = 1;
repeated JointCmd cmds = 2;
double vel = 3;
}
message Response{
CommandHeader.Feedback header= 1;
}
}
// 获取当前位姿的4x4矩阵
message GetPoseMatrix {
message Request {
CommandHeader.Request header = 1;
string base_link = 2; // 基坐标系
string ee_link = 3; // 末端连杆名称
}
message Response {
CommandHeader.Feedback header = 1;
TransformMatrix4x4 matrix = 2; // 4x4 位姿矩阵
}
}
// 根据关节角度计算正运动学位姿矩阵
message ComputeForwardKinematics {
message Request {
CommandHeader.Request header = 1;
string base_link = 2; // 基坐标系
string ee_link = 3; // 末端连杆名称
repeated JointCmd joints = 4; // 关节角度
}
message Response {
CommandHeader.Feedback header = 1;
TransformMatrix4x4 matrix = 2; // 计算出的4x4位姿矩阵
}
}

4
protos/cmvr/api/humanoid_robot_service.proto
View File

@ -15,4 +15,8 @@ service HumanoidRobotService{
rpc getJointState(JointRequest) returns (JointResponse); rpc getJointState(JointRequest) returns (JointResponse);
rpc getPose(GetPose.Request) returns (GetPose.Response); rpc getPose(GetPose.Request) returns (GetPose.Response);
rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response); rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response);
rpc servoJ(ServoJ.Request) returns (ServoJ.Response);
rpc getPoseMatrix(GetPoseMatrix.Request) returns (GetPoseMatrix.Response);
rpc computeForwardKinematics(ComputeForwardKinematics.Request) returns (ComputeForwardKinematics.Response);
} }