add help scripts

orbbec_camera/scripts/echo_extrintisics.py

@@ -0,0 +1,33 @@
+import imp
+from orbbec_camera_msgs.msg import Extrinsics
+from rclpy.node import Node
+from rclpy.qos import qos_profile_system_default
+import rclpy
+
+
+class EchoExtrinsics(Node):
+    def __init__(self):
+        super().__init__("echo_extrinsics")
+        self.subscription = self.create_subscription(
+            Extrinsics,
+            "/camera/extrinsics",
+            self.callback,
+            qos_profile_system_default
+        )
+
+    def callback(self, msg: Extrinsics):
+        self.get_logger().info("=====rotation====")
+        for r in msg.rotation:
+            print("%s ", r)
+
+        self.get_logger().info("=====translation====")
+        for t in msg.translation:
+            print("%s ", t)
+
+
+def main(args=None):
+    rclpy.init()
+
+
+if __name__ == "__main__":
+    main()

orbbec_camera/src/device_ctrl_service.cpp

@@ -9,7 +9,7 @@ void OBCameraNode::setupCameraCtrlServices() {
   for (auto stream_index : IMAGE_STREAMS) {
     auto stream_name = stream_name_[stream_index.first];
     if (enable_[stream_index]) {
-      std::string service_name = "/get/" + stream_name + "/exposure";
+      std::string service_name = "get/" + stream_name + "/exposure";
       get_exposure_srv_[stream_index] = node_->create_service<GetInt32>(
           service_name, [this, stream_index = stream_index](
                             const std::shared_ptr<rmw_request_id_t> request_header,
@@ -18,7 +18,7 @@ void OBCameraNode::setupCameraCtrlServices() {
             getExposureCallback(request, response, stream_index);
           });
 
-      service_name = "/set/" + stream_name + "/exposure";
+      service_name = "set/" + stream_name + "/exposure";
       set_exposure_srv_[stream_index] = node_->create_service<SetInt32>(
           service_name, [this, stream_index = stream_index](
                             const std::shared_ptr<rmw_request_id_t> request_header,
@@ -26,7 +26,7 @@ void OBCameraNode::setupCameraCtrlServices() {
                             std::shared_ptr<SetInt32::Response> response) {
             setExposureCallback(request, response, stream_index);
           });
-      service_name = "/get/" + stream_name + "/gain";
+      service_name = "get/" + stream_name + "/gain";
       get_gain_srv_[stream_index] = node_->create_service<GetInt32>(
           service_name, [this, stream_index = stream_index](
                             const std::shared_ptr<rmw_request_id_t> request_header,
@@ -44,7 +44,7 @@ void OBCameraNode::setupCameraCtrlServices() {
             setGainCallback(request, response, stream_index);
           });
       if (stream_index.first == OB_STREAM_COLOR || stream_index.first == OB_STREAM_DEPTH) {
-        service_name = "/set/" + stream_name +
+        service_name = "set/" + stream_name +
                        "/"
                        "auto_exposure";
         set_auto_exposure_srv_[stream_index] = node_->create_service<SetBool>(
@@ -58,39 +58,39 @@ void OBCameraNode::setupCameraCtrlServices() {
     }
   }
   set_fan_mode_srv_ = node_->create_service<SetInt32>(
-      "/set_fan_mode", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+      "set_fan_mode", [this](const std::shared_ptr<rmw_request_id_t> request_header,
                               const std::shared_ptr<SetInt32::Request> request,
                               std::shared_ptr<SetInt32::Response> response) {
         setFanModeCallback(request_header, request, response);
       });
   set_floor_enable_srv_ = node_->create_service<SetBool>(
-      "/set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+      "set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
                                   const std::shared_ptr<SetBool::Request> request,
                                   std::shared_ptr<SetBool::Response> response) {
         setFloorEnableCallback(request_header, request, response);
       });
   set_laser_enable_srv_ = node_->create_service<SetBool>(
-      "/set_laser_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+      "set_laser_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
                                   const std::shared_ptr<SetBool::Request> request,
                                   std::shared_ptr<SetBool::Response> response) {
         setLaserEnableCallback(request_header, request, response);
       });
   set_ldp_enable_srv_ = node_->create_service<SetBool>(
-      "/set_ldp_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+      "set_ldp_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
                                 const std::shared_ptr<SetBool::Request> request,
                                 std::shared_ptr<SetBool::Response> response) {
         setLdpEnableCallback(request_header, request, response);
       });
 
   get_white_balance_srv_ = node_->create_service<GetInt32>(
-      "/get/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+      "get/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
                                    const std::shared_ptr<GetInt32::Request> request,
                                    std::shared_ptr<GetInt32::Response> response) {
         getWhiteBalanceCallback(request_header, request, response);
       });
 
   set_white_balance_srv_ = node_->create_service<SetInt32>(
-      "/set/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+      "set/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
                                    const std::shared_ptr<SetInt32::Request> request,
                                    std::shared_ptr<SetInt32::Response> response) {
         setWhiteBalanceCallback(request_header, request, response);
@@ -136,6 +136,7 @@ void OBCameraNode::getGainCallback(const std::shared_ptr<GetInt32::Request>& req
       break;
   }
 }
+
 void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& request,
                                    std::shared_ptr<SetInt32::Response>& response,
                                    const stream_index_pair& stream_index) {

orbbec_camera/src/ob_camera_node.cpp

@@ -209,7 +209,6 @@ void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set,
   }
   if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
     publishColorPointCloud(frame_set, t);
-    // publishDepthPointCloud(frame_set, t);
   }
   //  } else if (frame_set->depthFrame() != nullptr) {
   //    publishDepthPointCloud(frame_set, t);
@@ -222,6 +221,7 @@ void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_se
   auto frame = point_cloud_filter_.process(frame_set);
   size_t point_size = frame->dataSize() / sizeof(OBPoint);
   auto* points = (OBPoint*)frame->data();
+  CHECK_NOTNULL(points);
   sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
   modifier.setPointCloud2FieldsByString(1, "xyz");
   modifier.resize(point_size);
@@ -262,6 +262,7 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
   auto frame = point_cloud_filter_.process(frame_set);
   size_t point_size = frame->dataSize() / sizeof(OBColorPoint);
   auto* points = (OBColorPoint*)frame->data();
+  CHECK_NOTNULL(points);
   sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
   modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
   modifier.resize(point_size);