mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-04 03:57:46 +08:00
Update user calibration service interface and device status reporting
This commit is contained in:
xiexun
@@ -461,10 +461,15 @@ void OBCameraNodeDriver::deviceStatusTimer() {
|
||||
status_msg.connection_type = device_info_->getConnectionType();
|
||||
|
||||
auto camera_params = device_->getCalibrationCameraParamList();
|
||||
bool calibration_from_device = (camera_params != nullptr && camera_params->count() > 0);
|
||||
status_msg.calibration_from_device = calibration_from_device;
|
||||
bool calibration_from_factory = (camera_params != nullptr && camera_params->count() > 0);
|
||||
status_msg.calibration_from_factory = calibration_from_factory;
|
||||
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);
|
||||
// RCLCPP_INFO_STREAM(logger_, "deviceStatusTimer() ");
|
||||
|
||||
+132
-140
@@ -226,36 +226,32 @@ void OBCameraNode::setupCameraCtrlServices() {
|
||||
});
|
||||
write_customerdata_srv_ = node_->create_service<SetString>(
|
||||
"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);
|
||||
});
|
||||
read_customerdata_srv_ = node_->create_service<GetString>(
|
||||
"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);
|
||||
});
|
||||
set_camera_params_srv_ = node_->create_service<SetCameraParams>(
|
||||
"set_camera_params",
|
||||
[this](const std::shared_ptr<SetCameraParams::Request> request,
|
||||
std::shared_ptr<SetCameraParams::Response> response) {
|
||||
setCameraParamsCallback(request, response);
|
||||
set_user_calib_params_srv_ = node_->create_service<SetUserCalibParams>(
|
||||
"set_user_calib_params", [this](const std::shared_ptr<SetUserCalibParams::Request> request,
|
||||
std::shared_ptr<SetUserCalibParams::Response> response) {
|
||||
setUserCalibParamsCallback(request, response);
|
||||
});
|
||||
get_camera_params_srv_ = node_->create_service<GetCameraParams>(
|
||||
"get_camera_params",
|
||||
[this](const std::shared_ptr<GetCameraParams::Request> request,
|
||||
std::shared_ptr<GetCameraParams::Response> response) {
|
||||
getCameraParamsCallback(request, response);
|
||||
get_user_calib_params_srv_ = node_->create_service<GetUserCalibParams>(
|
||||
"get_user_calib_params", [this](const std::shared_ptr<GetUserCalibParams::Request> request,
|
||||
std::shared_ptr<GetUserCalibParams::Response> response) {
|
||||
getUserCalibParamsCallback(request, response);
|
||||
});
|
||||
set_streams_enable_srv_ = node_->create_service<SetBool>(
|
||||
"set_streams_enable",
|
||||
[this](const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
"set_streams_enable", [this](const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setStreamsEnableCallback(request, response);
|
||||
});
|
||||
get_streams_enable_srv_ = node_->create_service<GetBool>(
|
||||
"get_streams_enable",
|
||||
[this](const std::shared_ptr<GetBool::Request> request,
|
||||
std::shared_ptr<GetBool::Response> response) {
|
||||
"get_streams_enable", [this](const std::shared_ptr<GetBool::Request> request,
|
||||
std::shared_ptr<GetBool::Response> response) {
|
||||
getStreamsEnableCallback(request, response);
|
||||
});
|
||||
}
|
||||
@@ -1204,145 +1200,141 @@ void OBCameraNode::sendSoftwareTriggerCallback(
|
||||
}
|
||||
}
|
||||
|
||||
bool OBCameraNode::writeCustomerData(const std::string &data) {
|
||||
if (data.empty()) return false;
|
||||
bool OBCameraNode::writeCustomerData(const std::string& data) {
|
||||
if (data.empty()) return false;
|
||||
|
||||
std::string md5_value = calcMD5(data);
|
||||
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 md5_value = calcMD5(data);
|
||||
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 write_buffer = len_bytes + md5_value + data;
|
||||
device_->writeCustomerData(write_buffer.c_str(), write_buffer.size());
|
||||
std::string write_buffer = len_bytes + md5_value + data;
|
||||
device_->writeCustomerData(write_buffer.c_str(), write_buffer.size());
|
||||
|
||||
std::vector<uint8_t> read_buffer(write_buffer.size() + 8);
|
||||
uint32_t read_len = 0;
|
||||
device_->readCustomerData(read_buffer.data(), &read_len);
|
||||
std::vector<uint8_t> read_buffer(write_buffer.size() + 8);
|
||||
uint32_t read_len = 0;
|
||||
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;
|
||||
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_net = 0;
|
||||
memcpy(&read_data_len_net, read_buffer.data(), sizeof(uint32_t));
|
||||
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 data_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t) + 32),
|
||||
read_data_len);
|
||||
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),
|
||||
read_data_len);
|
||||
|
||||
return calcMD5(data_read) == md5_read;
|
||||
return calcMD5(data_read) == md5_read;
|
||||
}
|
||||
|
||||
bool OBCameraNode::readCustomerData(std::string &out_data) {
|
||||
std::vector<uint8_t> read_buffer(40960);
|
||||
uint32_t read_len = 0;
|
||||
device_->readCustomerData(read_buffer.data(), &read_len);
|
||||
if (read_len < sizeof(uint32_t) + 32) {
|
||||
return false;
|
||||
}
|
||||
uint32_t read_data_len_net = 0;
|
||||
memcpy(&read_data_len_net, read_buffer.data(), sizeof(uint32_t));
|
||||
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 data_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t) + 32),
|
||||
read_data_len);
|
||||
if (calcMD5(data_read) == md5_read) {
|
||||
out_data = std::move(data_read);
|
||||
return true;
|
||||
bool OBCameraNode::readCustomerData(std::string& out_data) {
|
||||
std::vector<uint8_t> read_buffer(40960);
|
||||
uint32_t read_len = 0;
|
||||
device_->readCustomerData(read_buffer.data(), &read_len);
|
||||
if (read_len < sizeof(uint32_t) + 32) {
|
||||
return false;
|
||||
}
|
||||
uint32_t read_data_len_net = 0;
|
||||
memcpy(&read_data_len_net, read_buffer.data(), sizeof(uint32_t));
|
||||
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 data_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t) + 32),
|
||||
read_data_len);
|
||||
if (calcMD5(data_read) == md5_read) {
|
||||
out_data = std::move(data_read);
|
||||
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 {
|
||||
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(
|
||||
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";
|
||||
} 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";
|
||||
void OBCameraNode::readCustomerDataCallback(const std::shared_ptr<GetString::Request>& request,
|
||||
std::shared_ptr<GetString::Response>& response) {
|
||||
(void)request;
|
||||
try {
|
||||
std::string data;
|
||||
if (readCustomerData(data)) {
|
||||
response->success = true;
|
||||
response->data = std::move(data);
|
||||
response->message = "read success";
|
||||
} else {
|
||||
response->success = false;
|
||||
response->message = "read failed: MD5 mismatch or data too short";
|
||||
}
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "read exception";
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::readCustomerDataCallback(
|
||||
const std::shared_ptr<GetString::Request>& request,
|
||||
std::shared_ptr<GetString::Response>& response) {
|
||||
|
||||
(void)request;
|
||||
try {
|
||||
std::string data;
|
||||
if (readCustomerData(data)) {
|
||||
response->success = true;
|
||||
response->data = std::move(data);
|
||||
response->message = "read success";
|
||||
} else {
|
||||
response->success = false;
|
||||
response->message = "read failed: MD5 mismatch or data too short";
|
||||
}
|
||||
} catch (...) {
|
||||
response->success = false;
|
||||
response->message = "read exception";
|
||||
void OBCameraNode::setUserCalibParamsCallback(
|
||||
const std::shared_ptr<SetUserCalibParams::Request>& request,
|
||||
std::shared_ptr<SetUserCalibParams::Response>& response) {
|
||||
try {
|
||||
std::ostringstream ss;
|
||||
for (const auto& v : request->k) ss << v << " ";
|
||||
for (const auto& v : request->d) ss << v << " ";
|
||||
for (const auto& v : request->rotation) ss << v << " ";
|
||||
for (const auto& v : request->translation) ss << v << " ";
|
||||
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::setCameraParamsCallback(
|
||||
const std::shared_ptr<SetCameraParams::Request>& request,
|
||||
std::shared_ptr<SetCameraParams::Response>& response)
|
||||
{
|
||||
try {
|
||||
std::ostringstream ss;
|
||||
for (const auto &v : request->k) ss << v << " ";
|
||||
for (const auto &v : request->d) ss << v << " ";
|
||||
for (const auto &v : request->rotation) ss << v << " ";
|
||||
for (const auto &v : request->translation) ss << v << " ";
|
||||
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";
|
||||
void OBCameraNode::getUserCalibParamsCallback(
|
||||
const std::shared_ptr<GetUserCalibParams::Request>& request,
|
||||
std::shared_ptr<GetUserCalibParams::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 < 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
|
||||
|
||||
Reference in New Issue
Block a user