Merge branch 'master' of code.orbbec.com.cn:OrbbecSDK/orbbec-ros-sdk

orbbec · Jan 9, 2024 · 54ca6bc · 54ca6bc
2 parents 0ca4b46 + d5650a5
commit 54ca6bc
Show file tree

Hide file tree

Showing 7 changed files with 165 additions and 10 deletions.
diff --git a/README.MD b/README.MD
@@ -395,6 +395,7 @@ The following are the launch parameters available:
   <!-- Unbinned Dense Default -->
   <!-- Unbinned Sparse Default -->
   <!-- Binned Sparse Default -->
+  <!-- Obstacle Avoidance -->
   <arg name="depth_work_mode" default=""/>
 ```
 
@@ -427,6 +428,7 @@ The frame fps and resolution of IR must be consistent with the depth. The corres
 | astra mini s pro                          | astra.launch         |
 | astra2                                    | astra2.launch        |
 | astra stereo s                            | stereo_s_u3.launch   |
+| astra pro2                                | astra_pro2.launch    |
 | dabai                                     | dabai.launch         |
 | dabai d1                                  | dabai_d1.launch      |
 | dabai dcw                                 | dabai_dcw.launch     |

diff --git a/README_CN.MD b/README_CN.MD
@@ -385,6 +385,7 @@ roslaunch orbbec_camera multi_camera.launch
   <!-- Unbinned Dense Default -->
   <!-- Unbinned Sparse Default -->
   <!-- Binned Sparse Default -->
+  <!-- Obstacle Avoidance -->
   <arg name="depth_work_mode" default=""/>
 ```
 
@@ -417,6 +418,7 @@ IR的分辨率和帧率必须和深度保持一致。不同模式和分辨率对
 | astra mini s pro                          | astra.launch         |
 | astra2                                    | astra2.launch        |
 | astra stereo s                            | stereo_s_u3.launch   |
+| astra pro2                                | astra_pro2.launch    |
 | dabai                                     | dabai.launch         |
 | dabai d1                                  | dabai_d1.launch      |
 | dabai dcw                                 | dabai_dcw.launch     |

diff --git a/launch/astra_pro2.launch b/launch/astra_pro2.launch
@@ -0,0 +1,86 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <arg name="output" default="screen"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="usb_port" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="enable_colored_point_cloud" default="true"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="color_width" default="640"/>
+    <arg name="color_height" default="480"/>
+    <arg name="color_fps" default="10"/>
+    <arg name="enable_color" default="true"/>
+    <arg name="flip_color" default="false"/>
+    <arg name="color_format" default="UYVY"/>
+    <arg name="enable_color_auto_exposure" default="true"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="480"/>
+    <arg name="depth_fps" default="10"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="480"/>
+    <arg name="ir_fps" default="10"/>
+    <arg name="enable_ir" default="false"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="enable_ir_auto_exposure" default="true"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_uri" default=""/>
+    <arg name="color_info_uri" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_d2c_viewer" default="false"/>
+    <arg name="enable_pipeline" default="true"/>
+    <arg name="enable_soft_filter" default="true"/>
+    <group ns="$(arg camera_name)">
+        <node name="camera" pkg="orbbec_camera" type="orbbec_camera_node" output="$(arg output)">
+            <param name="camera_name" value="$(arg camera_name)"/>
+            <param name="depth_registration" value="$(arg depth_registration)"/>
+            <param name="serial_number" type="string" value="$(arg serial_number)"/>
+            <param name="usb_port" type="string" value="$(arg usb_port)"/>
+            <param name="vendor_id" value="$(arg vendor_id)"/>
+            <param name="product_id" value="$(arg product_id)"/>
+            <param name="enable_point_cloud" value="$(arg enable_point_cloud)"/>
+            <param name="enable_colored_point_cloud" value="$(arg enable_colored_point_cloud)"/>
+            <param name="connection_delay" value="$(arg connection_delay)"/>
+            <param name="color_width" value="$(arg color_width)"/>
+            <param name="color_height" value="$(arg color_height)"/>
+            <param name="color_fps" value="$(arg color_fps)"/>
+            <param name="enable_color" value="$(arg enable_color)"/>
+            <param name="color_format" value="$(arg color_format)"/>
+            <param name="flip_color" value="$(arg flip_color)"/>
+            <param name="enable_color_auto_exposure" value="$(arg enable_color_auto_exposure)"/>
+            <param name="depth_width" value="$(arg depth_width)"/>
+            <param name="depth_height" value="$(arg depth_height)"/>
+            <param name="depth_fps" value="$(arg depth_fps)"/>
+            <param name="flip_depth" value="$(arg flip_depth)"/>
+            <param name="enable_depth" value="$(arg enable_depth)"/>
+            <param name="depth_format" value="$(arg depth_format)"/>
+            <param name="ir_width" value="$(arg ir_width)"/>
+            <param name="ir_height" value="$(arg ir_height)"/>
+            <param name="ir_fps" value="$(arg ir_fps)"/>
+            <param name="enable_ir" value="$(arg enable_ir)"/>
+            <param name="flip_ir" value="$(arg flip_ir)"/>
+            <param name="ir_format" value="$(arg ir_format)"/>
+            <param name="enable_ir_auto_exposure" value="$(arg enable_ir_auto_exposure)"/>
+            <param name="publish_tf" value="$(arg publish_tf)"/>
+            <param name="tf_publish_rate" value="$(arg tf_publish_rate)"/>
+            <param name="ir_info_uri" value="$(arg ir_info_uri)"/>
+            <param name="color_info_uri" value="$(arg color_info_uri)"/>
+            <param name="log_level" value="$(arg log_level)"/>
+            <param name="enable_d2c_viewer" value="$(arg enable_d2c_viewer)"/>
+            <param name="enable_pipeline" value="$(arg enable_pipeline)"/>
+            <param name="device_num" value="$(arg device_num)"/>
+            <param name="enable_soft_filter" value="$(arg enable_soft_filter)"/>
+            <remap from="/$(arg camera_name)/depth/color/points" to="/$(arg camera_name)/depth_registered/points"/>
+        </node>
+    </group>
+</launch>
diff --git a/launch/dabai_dw2.launch b/launch/dabai_dw2.launch
@@ -0,0 +1,67 @@
+<launch>
+    <!-- unique camera name-->
+    <arg name="camera_name" default="camera"/>
+    <arg name="output" default="screen"/>
+    <!-- Hardware depth registration -->
+    <arg name="depth_registration" default="false"/>
+    <arg name="serial_number" default=""/>
+    <arg name="usb_port" default=""/>
+    <arg name="device_num" default="1"/>
+    <arg name="vendor_id" default="0x2bc5"/>
+    <arg name="product_id" default=""/>
+    <arg name="enable_point_cloud" default="true"/>
+    <arg name="connection_delay" default="100"/>
+    <arg name="depth_width" default="640"/>
+    <arg name="depth_height" default="400"/>
+    <arg name="depth_fps" default="10"/>
+    <arg name="enable_depth" default="true"/>
+    <arg name="depth_format" default="Y11"/>
+    <arg name="flip_depth" default="false"/>
+    <arg name="ir_width" default="640"/>
+    <arg name="ir_height" default="400"/>
+    <arg name="ir_fps" default="10"/>
+    <arg name="flip_ir" default="false"/>
+    <arg name="enable_ir" default="true"/>
+    <arg name="ir_format" default="Y10"/>
+    <arg name="enable_ir_auto_exposure" default="true"/>
+    <arg name="publish_tf" default="true"/>
+    <arg name="tf_publish_rate" default="10.0"/>
+    <arg name="ir_info_uri" default=""/>
+    <arg name="color_info_uri" default=""/>
+    <arg name="log_level" default="none"/>
+    <arg name="enable_pipeline" default="true"/>
+    <arg name="enable_soft_filter" default="true"/>
+    <group ns="$(arg camera_name)">
+        <node name="camera" pkg="orbbec_camera" type="orbbec_camera_node" output="$(arg output)">
+            <param name="camera_name" value="$(arg camera_name)"/>
+            <param name="depth_registration" value="$(arg depth_registration)"/>
+            <param name="serial_number" type="string" value="$(arg serial_number)"/>
+            <param name="usb_port" type="string" value="$(arg usb_port)"/>
+            <param name="vendor_id" value="$(arg vendor_id)"/>
+            <param name="product_id" value="$(arg product_id)"/>
+            <param name="enable_point_cloud" value="$(arg enable_point_cloud)"/>
+            <param name="connection_delay" value="$(arg connection_delay)"/>
+            <param name="depth_width" value="$(arg depth_width)"/>
+            <param name="depth_height" value="$(arg depth_height)"/>
+            <param name="depth_fps" value="$(arg depth_fps)"/>
+            <param name="enable_depth" value="$(arg enable_depth)"/>
+            <param name="depth_format" value="$(arg depth_format)"/>
+            <param name="flip_depth" value="$(arg flip_depth)"/>
+            <param name="ir_width" value="$(arg ir_width)"/>
+            <param name="ir_height" value="$(arg ir_height)"/>
+            <param name="ir_fps" value="$(arg ir_fps)"/>
+            <param name="enable_ir" value="$(arg enable_ir)"/>
+            <param name="ir_format" value="$(arg ir_format)"/>
+            <param name="flip_ir" value="$(arg flip_ir)"/>
+            <param name="enable_ir_auto_exposure" value="$(arg enable_ir_auto_exposure)"/>
+            <param name="publish_tf" value="$(arg publish_tf)"/>
+            <param name="tf_publish_rate" value="$(arg tf_publish_rate)"/>
+            <param name="ir_info_uri" value="$(arg ir_info_uri)"/>
+            <param name="color_info_uri" value="$(arg color_info_uri)"/>
+            <param name="log_level" value="$(arg log_level)"/>
+            <param name="enable_pipeline" value="$(arg enable_pipeline)"/>
+            <param name="device_num" value="$(arg device_num)"/>
+            <param name="enable_soft_filter" value="$(arg enable_soft_filter)"/>
+        </node>
+    </group>
+</launch>
diff --git a/launch/gemini2.launch b/launch/gemini2.launch
@@ -62,6 +62,7 @@
     <!-- Unbinned Dense Default -->
     <!-- Unbinned Sparse Default -->
     <!-- Binned Sparse Default -->
+    <!-- Obstacle Avoidance -->
     <arg name="depth_work_mode" default=""/>
     <arg name="enable_frame_sync" default="false"/>
     <group ns="$(arg camera_name)">

diff --git a/src/ob_camera_node.cpp b/src/ob_camera_node.cpp
@@ -162,7 +162,6 @@ void OBCameraNode::getParameters() {
   soft_filter_speckle_size_ = nh_private_.param<int>("soft_filter_speckle_size", -1);
   depth_filter_config_ = nh_private_.param<std::string>("depth_filter_config", "");
   if (!depth_filter_config_.empty()) {
-    enable_soft_filter_ = false;
     enable_depth_filter_ = true;
   }
 
@@ -196,6 +195,9 @@ void OBCameraNode::startStreams() {
     if (enable_frame_sync_) {
       pipeline_->enableFrameSync();
     }
+    else {
+      pipeline_->disableFrameSync();
+    }
     try {
       setupPipelineConfig();
       pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet>& frame_set) {

diff --git a/src/ros_setup.cpp b/src/ros_setup.cpp
@@ -123,10 +123,10 @@ void OBCameraNode::setupDevices() {
     if (!depth_filter_config_.empty() && enable_depth_filter_) {
       ROS_INFO_STREAM("Load depth filter config: " << depth_filter_config_);
       device_->loadDepthFilterConfig(depth_filter_config_.c_str());
-    }
-
-    if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
-      device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
+    } else {
+      if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
+        device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
+      }
     }
 
     if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
@@ -152,11 +152,6 @@ void OBCameraNode::setupDevices() {
         device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, soft_filter_speckle_size_);
       }
     }
-
-    auto default_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
-    if (soft_filter_max_diff_ != -1 && default_soft_filter_max_diff != soft_filter_max_diff_) {
-      device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, soft_filter_max_diff_);
-    }
   } catch (const ob::Error& e) {
     ROS_ERROR_STREAM("Failed to setup devices: " << e.getMessage());
   } catch (const std::exception& e) {