Add save camera parameter services

README.MD

@@ -161,7 +161,7 @@ Here is the device support list of main branch (v1.x) and v2-main branch (v2.x):
     - [Efficient intra-process communication](#efficient-intra-process-communication)
     - [ROS2(Robot) vs Optical(Camera) Coordination Systems](#ros2robot-vs-opticalcamera-coordination-systems)
     - [Camera sensor structure](#camera-sensor-structure)
-    - [TF from coordinate A to coordinate B](#tf-from-coordinate-a-to-coordinate-b)
+    - [TF from coordinate A to coordinate B:](#tf-from-coordinate-a-to-coordinate-b)
     - [Compressed Image](#compressed-image)
     - [Building a Debian Package](#building-a-debian-package)
   - [Examples](#examples)
@@ -209,7 +209,7 @@ Install deb dependencies
 
 ```bash
 # assume you have sourced ROS environment, same blow
-sudo apt install libgflags-dev nlohmann-json3-dev libgoogle-glog-dev libgoogle-glog0v5 \
+sudo apt install libgflags-dev nlohmann-json3-dev libgoogle-glog-dev libgoogle-glog0v5 libssl-dev \
 ros-$ROS_DISTRO-image-transport ros-${ROS_DISTRO}-image-transport-plugins ros-${ROS_DISTRO}-compressed-image-transport \
 ros-$ROS_DISTRO-image-publisher ros-$ROS_DISTRO-camera-info-manager \
 ros-$ROS_DISTRO-diagnostic-updater ros-$ROS_DISTRO-diagnostic-msgs ros-$ROS_DISTRO-statistics-msgs \

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -58,6 +58,8 @@
 #include "orbbec_camera_msgs/srv/set_string.hpp"
 #include "orbbec_camera_msgs/srv/set_filter.hpp"
 #include "orbbec_camera_msgs/srv/set_arrays.hpp"
+#include "orbbec_camera_msgs/srv/get_camera_params.hpp"
+#include "orbbec_camera_msgs/srv/set_camera_params.hpp"
 #include "orbbec_camera/constants.h"
 #include "orbbec_camera/dynamic_params.h"
 #include "orbbec_camera/d2c_viewer.h"
@@ -113,6 +115,8 @@ using SetBool = std_srvs::srv::SetBool;
 using GetBool = orbbec_camera_msgs::srv::GetBool;
 using SetFilter = orbbec_camera_msgs::srv::SetFilter;
 using SetArrays = orbbec_camera_msgs::srv::SetArrays;
+using SetCameraParams = orbbec_camera_msgs::srv::SetCameraParams;
+using GetCameraParams = orbbec_camera_msgs::srv::GetCameraParams;
 
 typedef std::pair<ob_stream_type, int> stream_index_pair;
 
@@ -369,11 +373,21 @@ class OBCameraNode {
   void switchIRCameraCallback(const std::shared_ptr<SetString::Request>& request,
                               std::shared_ptr<SetString::Response>& response);
 
-  void setWriteCustomerData(const std::shared_ptr<SetString::Request>& request,
+  bool writeCustomerData(const std::string &data);
+
+  bool readCustomerData(std::string &out_data);
+
+  void writeCustomerDataCallback(const std::shared_ptr<SetString::Request>& request,
                                  std::shared_ptr<SetString::Response>& response);
 
-  void setReadCustomerData(const std::shared_ptr<SetString::Request>& request,
+  void readCustomerDataCallback(const std::shared_ptr<GetString::Request>& request,
-                           std::shared_ptr<SetString::Response>& response);
+                                std::shared_ptr<GetString::Response>& response);
+
+void getCameraParamsCallback(const std::shared_ptr<GetCameraParams::Request>& request,
+                                          std::shared_ptr<GetCameraParams::Response>& response);
+
+void setCameraParamsCallback(const std::shared_ptr<SetCameraParams::Request>& request,
+                                          std::shared_ptr<SetCameraParams::Response>& response);
 
   void setIRLongExposureCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                                  std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
@@ -449,7 +463,10 @@ class OBCameraNode {
 
   void setDisparitySearchOffset();
 
+  bool isWriteCustomerDataSuccess() const;
+
  private:
+  std::atomic_bool write_customer_data_success_{false};
   rclcpp::Node* node_ = nullptr;
   std::shared_ptr<ob::Device> device_ = nullptr;
   std::shared_ptr<Parameters> parameters_ = nullptr;
@@ -520,8 +537,8 @@ class OBCameraNode {
   rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
   rclcpp::Service<GetString>::SharedPtr get_sdk_version_srv_;
   rclcpp::Service<SetString>::SharedPtr switch_ir_camera_srv_;
-  rclcpp::Service<SetString>::SharedPtr set_write_customerdata_srv_;
+  rclcpp::Service<SetString>::SharedPtr write_customerdata_srv_;
-  rclcpp::Service<SetString>::SharedPtr set_read_customerdata_srv_;
+  rclcpp::Service<GetString>::SharedPtr read_customerdata_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ir_long_exposure_srv_;
   std::map<stream_index_pair, rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr>
       set_auto_exposure_srv_;
@@ -543,6 +560,8 @@ class OBCameraNode {
   rclcpp::Service<SetFilter>::SharedPtr set_filter_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<GetCameraParams>::SharedPtr get_camera_params_srv_;
+  rclcpp::Service<SetCameraParams>::SharedPtr set_camera_params_srv_;
 
   bool enable_sync_output_accel_gyro_ = false;
   bool publish_tf_ = false;

orbbec_camera/include/orbbec_camera/utils.h

@@ -27,6 +27,11 @@
 #include "magic_enum/magic_enum.hpp"
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <opencv2/opencv.hpp>
+#include <openssl/evp.h>
+#include <sstream>
+#include <iomanip>
+#include <arpa/inet.h>
+
 
 namespace orbbec_camera {
 inline void LogFatal(const char* file, int line, const std::string& message) {
@@ -194,4 +199,6 @@ cv::Mat undistortImage(const cv::Mat& image, const OBCameraIntrinsic& intrinsic,
                        const OBCameraDistortion& distortion);
 
 std::string getDistortionModels(OBCameraDistortion distortion);
+
+std::string calcMD5(const std::string &data);
 }  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -4003,5 +4003,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
     response->success = false;
   }
 }
-
+bool OBCameraNode::isWriteCustomerDataSuccess() const {
+    return write_customer_data_success_.load();
+}
 }  // namespace orbbec_camera

orbbec_camera/src/ros_service.cpp

@@ -224,15 +224,27 @@ void OBCameraNode::setupCameraCtrlServices() {
                                       std::shared_ptr<SetBool::Response> response) {
         sendSoftwareTriggerCallback(request, response);
       });
-  set_write_customerdata_srv_ = node_->create_service<SetString>(
+  write_customerdata_srv_ = node_->create_service<SetString>(
-      "set_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) {
-        setWriteCustomerData(request, response);
+        writeCustomerDataCallback(request, response);
       });
-  set_read_customerdata_srv_ = node_->create_service<SetString>(
+  read_customerdata_srv_ = node_->create_service<GetString>(
-      "set_read_customer_data", [this](const std::shared_ptr<SetString::Request> request,
+      "read_customer_data", [this](const std::shared_ptr<GetString::Request> request,
-                                       std::shared_ptr<SetString::Response> response) {
+                                       std::shared_ptr<GetString::Response> response) {
-        setReadCustomerData(request, 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);
+      });
+  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);
       });
   set_streams_enable_srv_ = node_->create_service<SetBool>(
       "set_streams_enable",
@@ -1192,50 +1204,145 @@ void OBCameraNode::sendSoftwareTriggerCallback(
   }
 }
 
-void OBCameraNode::setWriteCustomerData(const std::shared_ptr<SetString::Request>& request,
+bool OBCameraNode::writeCustomerData(const std::string &data) {
-                                        std::shared_ptr<SetString::Response>& response) {
+    if (data.empty()) return false;
-  if (request->data.empty()) {
+
-    response->success = false;
+    std::string md5_value = calcMD5(data);
-    response->message = "set write customer data is empty";
+    uint32_t data_len_net = htonl(static_cast<uint32_t>(data.size()));
-    return;
+    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::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;
+
+    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);
+
+    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;
     }
-  try {
+    uint32_t read_data_len_net = 0;
-    device_->writeCustomerData(request->data.c_str(), request->data.size());
+    memcpy(&read_data_len_net, read_buffer.data(), sizeof(uint32_t));
-    response->message = "set write customer data is " + request->data;
+    uint32_t read_data_len = ntohl(read_data_len_net);
-    response->success = true;
+    std::string md5_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t)), 32);
-    return;
+    std::string data_read(reinterpret_cast<char*>(read_buffer.data() + sizeof(uint32_t) + 32),
-  } catch (const ob::Error& e) {
+                          read_data_len);
-    response->message = e.getMessage();
+    if (calcMD5(data_read) == md5_read) {
-    response->success = false;
+        out_data = std::move(data_read);
-  } catch (const std::exception& e) {
+        return true;
-    response->message = e.what();
+    } else {
-    response->success = false;
+        return false;
-  } catch (...) {
-    response->message = "unknown error";
-    response->success = false;
     }
 }
-void OBCameraNode::setReadCustomerData(const std::shared_ptr<SetString::Request>& request,
+
+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::vector<uint8_t> customer_date;
+        std::string data;
-    customer_date.resize(40960);
+        if (readCustomerData(data)) {
-    uint32_t customer_date_len = 0;
-    device_->readCustomerData(customer_date.data(), &customer_date_len);
-    std::string customer_date_str(customer_date.begin(), customer_date.end());
-    response->message = "read customer data is " + customer_date_str;
             response->success = true;
-    return;
+            response->data = std::move(data);
-  } catch (const ob::Error& e) {
+            response->message = "read success";
-    response->message = e.getMessage();
+        } else {
-    response->success = false;
-  } catch (const std::exception& e) {
-    response->message = e.what();
             response->success = false;
+            response->message = "read failed: MD5 mismatch or data too short";
+        }
     } catch (...) {
-    response->message = "unknown error";
         response->success = false;
+        response->message = "read exception";
+    }
+}
+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";
     }
 }
 }  // namespace orbbec_camera

orbbec_camera/src/utils.cpp

@@ -933,4 +933,21 @@ std::string getDistortionModels(OBCameraDistortion distortion) {
       return sensor_msgs::distortion_models::PLUMB_BOB;
   }
 }
+
+std::string calcMD5(const std::string &data) {
+    unsigned char digest[EVP_MAX_MD_SIZE];
+    unsigned int digest_len = 0;
+
+    EVP_MD_CTX* ctx = EVP_MD_CTX_new();
+    EVP_DigestInit_ex(ctx, EVP_md5(), nullptr);
+    EVP_DigestUpdate(ctx, data.data(), data.size());
+    EVP_DigestFinal_ex(ctx, digest, &digest_len);
+    EVP_MD_CTX_free(ctx);
+
+    std::stringstream ss;
+    ss << std::hex << std::setfill('0');
+    for (unsigned int i = 0; i < digest_len; ++i)
+        ss << std::setw(2) << (int)digest[i];
+    return ss.str();
+}
 }  // namespace orbbec_camera

orbbec_camera_msgs/CMakeLists.txt

@@ -31,6 +31,8 @@ rosidl_generate_interfaces(
     "srv/SetInt32.srv"
     "srv/SetString.srv"
     "srv/SetArrays.srv"
+    "srv/GetCameraParams.srv"
+    "srv/SetCameraParams.srv"
     DEPENDENCIES
     sensor_msgs
     std_msgs

orbbec_camera_msgs/srv/GetCameraParams.srv

@@ -0,0 +1,7 @@
+---
+float64[9] k
+float64[5] d
+float64[9] rotation
+float64[3] translation
+bool success
+string message

orbbec_camera_msgs/srv/SetCameraParams.srv

@@ -0,0 +1,7 @@
+float64[9] k
+float64[5] d
+float64[9] rotation
+float64[3] translation
+---
+bool success
+string message