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

update camera intrinsics

Browse Source
This commit is contained in:
linbo 2025-08-27 15:30:51 +08:00
parent d466303937
commit ab5aadd82e
21 changed files with 275 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
config/cabin_robot.xml
View File

@ -13,16 +13,16 @@
<Camera>
<!-- <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
<!-- <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
<!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="color" codec="H265"/>-->
<!-- <RealsenseCamera id="cam4" serial="243122075614" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" codec="H265"/>-->
<!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<RealsenseCamera id="cam4" serial="243122075614" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>
<!-- <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>-->
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" codec="H265"/>-->
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
</Camera>
<DexHand>
<!-- <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="10.148.108.115" port="6000">-->
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
<!-- </RH56DFTP>-->
<RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="10.148.108.115" port="6000">
<Freedom order="01" default_force="500" default_speed="500" />
</RH56DFTP>
<!-- <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="10.148.108.113" port="6000">-->
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
<!-- </RH56DFTP>-->
@ -32,38 +32,38 @@
<!-- <LeftArm id="left_arm" devtype="ti5Robot" />-->
<!-- <RightArm />-->
<!-- <Neck/>-->
<Humanoid id="hc01" dof="14"
urdf="/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
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"
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"
bufferSize="50">
<CanManger id="" devId="">
<LeftArmCan id = " " devId = " " channelId ="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="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="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"/>
<!-- <Humanoid id="hc01" dof="14"-->
<!-- urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"-->
<!-- 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"-->
<!-- 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"-->
<!-- bufferSize="50">-->
<!-- <CanManger id="" devId="">-->
<!-- <LeftArmCan id = " " devId = " " channelId ="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="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="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="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
</LeftArmCan>
<RightArmCan id = " " devId = " " channelId ="1">
<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="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="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="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan>
<WaistCan id = " " devId = " " channelId ="2">
<Motor id="14" 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"/>
</WaistCan>
</CanManger>
<!-- </LeftArmCan>-->
<!-- <RightArmCan id = " " devId = " " channelId ="1">-->
<!-- <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="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="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="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>-->
<!-- </RightArmCan>-->
<!-- <WaistCan id = " " devId = " " channelId ="2">-->
<!-- <Motor id="14" 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"/>-->
<!-- </WaistCan>-->
<!-- </CanManger>-->
</Humanoid>
<!-- </Humanoid>-->
</Robot>
<BioHead>
@ -83,8 +83,8 @@
</BioHead >
<Microphone>
<ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>
<!-- <ffmpegMicPhone id="mic2" alsa="hw:1" channels="2" sampleRate="44100" volume="80"/>-->
<!-- <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
<ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>
</Microphone>
<Speaker>
@ -131,7 +131,7 @@
</MonitorManager>
<gRPCServer port="50055">
<gRPCServer port="50060">
</gRPCServer>

6
include/devices/abstract_camera.h
View File

@ -47,9 +47,9 @@ namespace cmvr::device {
~AbstractCamera() override = default;
inline void getState(CameraState &state) {state = state_;}
virtual void getRGBImage(cv::Mat &color) {}
virtual void getDepthImage(cv::Mat &depth) {}
virtual void getRGBDImages(cv::Mat &color, cv::Mat &depth) {}
virtual void getRGBImage(cv::Mat &color, Rs2Intrinsics& intrinsics) {}
virtual void getDepthImage(cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
virtual void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
virtual void startRecording(const std::string &video_path) {}
virtual void stopRecording() {}
virtual void pauseRecording() {}

1
include/service/grpc_system_service.h
View File

@ -17,6 +17,7 @@ namespace cmvr::service
~gRPCSystemServiceImpl() override = default;
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 UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override;
private:
device::DeviceManager& dmgr_;
};

3
protos/cmvr/api/camera_command.proto
View File

@ -79,6 +79,7 @@ message GetRGBImageCommand {
message Feedback {
CommandHeader.Feedback header = 1;
FrameData color_frame = 2;
Rs2Intrinsics intrinsics = 3;
}
}
@ -90,6 +91,7 @@ message GetDepthImageCommand {
message Feedback {
CommandHeader.Feedback header = 1;
FrameData depth_frame = 2;
Rs2Intrinsics intrinsics = 3;
}
}
@ -102,6 +104,7 @@ message GetRGBDImagesCommand {
CommandHeader.Feedback header = 1;
FrameData color_frame = 2;
FrameData depth_frame = 3;
Rs2Intrinsics intrinsics = 4;
}
}

13
protos/cmvr/api/common.proto
View File

@ -28,3 +28,16 @@ message CommandHeader {
google.protobuf.Timestamp timestamp = 3; // 回复时间
}
}
message ConfigParam {
string param_name = 1;
oneof param_value {
int32 int_value = 2; // 整数类型
double double_value = 3; // 浮点数类型
string string_value = 4; // 字符串类型
bool bool_value = 5; // 布尔类型
bytes bytes_value = 6; // 二进制数据类型
}
}

11
protos/cmvr/api/system_command.proto
View File

@ -48,3 +48,14 @@ message GetSystemStatusCommand {
repeated DeviceList device_list = 7;
}
}
message UpdateParamsCommand {
message Request {
CommandHeader.Request header = 1;
repeated ConfigParam params = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
}
}

2
protos/cmvr/api/system_service.proto
View File

@ -8,4 +8,6 @@ package cmvr.api;
service SystemService {
rpc GetSystemInfo(GetSystemInfoCommand.Request) returns (GetSystemInfoCommand.Feedback) {}
rpc GetSystemStatus(GetSystemStatusCommand.Request) returns (GetSystemStatusCommand.Feedback) {}
rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {}
}

16
src/device_manager/device_manager.cpp
View File

@ -106,9 +106,23 @@ void DeviceManager::stop() {
throw std::runtime_error("[DeviceManager] (stop): Device manager start failed: " + string(e.what()));
}
}
std::shared_ptr<AbstractDevice> getDeviceBasePtr(DeviceVariant& variant) {
return std::visit([](auto& device_ptr) -> std::shared_ptr<AbstractDevice> {
// 利用多态，自动转换为基类指针
return device_ptr;
}, variant);
}
void DeviceManager::updateParam(const std::string& id, const std::pair<std::string, std::string> &param){
auto it = devices_.find(id);
if (it == devices_.end()) {
LOG(WARNING) << "[DeviceManager]: Device ID " << id << " not found.";
throw runtime_error("[DeviceManager]: Device ID " + id + " not found.");
}
// 获取基类指针（无需知道具体类型）
std::shared_ptr<AbstractDevice> base_dev = getDeviceBasePtr(it->second);
// 直接调用基类中定义的通用接口
base_dev->updateParams(param); // 调用配置接口
}

6
src/devices/camera/mechmind/mechmind_camera.cpp
View File

@ -78,7 +78,7 @@ void MechmindCamera::stop() {
}
}
void MechmindCamera::getRGBImage(cv::Mat& color) {
void MechmindCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
try {
std::lock_guard lock(dev_mtx_);
clear_error_();
@ -123,7 +123,7 @@ void MechmindCamera::getRGBImage(cv::Mat& color) {
}
}
void MechmindCamera::getDepthImage(cv::Mat& depth) {
void MechmindCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
try {
std::lock_guard lock(dev_mtx_);
clear_error_();
@ -149,7 +149,7 @@ void MechmindCamera::getDepthImage(cv::Mat& depth) {
}
}
void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth) {
void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) {
try {
std::lock_guard lock(dev_mtx_);
clear_error_();

6
src/devices/camera/mechmind/mechmind_camera.h
View File

@ -29,9 +29,9 @@ namespace cmvr::device
void init() override;
void start() override;
void stop() override;
void getRGBImage(cv::Mat& color) override;
void getDepthImage(cv::Mat& depth) override;
void getRGBDImages(cv::Mat &color, cv::Mat &depth) override;
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
void updateParams(const std::pair<std::string, std::string>& param) override;
void startRecording(const std::string &video_path) override;
void stopRecording() override;

27
src/devices/camera/realsense_camera/realsense_camera.cpp
View File

@ -226,7 +226,14 @@ void RealsenseCamera::stop() {
}
}
void RealsenseCamera::getRGBImage(cv::Mat& color) {
void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
intrinsics.cx = intrinsics_.ppx;
intrinsics.cy = intrinsics_.ppy;
intrinsics.fx = intrinsics_.fx;
intrinsics.fy = intrinsics_.fy;
for (int i = 0; i < 5 ; i++) {
intrinsics.coeffs[i] = intrinsics_.coeffs[i];
}
try {
std::lock_guard lock(ctrl_mtx_);
clear_error_();
@ -252,8 +259,15 @@ void RealsenseCamera::getRGBImage(cv::Mat& color) {
}
}
void RealsenseCamera::getDepthImage(cv::Mat& depth) {
void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
intrinsics.cx = intrinsics_.ppx;
intrinsics.cy = intrinsics_.ppy;
intrinsics.fx = intrinsics_.fx;
intrinsics.fy = intrinsics_.fy;
for (int i = 0; i < 5 ; i++) {
intrinsics.coeffs[i] = intrinsics_.coeffs[i];
}
auto frame = stream_frame_buffer_->pop(getImageIndex_);
if (frame.has_value()) {
depth = frame.value().depthImage.clone();
@ -283,8 +297,15 @@ void RealsenseCamera::getDepthImage(cv::Mat& depth) {
}
}
void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth) {
void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {
intrinsics.cx = intrinsics_.ppx;
intrinsics.cy = intrinsics_.ppy;
intrinsics.fx = intrinsics_.fx;
intrinsics.fy = intrinsics_.fy;
for (int i = 0; i < 5 ; i++) {
intrinsics.coeffs[i] = intrinsics_.coeffs[i];
}
auto frame = stream_frame_buffer_->pop(getImageIndex_);
if (frame.has_value()) {
color = frame.value().rgbImage.clone();

6
src/devices/camera/realsense_camera/realsense_camera.h
View File

@ -21,9 +21,9 @@ namespace cmvr::device{
void init() override;
void start() override;
void stop() override;
void getRGBImage(cv::Mat& color) override;
void getDepthImage(cv::Mat& depth) override;
void getRGBDImages(cv::Mat &color, cv::Mat &depth) override;
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
void updateParams(const std::pair<std::string, std::string>& param) override;
void startRecording(const std::string &video_path) override;
void stopRecording() override;

6
src/devices/camera/uvc_camera/uvc_camera.cpp
View File

@ -187,7 +187,7 @@ void UVCCamera::stop() {
}
}
void UVCCamera::getRGBImage(cv::Mat& color)
void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
{
std::lock_guard lock(ctrl_mtx_);
clear_error_();
@ -207,11 +207,11 @@ void UVCCamera::getRGBImage(cv::Mat& color)
}
}
void UVCCamera::getDepthImage(cv::Mat& depth) {
void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
throw runtime_error("[UVCCamera] (getDepthImage): unsupported usage");
}
void UVCCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth) {
void UVCCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {
throw runtime_error("[UVCCamera] (getDepthImage): unsupported usage");
}

6
src/devices/camera/uvc_camera/uvc_camera.h
View File

@ -75,9 +75,9 @@ namespace cmvr::device {
void init() override;
void start() override;
void stop() override;
void getRGBImage(cv::Mat& color) override;
void getDepthImage(cv::Mat& depth) override;
void getRGBDImages(cv::Mat &color, cv::Mat &depth) override;
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
void updateParams(const std::pair<std::string, std::string>& param) override;
void startRecording(const std::string &video_path) override;
void stopRecording() override;

2
src/devices/canbus/CMakeLists.txt
View File

@ -13,8 +13,6 @@ add_library(cmvr_es::device::canbus ALIAS canbus)
target_link_libraries(canbus
PRIVATE
# proto-objects
# gRPC::grpc++
protobuf::libprotobuf
glog::glog
# PUBLIC pcanbasic

20
src/devices/microphone/ffmpeg_microphone/ffmpeg_microphone.cpp
View File

@ -456,4 +456,24 @@ void ffmpegMicroPhone::closeFFmpeg() {
}
avformat_free_context(output_fmt_ctx);
}
}
void ffmpegMicroPhone::updateParams(const std::pair<std::string, std::string>& param)
{
std::lock_guard lock(mtx_);
try {
if (param.first == "sampleRate") {
sample_rate_ = stoi(param.second);
}
else if (param.first == "channels") {
channels_ = stoi(param.second);
}
stop();
init();
start();
}
catch (exception &e) {
throw runtime_error(e.what());
}
}

2
src/devices/microphone/ffmpeg_microphone/ffmpeg_microphone.h
View File

@ -28,7 +28,7 @@ namespace cmvr::device {
void init() override;
void start() override;
void stop() override;
void updateParams(const std::pair<std::string, std::string>& param) override;
void getState(MicrophoneState &state) override;
void startRecording(const std::string& outputFilePath) override;
void stopRecording() override;

20
src/devices/speaker/ffmpeg_speaker/ffmpeg_speaker.cpp
View File

@ -355,3 +355,23 @@ void ffmpegSpeaker::play_audio_() {
//state_.is_running = false;
resetPlayState();
}
void ffmpegSpeaker::updateParams(const std::pair<std::string, std::string>& param)
{
std::lock_guard lock(mtx_);
try {
if (param.first == "sampleRate") {
sample_rate_ = stoi(param.second);
}
else if (param.first == "channels") {
channels_ = stoi(param.second);
}
stop();
init();
start();
}
catch (exception &e) {
throw runtime_error(e.what());
}
}

1
src/devices/speaker/ffmpeg_speaker/ffmpeg_speaker.h
View File

@ -28,6 +28,7 @@ namespace cmvr::device {
void init() override;
void start() override;
void stop() override;
void updateParams(const std::pair<std::string, std::string>& param) override;
void play(const std::string& audio_path) override;
void setVolume(int volume) override;
[[nodiscard]] int getVolume() const override;

36
src/service/grpc_camera_service.cpp
View File

@ -90,7 +90,17 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id;
cv::Mat image;
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
dev->getRGBImage(image);
Rs2Intrinsics intrinsics = {0};
dev->getRGBImage(image,intrinsics);
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);
for (int i = 0; i < 5 ; i++) {
response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
}
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@ -131,8 +141,18 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImage): id=" << dev_id;
cv::Mat image;
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
dev->getRGBImage(image);
Rs2Intrinsics intrinsics = {0};
dev->getDepthImage(image,intrinsics);
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);
for (int i = 0; i < 5 ; i++) {
response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
}
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
if (image.type() == CV_8UC1) {
@ -175,7 +195,17 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImages): id=" << dev_id;
cv::Mat color_image, depth_image;
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
dev->getRGBDImages(color_image, depth_image);
Rs2Intrinsics intrinsics = {0};
dev->getRGBDImages(color_image,depth_image, intrinsics);
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);
for (int i = 0; i < 5 ; i++) {
response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
}
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());

74
src/service/grpc_system_service.cpp
View File

@ -88,3 +88,77 @@ grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context
return grpc::Status::OK;
}
}
grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCSystemServiceImpl] (UpdateParams): id=" << dev_id;
auto params = request->params();
for (auto& param : params) {
//遍历拿到需要配置的自由度,传入的是百分比0-1
std::string param_name = param.param_name();
std::string param_value_str; // 存储转换后的字符串值
// 根据 oneof 类型分支转换
switch (param.param_value_case()) {
case cmvr::api::ConfigParam::kIntValue: {
// 整数类型：用 std::to_string 转换
int32_t int_val = param.int_value();
param_value_str = std::to_string(int_val);
break;
}
case cmvr::api::ConfigParam::kDoubleValue: {
// 浮点数类型：用 std::to_string 转换（也可按需用 printf 控制精度）
double double_val = param.double_value();
param_value_str = std::to_string(double_val);
// （可选）若需控制浮点数精度（如保留2位小数），可替换为：
// char buf[64];
// snprintf(buf, sizeof(buf), "%.2f", double_val);
// param_value_str = buf;
break;
}
case cmvr::api::ConfigParam::kStringValue: {
// 字符串类型：直接赋值（本身就是 string）
param_value_str = param.string_value();
break;
}
case cmvr::api::ConfigParam::kBoolValue: {
// 布尔类型：转为 "true" 或 "false"
bool bool_val = param.bool_value();
param_value_str = bool_val ? "true" : "false";
break;
}
case cmvr::api::ConfigParam::kBytesValue: {
// 二进制类型：可选方案（二选一，根据你的需求）
// 方案1：转为十六进制字符串（推荐，二进制数据可视化）
const auto& bytes_val = param.bytes_value();
static const char hex_chars[] = "0123456789ABCDEF";
std::string hex_str;
hex_str.reserve(bytes_val.size() * 2); // 预分配空间，提升效率
for (unsigned char c : bytes_val) {
hex_str.push_back(hex_chars[c >> 4]);
hex_str.push_back(hex_chars[c & 0x0F]);
}
param_value_str = hex_str; // 结果如 "A3B2C1"
break;
}
case cmvr::api::ConfigParam::PARAM_VALUE_NOT_SET: {
// 未设置值的异常场景：赋值为空字符串或标记
param_value_str = "[UNSET]";
LOG(WARNING) << "[UpdateParams] Param '" << param_name << "' has no value";
break;
}
}
dmgr_.updateParam(dev_id,std::pair<std::string,std::string>(param_name,param_value_str));
}
return grpc::Status::OK;
}
catch (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;
}
}