diff --git a/cmvr-es/common/CMakeLists.txt b/cmvr-es/common/CMakeLists.txt index 3bdc2226..a60df3c2 100644 --- a/cmvr-es/common/CMakeLists.txt +++ b/cmvr-es/common/CMakeLists.txt @@ -4,6 +4,7 @@ file(GLOB SRC ${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/RealSenseCapture.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoFrameEncoder.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoWriter.cpp ${CMAKE_CURRENT_SOURCE_DIR}/curve/src/s_curve.cpp @@ -26,6 +27,10 @@ target_link_libraries(common PUBLIC avutil avcodec swscale + realsense2 + opencv_core + opencv_imgproc + opencv_videoio ) add_library(cmvr_es::common ALIAS common) diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml index fa833a13..ecc9ae62 100644 --- a/cmvr-es/common/config/cabin_robot.xml +++ b/cmvr-es/common/config/cabin_robot.xml @@ -32,44 +32,44 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp index a0690df6..2eaf4c98 100644 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ b/cmvr-es/common/utils/config_helper/src/config_setting.cpp @@ -2,18 +2,18 @@ #include -// 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() { - 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, basePath() + "ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt", diff --git a/cmvr-es/common/utils/ffmpeg/include/CameraCapture.h b/cmvr-es/common/utils/ffmpeg/include/CameraCapture.h index b3561c27..6f902185 100644 --- a/cmvr-es/common/utils/ffmpeg/include/CameraCapture.h +++ b/cmvr-es/common/utils/ffmpeg/include/CameraCapture.h @@ -4,26 +4,29 @@ #include #include #include +#include extern "C"{ #include #include #include +#include } namespace ffmpeg { 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 height = 720; int fps = 30; - AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P; + AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P; // 保持原有的像素格式 }; class CameraCapture { public: - using FrameCallback = std::function; + // 回调函数：同时返回AVFrame和cv::Mat + using FrameCallback = std::function; CameraCapture(); ~CameraCapture(); @@ -54,6 +57,9 @@ namespace ffmpeg { bool is_capturing_ = false; AVPacket* packet_ = nullptr; AVFrame* frame_ = nullptr; + AVFrame* rgb_frame_ = nullptr; // 用于转换的RGB帧 + SwsContext* sws_ctx_ = nullptr; // 格式转换上下文 + int64_t frame_count_ = 0; // 初始化设备（平台相关） int init_device(); @@ -61,6 +67,12 @@ namespace ffmpeg { // 初始化解码器 int init_decoder(); + // 初始化SWS上下文 + int init_sws_context(); + + // 将AVFrame转换为cv::Mat + cv::Mat avframe_to_cvmat(AVFrame* frame); + // 释放资源 void cleanup(); }; diff --git a/cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h b/cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h new file mode 100644 index 00000000..66e5578d --- /dev/null +++ b/cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h @@ -0,0 +1,94 @@ +// RealSenseCapture.h +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +extern "C" { +#include +#include +#include +} + +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; + + 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 is_capturing_{false}; + std::unique_ptr 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 \ No newline at end of file diff --git a/cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp b/cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp index 75ac0e70..001e0c18 100644 --- a/cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp +++ b/cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp @@ -9,6 +9,7 @@ namespace ffmpeg { CameraCapture::CameraCapture() { packet_ = av_packet_alloc(); frame_ = av_frame_alloc(); + rgb_frame_ = av_frame_alloc(); } CameraCapture::~CameraCapture() { @@ -17,6 +18,8 @@ CameraCapture::~CameraCapture() { if (packet_) av_packet_free(&packet_); 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) { @@ -36,6 +39,25 @@ int CameraCapture::initialize(const CameraConfig& config) { 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; } @@ -56,8 +78,15 @@ int CameraCapture::init_device() { 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) { char err_buf[1024]; av_strerror(ret, err_buf, sizeof(err_buf)); @@ -114,6 +143,46 @@ int CameraCapture::init_decoder() { 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) { if (!callback || !fmt_ctx_ || !decoder_ctx_) { std::cerr << "参数无效或未初始化" << std::endl; @@ -121,6 +190,9 @@ int CameraCapture::start_capture(FrameCallback callback) { } is_capturing_ = true; + frame_count_ = 0; + + std::cout << "开始采集..." << std::endl; while (is_capturing_) { // 读取数据包 @@ -154,15 +226,18 @@ int CameraCapture::start_capture(FrameCallback callback) { break; } - // 回调处理帧 - callback(frame_); + // 设置PTS + 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_); - - // 控制帧率 - std::this_thread::sleep_for(std::chrono::milliseconds(1000 / config_.fps)); } return 0; diff --git a/cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp b/cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp new file mode 100644 index 00000000..d1165a49 --- /dev/null +++ b/cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp @@ -0,0 +1,301 @@ +// RealSenseCapture.cpp +#include "../include/RealSenseCapture.h" +#include +#include + +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 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(); + const int width = vf.get_width(); + const int height = vf.get_height(); + const uint8_t* data = reinterpret_cast(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(); + 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 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(); + 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(&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( + 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 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 \ No newline at end of file diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index 43aa315d..ec616665 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -218,10 +218,10 @@ void RealsenseCamera::init() { } // 初始化深度图编码器（示例参数：640x480，15fps，H.265） - if (!initSingleEncoder(depthEncoder_, codec_, width_, height_, fps_)) { - LOG(ERROR) << "[RealsenseCamera] (start): Failed to init depth encoder!"; - throw runtime_error("Failed to init depth encoder!"); - } + // if (!initSingleEncoder(depthEncoder_, codec_, width_, height_, fps_)) { + // LOG(ERROR) << "[RealsenseCamera] (start): Failed to init depth encoder!"; + // throw runtime_error("Failed to init depth encoder!"); + // } LOG(INFO) << "[RealsenseCamera] (init): realsense camera initialized at index " << serial_; // 记录初始化成功日志 } catch (const exception &error){ @@ -884,7 +884,7 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr& enco ctx->time_base = {1, fps}; // 时间基（1/fps） ctx->framerate = {fps, 1}; // 帧率 ctx->max_b_frames = 0; // 禁用B帧（降低延迟） - ctx->gop_size = 10;//I帧间隔1，每一帧都是I帧 + ctx->gop_size = 10;//I帧间隔 // 5. 设置编码器私有参数 if (codec->id == AV_CODEC_ID_H264) { av_opt_set(ctx->priv_data, "preset", "ultrafast", 0); @@ -895,6 +895,24 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr& enco } else if (codec->id == AV_CODEC_ID_HEVC) { // 降低 x265 控制台日志噪声（如 "encoded 0 frames"）。 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. 设置像素格式（不变） @@ -938,6 +956,17 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr& enco 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**)¶ms) >= 0) { + std::cout << " x265-params: " << params << std::endl; + av_free(params); + } + } + std::cout << "Successfully initialized " << codec_name << " encoder ( " << width << "x" << height << "@" << fps << "fps )" << std::endl; return true; diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 823caf44..29ffb6f8 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -176,6 +176,9 @@ namespace cmvr::device{ virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0; virtual std::vector 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& joints_angle, Eigen::Matrix4d& cur_pose, bool is_tcp = true) = 0; protected: int dof_{}; RobotState state_{}; diff --git a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h b/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h index a8a404ea..b9f0b9f9 100644 --- a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h +++ b/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h @@ -6,7 +6,7 @@ #include "device_manager/include/device_manager.h" #include "cmvr/api/humanoid_robot_service.grpc.pb.h" - +#include "ik_solver/include/pinocchio_qp_ik_solver.h" namespace cmvr { namespace 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 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: device::DeviceManager& dmgr_; + + + + //矩阵转换 + std::shared_ptr pinocchio_ik_{nullptr}; }; diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index ce34dc9d..fb033cc0 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -95,9 +95,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, response->mutable_header()->set_success(true); response->mutable_intrinsics()->set_fx(intrinsics.fx); - response->mutable_intrinsics()->set_fy(intrinsics.fx); - response->mutable_intrinsics()->set_cx(intrinsics.fx); - response->mutable_intrinsics()->set_cy(intrinsics.fx); + response->mutable_intrinsics()->set_fy(intrinsics.fy); + response->mutable_intrinsics()->set_cx(intrinsics.cx); + response->mutable_intrinsics()->set_cy(intrinsics.cy); for (int i = 0; i < 5 ; 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_intrinsics()->set_fx(intrinsics.fx); - response->mutable_intrinsics()->set_fy(intrinsics.fx); - response->mutable_intrinsics()->set_cx(intrinsics.fx); - response->mutable_intrinsics()->set_cy(intrinsics.fx); + response->mutable_intrinsics()->set_fy(intrinsics.fy); + response->mutable_intrinsics()->set_cx(intrinsics.cx); + response->mutable_intrinsics()->set_cy(intrinsics.cy); for (int i = 0; i < 5 ; 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_intrinsics()->set_fx(intrinsics.fx); - response->mutable_intrinsics()->set_fy(intrinsics.fx); - response->mutable_intrinsics()->set_cx(intrinsics.fx); - response->mutable_intrinsics()->set_cy(intrinsics.fx); + response->mutable_intrinsics()->set_fy(intrinsics.fy); + response->mutable_intrinsics()->set_cx(intrinsics.cx); + response->mutable_intrinsics()->set_cy(intrinsics.cy); for (int i = 0; i < 5 ; 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_intrinsics()->set_fx(frame_data.intrinsics.fx); - response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx); - response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx); - response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); + response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx); + response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy); for (int i = 0; i < 5 ; i++) { response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]); } diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp index f121f1dd..5cade894 100644 --- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp @@ -14,7 +14,14 @@ using namespace cmvr::api; using google::protobuf::util::TimeUtil; -gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()){} +gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()) +{ + + pinocchio_ik_ = std::make_shared("/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, @@ -244,3 +251,182 @@ grpc::Status gRPCHumanoidRobotServiceImpl::calibrateZeroQ(grpc::ServerContext* c *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); 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(request->header().device_id()); + std::vector 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(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(request->header().device_id()); + + // 解析关节命令 + std::vector 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 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; +} \ No newline at end of file diff --git a/protos/cmvr/api/humanoid_robot_command.proto b/protos/cmvr/api/humanoid_robot_command.proto index f39ad778..75804fb1 100644 --- a/protos/cmvr/api/humanoid_robot_command.proto +++ b/protos/cmvr/api/humanoid_robot_command.proto @@ -40,6 +40,17 @@ enum RobotJointIndexDirection{ ROTATE_Y = 9; // 绕Y轴旋转 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 Request{ CommandHeader.Request header = 1; @@ -144,3 +155,48 @@ message CalibrateZeroQ { 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位姿矩阵 + } +} \ No newline at end of file diff --git a/protos/cmvr/api/humanoid_robot_service.proto b/protos/cmvr/api/humanoid_robot_service.proto index 33502e50..b046eb92 100644 --- a/protos/cmvr/api/humanoid_robot_service.proto +++ b/protos/cmvr/api/humanoid_robot_service.proto @@ -15,4 +15,8 @@ service HumanoidRobotService{ rpc getJointState(JointRequest) returns (JointResponse); rpc getPose(GetPose.Request) returns (GetPose.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); }