Merge pull request #52 from 3watt/master

Update from main (sync)
orbbec · Oct 16, 2024 · aa64971 · aa64971
2 parents 7c60a54 + cad3f8f
commit aa64971
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 3 deletions.
diff --git a/include/orbbec_camera/ob_camera_node.h b/include/orbbec_camera/ob_camera_node.h
@@ -301,6 +301,9 @@ class OBCameraNode {
 
   bool switchIRDataSourceChannelCallback(SetStringRequest &request, SetStringResponse &response);
 
+  bool setIRLongExposureCallback(std_srvs::SetBoolRequest& request,
+                                 std_srvs::SetBoolResponse& response);
+
  private:
   ros::NodeHandle nh_;
   ros::NodeHandle nh_private_;
@@ -384,6 +387,7 @@ class OBCameraNode {
   ros::ServiceServer get_ldp_measure_distance_srv_;
 
   bool publish_tf_ = true;
+  bool publish_imu_tf_ = true;
   std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_ = nullptr;
   std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_ = nullptr;
   std::vector<geometry_msgs::TransformStamped> static_tf_msgs_;

diff --git a/src/ob_camera_node.cpp b/src/ob_camera_node.cpp
@@ -144,12 +144,13 @@ void OBCameraNode::getParameters() {
     param_name = stream_name_[stream_index] + "_rotation";
     image_rotation_[stream_index] = nh_private_.param<int>(param_name, 0);
   }
-  depth_aligned_frame_id_[DEPTH] = optical_frame_id_[COLOR];
 
+  depth_aligned_frame_id_[DEPTH] = optical_frame_id_[COLOR];
   use_hardware_time_ = nh_private_.param<bool>("use_hardware_time", true);
   publish_tf_ = nh_private_.param<bool>("publish_tf", false);
+  publish_imu_tf_ = nh_private_.param<bool>("publish_imu_tf_", false);
   depth_registration_ = nh_private_.param<bool>("depth_registration", false);
-  enable_frame_sync_ = nh_private_.param<bool>("enable_frame_sync", false);
+  enable_frame_sync_ = nh_private_.param<bool>("enable_frame_sync", true);
   ir_info_uri_ = nh_private_.param<std::string>("ir_info_uri", "");
   color_info_uri_ = nh_private_.param<std::string>("color_info_uri", "");
   enable_d2c_viewer_ = nh_private_.param<bool>("enable_d2c_viewer", false);

diff --git a/src/ros_service.cpp b/src/ros_service.cpp
@@ -815,6 +815,26 @@ bool OBCameraNode::switchIRDataSourceChannelCallback(SetStringRequest& request,
     response.message = ss.str();
     return false;
   }
-  return false;
+}
+
+bool OBCameraNode::setIRLongExposureCallback(std_srvs::SetBoolRequest& request,
+                                             std_srvs::SetBoolResponse& response) {
+  try {
+    device_->setBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL, request.data);
+    return true;
+  } catch (const ob::Error& e) {
+    std::stringstream ss;
+    ss << "Failed to set IR long exposure: " << e.getMessage();
+    ROS_ERROR_STREAM(ss.str());
+    response.message = ss.str();
+    return false;
+
+  } catch (...) {
+    std::stringstream ss;
+    ss << "Failed to set IR long exposure: unknown error";
+    ROS_ERROR_STREAM(ss.str());
+    response.message = ss.str();
+    return false;
+  }
 }
 }  // namespace orbbec_camera
diff --git a/src/utils.cpp b/src/utils.cpp
@@ -388,6 +388,15 @@ std::ostream &operator<<(std::ostream &os, const OBCameraParam &rhs) {
   os << "cy : " << rgb_intrinsic.cy << "\n";
   os << "width : " << rgb_intrinsic.width << "\n";
   os << "height : " << rgb_intrinsic.height << "\n";
+  os << "=====extrinsic=====\n";
+  for (float i : rhs.transform.rot) {
+    os << i << " ";
+  }
+  os << "\n";
+  for (float tran : rhs.transform.trans) {
+    os << tran << " ";
+  }
+  os << "\n";
   return os;
 }