i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-12 15:29:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update user calibration service interface and device status reporting

Browse Source
This commit is contained in:
xiexun
2025-09-02 17:08:21 +08:00
parent 211edd146b
commit d87f93d440
7 changed files with 168 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+25 -10
View File
@@ -58,8 +58,8 @@
#include "orbbec_camera_msgs/srv/set_string.hpp" #include "orbbec_camera_msgs/srv/set_string.hpp"
#include "orbbec_camera_msgs/srv/set_filter.hpp" #include "orbbec_camera_msgs/srv/set_filter.hpp"
#include "orbbec_camera_msgs/srv/set_arrays.hpp" #include "orbbec_camera_msgs/srv/set_arrays.hpp"
#include "orbbec_camera_msgs/srv/get_camera_params.hpp" #include "orbbec_camera_msgs/srv/get_user_calib_params.hpp"
#include "orbbec_camera_msgs/srv/set_camera_params.hpp" #include "orbbec_camera_msgs/srv/set_user_calib_params.hpp"
#include "orbbec_camera/constants.h" #include "orbbec_camera/constants.h"
#include "orbbec_camera/dynamic_params.h" #include "orbbec_camera/dynamic_params.h"
#include "orbbec_camera/d2c_viewer.h" #include "orbbec_camera/d2c_viewer.h"
@@ -115,8 +115,8 @@ using SetBool = std_srvs::srv::SetBool;
using GetBool = orbbec_camera_msgs::srv::GetBool; using GetBool = orbbec_camera_msgs::srv::GetBool;
using SetFilter = orbbec_camera_msgs::srv::SetFilter; using SetFilter = orbbec_camera_msgs::srv::SetFilter;
using SetArrays = orbbec_camera_msgs::srv::SetArrays; using SetArrays = orbbec_camera_msgs::srv::SetArrays;
using SetCameraParams = orbbec_camera_msgs::srv::SetCameraParams; using SetUserCalibParams = orbbec_camera_msgs::srv::SetUserCalibParams;
using GetCameraParams = orbbec_camera_msgs::srv::GetCameraParams; using GetUserCalibParams = orbbec_camera_msgs::srv::GetUserCalibParams;
typedef std::pair<ob_stream_type, int> stream_index_pair; typedef std::pair<ob_stream_type, int> stream_index_pair;
@@ -202,6 +202,20 @@ class OBCameraNode {
} }
} }
bool checkUserCalibrationReady() {
static bool first_check = true;
if (first_check) {
first_check = false;
std::string data_read;
if (readCustomerData(data_read)) {
user_calibration_ready_ = true;
} else {
user_calibration_ready_ = false;
}
}
return user_calibration_ready_;
}
private: private:
struct IMUData { struct IMUData {
IMUData() = default; IMUData() = default;
@@ -385,11 +399,11 @@ class OBCameraNode {
void readCustomerDataCallback(const std::shared_ptr<GetString::Request>& request, void readCustomerDataCallback(const std::shared_ptr<GetString::Request>& request,
std::shared_ptr<GetString::Response>& response); std::shared_ptr<GetString::Response>& response);
void getCameraParamsCallback(const std::shared_ptr<GetCameraParams::Request>& request, void getUserCalibParamsCallback(const std::shared_ptr<GetUserCalibParams::Request>& request,
std::shared_ptr<GetCameraParams::Response>& response); std::shared_ptr<GetUserCalibParams::Response>& response);
void setCameraParamsCallback(const std::shared_ptr<SetCameraParams::Request>& request, void setUserCalibParamsCallback(const std::shared_ptr<SetUserCalibParams::Request>& request,
std::shared_ptr<SetCameraParams::Response>& response); std::shared_ptr<SetUserCalibParams::Response>& response);
void setIRLongExposureCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request, void setIRLongExposureCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response); std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
@@ -469,6 +483,7 @@ class OBCameraNode {
private: private:
std::atomic_bool write_customer_data_success_{false}; std::atomic_bool write_customer_data_success_{false};
std::atomic_bool user_calibration_ready_{false};
rclcpp::Node* node_ = nullptr; rclcpp::Node* node_ = nullptr;
std::shared_ptr<ob::Device> device_ = nullptr; std::shared_ptr<ob::Device> device_ = nullptr;
std::shared_ptr<Parameters> parameters_ = nullptr; std::shared_ptr<Parameters> parameters_ = nullptr;
@@ -562,8 +577,8 @@ class OBCameraNode {
rclcpp::Service<SetFilter>::SharedPtr set_filter_srv_; rclcpp::Service<SetFilter>::SharedPtr set_filter_srv_;
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_streams_enable_srv_; rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_streams_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_streams_enable_srv_; rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_streams_enable_srv_;
rclcpp::Service<GetCameraParams>::SharedPtr get_camera_params_srv_; rclcpp::Service<GetUserCalibParams>::SharedPtr get_user_calib_params_srv_;
rclcpp::Service<SetCameraParams>::SharedPtr set_camera_params_srv_; rclcpp::Service<SetUserCalibParams>::SharedPtr set_user_calib_params_srv_;
bool enable_sync_output_accel_gyro_ = false; bool enable_sync_output_accel_gyro_ = false;
bool publish_tf_ = false; bool publish_tf_ = false;
orbbec_camera/src/ob_camera_node_driver.cpp
+8 -3
View File
@@ -461,10 +461,15 @@ void OBCameraNodeDriver::deviceStatusTimer() {
status_msg.connection_type = device_info_->getConnectionType(); status_msg.connection_type = device_info_->getConnectionType();
auto camera_params = device_->getCalibrationCameraParamList(); auto camera_params = device_->getCalibrationCameraParamList();
bool calibration_from_device = (camera_params != nullptr && camera_params->count() > 0); bool calibration_from_factory = (camera_params != nullptr && camera_params->count() > 0);
status_msg.calibration_from_device = calibration_from_device; status_msg.calibration_from_factory = calibration_from_factory;
status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated(); status_msg.calibration_from_launch_param = ob_camera_node_->isParamCalibrated();
// status_msg.customer_calibration_ready = ob_camera_node_->isWriteCustomerDataSuccess();
if (!ob_camera_node_->checkUserCalibrationReady()) {
status_msg.customer_calibration_ready = false;
} else {
status_msg.customer_calibration_ready = true;
}
ob_camera_node_->publishDeviceStatus(status_msg); ob_camera_node_->publishDeviceStatus(status_msg);
// RCLCPP_INFO_STREAM(logger_, "deviceStatusTimer() "); // RCLCPP_INFO_STREAM(logger_, "deviceStatusTimer() ");
orbbec_camera/src/ros_service.cpp
+132 -140
View File
@@ -226,36 +226,32 @@ void OBCameraNode::setupCameraCtrlServices() {
}); });
write_customerdata_srv_ = node_->create_service<SetString>( write_customerdata_srv_ = node_->create_service<SetString>(
"write_customer_data", [this](const std::shared_ptr<SetString::Request> request, "write_customer_data", [this](const std::shared_ptr<SetString::Request> request,
std::shared_ptr<SetString::Response> response) { std::shared_ptr<SetString::Response> response) {
writeCustomerDataCallback(request, response); writeCustomerDataCallback(request, response);
}); });
read_customerdata_srv_ = node_->create_service<GetString>( read_customerdata_srv_ = node_->create_service<GetString>(
"read_customer_data", [this](const std::shared_ptr<GetString::Request> request, "read_customer_data", [this](const std::shared_ptr<GetString::Request> request,
std::shared_ptr<GetString::Response> response) { std::shared_ptr<GetString::Response> response) {
readCustomerDataCallback(request, response); readCustomerDataCallback(request, response);
}); });
set_camera_params_srv_ = node_->create_service<SetCameraParams>( set_user_calib_params_srv_ = node_->create_service<SetUserCalibParams>(
"set_camera_params", "set_user_calib_params", [this](const std::shared_ptr<SetUserCalibParams::Request> request,
[this](const std::shared_ptr<SetCameraParams::Request> request, std::shared_ptr<SetUserCalibParams::Response> response) {
std::shared_ptr<SetCameraParams::Response> response) { setUserCalibParamsCallback(request, response);
setCameraParamsCallback(request, response);
}); });
get_camera_params_srv_ = node_->create_service<GetCameraParams>( get_user_calib_params_srv_ = node_->create_service<GetUserCalibParams>(
"get_camera_params", "get_user_calib_params", [this](const std::shared_ptr<GetUserCalibParams::Request> request,
[this](const std::shared_ptr<GetCameraParams::Request> request, std::shared_ptr<GetUserCalibParams::Response> response) {
std::shared_ptr<GetCameraParams::Response> response) { getUserCalibParamsCallback(request, response);
getCameraParamsCallback(request, response);
}); });
set_streams_enable_srv_ = node_->create_service<SetBool>( set_streams_enable_srv_ = node_->create_service<SetBool>(
"set_streams_enable", "set_streams_enable", [this](const std::shared_ptr<SetBool::Request> request,
[this](const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response) {
std::shared_ptr<SetBool::Response> response) {
setStreamsEnableCallback(request, response); setStreamsEnableCallback(request, response);
}); });
get_streams_enable_srv_ = node_->create_service<GetBool>( get_streams_enable_srv_ = node_->create_service<GetBool>(
"get_streams_enable", "get_streams_enable", [this](const std::shared_ptr<GetBool::Request> request,
[this](const std::shared_ptr<GetBool::Request> request, std::shared_ptr<GetBool::Response> response) {
std::shared_ptr<GetBool::Response> response) {
getStreamsEnableCallback(request, response); getStreamsEnableCallback(request, response);
}); });
} }
@@ -1204,145 +1200,141 @@ void OBCameraNode::sendSoftwareTriggerCallback(
} }
} }
bool OBCameraNode::writeCustomerData(const std::string &data) { bool OBCameraNode::writeCustomerData(const std::string& data) {
if (data.empty()) return false; if (data.empty()) return false;
std::string md5_value = calcMD5(data); std::string md5_value = calcMD5(data);
uint32_t data_len_net = htonl(static_cast<uint32_t>(data.size())); uint32_t data_len_net = htonl(static_cast<uint32_t>(data.size()));
std::string len_bytes(reinterpret_cast<char*>(&data_len_net), sizeof(data_len_net)); std::string len_bytes(reinterpret_cast<char*>(&data_len_net), sizeof(data_len_net));
std::string write_buffer = len_bytes + md5_value + data; std::string write_buffer = len_bytes + md5_value + data;
device_->writeCustomerData(write_buffer.c_str(), write_buffer.size()); device_->writeCustomerData(write_buffer.c_str(), write_buffer.size());
std::vector<uint8_t> read_buffer(write_buffer.size() + 8); std::vector<uint8_t> read_buffer(write_buffer.size() + 8);
uint32_t read_len = 0; uint32_t read_len = 0;
device_->readCustomerData(read_buffer.data(), &read_len); device_->readCustomerData(read_buffer.data(), &read_len);
if (read_len < sizeof(uint32_t) + 32) return false; if (read_len < sizeof(uint32_t) + 32) return false;
uint32_t read_data_len_net = 0; uint32_t read_data_len_net = 0;
memcpy(&read_data_len_net, read_buffer.data(), sizeof(uint32_t)); memcpy(&read_data_len_net, read_buffer.data(), sizeof(uint32_t));
uint32_t read_data_len = ntohl(read_data_len_net); uint32_t read_data_len = ntohl(read_data_len_net);
std::string md5_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t)), 32); std::string md5_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t)), 32);
std::string data_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t) + 32), std::string data_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t) + 32),
read_data_len); read_data_len);
return calcMD5(data_read) == md5_read; return calcMD5(data_read) == md5_read;
} }
bool OBCameraNode::readCustomerData(std::string &out_data) { bool OBCameraNode::readCustomerData(std::string& out_data) {
std::vector<uint8_t> read_buffer(40960); std::vector<uint8_t> read_buffer(40960);
uint32_t read_len = 0; uint32_t read_len = 0;
device_->readCustomerData(read_buffer.data(), &read_len); device_->readCustomerData(read_buffer.data(), &read_len);
if (read_len < sizeof(uint32_t) + 32) { if (read_len < sizeof(uint32_t) + 32) {
return false; return false;
} }
uint32_t read_data_len_net = 0; uint32_t read_data_len_net = 0;
memcpy(&read_data_len_net, read_buffer.data(), sizeof(uint32_t)); memcpy(&read_data_len_net, read_buffer.data(), sizeof(uint32_t));
uint32_t read_data_len = ntohl(read_data_len_net); uint32_t read_data_len = ntohl(read_data_len_net);
std::string md5_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t)), 32); std::string md5_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t)), 32);
std::string data_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t) + 32), std::string data_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t) + 32),
read_data_len); read_data_len);
if (calcMD5(data_read) == md5_read) { if (calcMD5(data_read) == md5_read) {
out_data = std::move(data_read); out_data = std::move(data_read);
return true; return true;
} else {
return false;
}
}
void OBCameraNode::writeCustomerDataCallback(const std::shared_ptr<SetString::Request>& request,
std::shared_ptr<SetString::Response>& response) {
if (request->data.empty()) {
response->success = false;
response->message = "data empty";
return;
}
try {
if (writeCustomerData(request->data)) {
write_customer_data_success_ = true;
response->success = true;
response->message = "write and verify success";
user_calibration_ready_ = true;
} else { } else {
return false; write_customer_data_success_ = false;
response->success = false;
response->message = "write failed: MD5 mismatch or read too short";
user_calibration_ready_ = false;
} }
} catch (...) {
response->success = false;
response->message = "write exception";
}
} }
void OBCameraNode::writeCustomerDataCallback( void OBCameraNode::readCustomerDataCallback(const std::shared_ptr<GetString::Request>& request,
const std::shared_ptr<SetString::Request>& request, std::shared_ptr<GetString::Response>& response) {
std::shared_ptr<SetString::Response>& response) { (void)request;
try {
if (request->data.empty()) { std::string data;
response->success = false; if (readCustomerData(data)) {
response->message = "data empty"; response->success = true;
return; response->data = std::move(data);
} response->message = "read success";
} else {
try { response->success = false;
if (writeCustomerData(request->data)) { response->message = "read failed: MD5 mismatch or data too short";
write_customer_data_success_ = true;
response->success = true;
response->message = "write and verify success";
} else {
write_customer_data_success_ = false;
response->success = false;
response->message = "write failed: MD5 mismatch or read too short";
}
} catch (...) {
response->success = false;
response->message = "write exception";
} }
} catch (...) {
response->success = false;
response->message = "read exception";
}
} }
void OBCameraNode::setUserCalibParamsCallback(
void OBCameraNode::readCustomerDataCallback( const std::shared_ptr<SetUserCalibParams::Request>& request,
const std::shared_ptr<GetString::Request>& request, std::shared_ptr<SetUserCalibParams::Response>& response) {
std::shared_ptr<GetString::Response>& response) { try {
std::ostringstream ss;
(void)request; for (const auto& v : request->k) ss << v << " ";
try { for (const auto& v : request->d) ss << v << " ";
std::string data; for (const auto& v : request->rotation) ss << v << " ";
if (readCustomerData(data)) { for (const auto& v : request->translation) ss << v << " ";
response->success = true; std::string serialized_data = ss.str();
response->data = std::move(data); if (writeCustomerData(serialized_data)) {
response->message = "read success"; response->success = true;
} else { response->message = "write and verify success";
response->success = false; } else {
response->message = "read failed: MD5 mismatch or data too short"; response->success = false;
} response->message = "write failed";
} catch (...) {
response->success = false;
response->message = "read exception";
} }
} catch (...) {
response->success = false;
response->message = "exception occurred";
}
} }
void OBCameraNode::setCameraParamsCallback( void OBCameraNode::getUserCalibParamsCallback(
const std::shared_ptr<SetCameraParams::Request>& request, const std::shared_ptr<GetUserCalibParams::Request>& request,
std::shared_ptr<SetCameraParams::Response>& response) std::shared_ptr<GetUserCalibParams::Response>& response) {
{ (void)request;
try { try {
std::ostringstream ss; std::string data_read;
for (const auto &v : request->k) ss << v << " "; if (!readCustomerData(data_read)) {
for (const auto &v : request->d) ss << v << " "; response->success = false;
for (const auto &v : request->rotation) ss << v << " "; response->message = "read failed";
for (const auto &v : request->translation) ss << v << " "; return;
std::string serialized_data = ss.str();
if (writeCustomerData(serialized_data)) {
response->success = true;
response->message = "write and verify success";
} else {
response->success = false;
response->message = "write failed";
}
} catch (...) {
response->success = false;
response->message = "exception occurred";
}
}
void OBCameraNode::getCameraParamsCallback(
const std::shared_ptr<GetCameraParams::Request>& request,
std::shared_ptr<GetCameraParams::Response>& response)
{
(void)request;
try {
std::string data_read;
if (!readCustomerData(data_read)) {
response->success = false;
response->message = "read failed";
return;
}
std::istringstream ss(data_read);
for (size_t i = 0; i < 9; ++i) ss >> response->k[i];
for (size_t i = 0; i < 5; ++i) ss >> response->d[i];
for (size_t i = 0; i < 9; ++i) ss >> response->rotation[i];
for (size_t i = 0; i < 3; ++i) ss >> response->translation[i];
response->success = true;
response->message = "read success";
} catch (...) {
response->success = false;
response->message = "exception occurred";
} }
std::istringstream ss(data_read);
for (size_t i = 0; i < 9; ++i) ss >> response->k[i];
for (size_t i = 0; i < 8; ++i) ss >> response->d[i];
for (size_t i = 0; i < 9; ++i) ss >> response->rotation[i];
for (size_t i = 0; i < 3; ++i) ss >> response->translation[i];
response->success = true;
response->message = "read success";
} catch (...) {
response->success = false;
response->message = "exception occurred";
}
} }
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera_msgs/CMakeLists.txt
+2 -2
View File
@@ -31,8 +31,8 @@ rosidl_generate_interfaces(
"srv/SetInt32.srv" "srv/SetInt32.srv"
"srv/SetString.srv" "srv/SetString.srv"
"srv/SetArrays.srv" "srv/SetArrays.srv"
"srv/GetCameraParams.srv" "srv/GetUserCalibParams.srv"
"srv/SetCameraParams.srv" "srv/SetUserCalibParams.srv"
DEPENDENCIES DEPENDENCIES
sensor_msgs sensor_msgs
std_msgs std_msgs
orbbec_camera_msgs/msg/DeviceStatus.msg
+1 -1
View File
@@ -28,5 +28,5 @@ string connection_type # e.g. "USB2.0", "USB3.0", "GigE"
# --- Calibration status --- # --- Calibration status ---
bool customer_calibration_ready bool customer_calibration_ready
bool calibration_from_device bool calibration_from_factory
bool calibration_from_launch_param bool calibration_from_launch_param
orbbec_camera_msgs/srv/GetCameraParams.srv
-7
View File
@@ -1,7 +0,0 @@
---
float64[9] k
float64[5] d
float64[9] rotation
float64[3] translation
bool success
string message
orbbec_camera_msgs/srv/SetCameraParams.srv
-7
View File
@@ -1,7 +0,0 @@
float64[9] k
float64[5] d
float64[9] rotation
float64[3] translation
---
bool success
string message