wls filter and params for oak

biodigitalmatter · Aug 21, 2024 · b5f468f · b5f468f
1 parent b1b460d
commit b5f468f
Show file tree

Hide file tree

Showing 3 changed files with 64 additions and 15 deletions.
diff --git a/config/OAK-D.yaml b/config/OAK-D.yaml
@@ -0,0 +1,5 @@
+/rgbd_camera:
+  camera_i_nn_type: none
+  stereo_i_subpixel: true
+  left_i_publish_topic: true
+  stereo_i_output_disparity: true
diff --git a/launch/rgbd.launch b/launch/rgbd.launch
@@ -22,6 +22,12 @@
     <arg name="no_record" default="false" />
     <arg name="no_mapping" default="false" />
 
+    <param name="$(arg name)/camera_i_ip" value="192.168.8.10"
+        if="$(eval
+    rgbd_oak_model == 'OAK-D-POE'" />
+    <param name="$(arg name)/camera_i_mx_id" value="18443010C10B361300"
+        if="$(eval rgbd_oak_model == 'OAK-D-PRO'" />
+
     <include file="$(find depthai_ros_driver)/launch/camera.launch"
         if="$(arg use_oak)">
         <arg name="name" value="$(arg name)" />
@@ -34,7 +40,7 @@
         <arg name="cam_yaw" value="$(arg rgbd_yaw)" />
         <arg name="camera_model" value="$(arg rgbd_oak_model)" />
         <arg name="params_file"
-            value="$(find biodigitalmatter_ros)/config/$(arg rgbd_oak_model).yaml"
+            value="$(find biodigitalmatter_ros)/config/OAK-D.yaml"
         />
     </include>
 

diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch
@@ -4,45 +4,83 @@
 
     <arg name="name" default="rgbd_camera" />
     <arg name="rectify_rgb" default="true" />
-    <arg name="no_record" default="false" />
+    <arg
+        name="no_record" default="false" />
+    <arg name="use_wls_filter" default="true" />
 
-    <node pkg="nodelet" type="nodelet" name="rectify_color"
-        args="load image_proc/rectify $(arg  name)_nodelet_manager" if="$(arg rectify_rgb)">
+    <node
+        pkg="nodelet" type="nodelet" name="rectify_color"
+        args="load image_proc/rectify $(arg  name)_nodelet_manager" if="$(arg
+    rectify_rgb)"
+        required="true">
         <remap from="image_mono" to="$(arg name)/rgb/image_raw" />
         <remap from="image_rect" to="$(arg name)/rgb/image_rect" />
     </node>
 
-    <node pkg="nodelet" type="nodelet" name="depth_image_to_rgb_pointcloud"
-        args="load depth_image_proc/point_cloud_xyzrgb $(arg  name)_nodelet_manager">
+    <node pkg="nodelet"
+        type="nodelet" name="rectify_left"
+        args="load image_proc/rectify $(arg  name)_nodelet_manager">
+        <remap from="image_mono" to="$(arg name)/left/image_raw" />
+        <remap from="image_rect" to="$(arg name)/left/image_rect" />
+    </node>
+
+    <!-- TODO: Why is this not needed? -->
+    <!-- node pkg="nodelet" type="nodelet" name="rectify_stereo"
+        args="load image_proc/rectify $(arg  name)_nodelet_manager">
+        <remap from="image_mono" to="$(arg name)/stero/image_raw" />
+        <remap from="image_rect" to="$(arg name)/stereo/image_rect" />
+    </node -->
+
+
+    <node name="wls"
+        pkg="nodelet" type="nodelet" output="screen"
+        required="true" if="$(arg use_wls_filter)" /> args="load depthai_filters/WLSFilter $(arg
+    name)_nodelet_manager"> <remap from="/stereo/image_raw" to="$(arg name)/stereo/image_raw" />
+        <remap
+        from="/stereo/left/rect" to="$(arg name)/left/image_rect" />
+        <remap
+        from="/stereo/camera_info" to="$(arg name)/stereo/camera_info" />
+        <remap
+        from="/stereo/wls/filtered" to="$(arg name)/stereo/wls/filtered" />
+    </node>
+
+    <node
+        pkg="nodelet" type="nodelet" name="depth_image_to_rgb_pointcloud"
+        args="load depth_image_proc/point_cloud_xyzrgb $(arg  name)_nodelet_manager"
+        required="true">
         <param name="queue_size" value="10" />
         <remap from="rgb/camera_info" to="$(arg name)/rgb/camera_info" />
+
         <remap from="rgb/image_rect_color" to="$(arg name)/rgb/image_rect" if="$(arg rectify_rgb)" />
         <remap from="rgb/image_rect_color" to="$(arg name)/rgb/image_raw"
             unless="$(arg rectify_rgb)" />
-        <remap from="depth_registered/image_rect" to="$(arg name)/stereo/image_raw" />
-        <remap from="depth_registered/points" to="$(arg name)/points" />
 
+        <remap from="depth_registered/image_rect" to="$(arg name)/stereo/wls/filtered"
+            if="$(arg use_wls_filter)" />
+        <remap from="depth_registered/image_rect" to="$(arg name)/stereo/image_raw"
+            unless="$(arg use_wls_filter)" />
+
+        <remap from="depth_registered/points" to="$(arg name)/points" />
     </node>
 
-    <node name="store_pointclouds" pkg="pcl_ros" type="pointcloud_to_pcd"
+    <node
+        name="store_pointclouds" pkg="pcl_ros" type="pointcloud_to_pcd"
         unless="$(arg no_record)">
         <remap from="input" to="/points" />
         <param name="fixed_frame" value="world" />
         <param name="prefix" value="$(env HOME)/pcds/" />
     </node>
 
-    <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
+    <node pkg="octomap_server"
+        type="octomap_server_node" name="octomap_server">
         <param name="resolution" value="0.005" />
-
-        <!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
         <param name="frame_id" type="string" value="world" />
-        <param name="filter_ground" type="bool" value="true" />
+        <param name="filter_ground" type="bool" value="map" />
 
         <!-- maximum range to integrate (speedup!) -->
-        <param name="sensor_model/max_range" value="5.0" />
+        <param name="sensor_model/max_range" value="2.0" />
 
         <!-- data source to integrate (PointCloud2) -->
         <remap from="cloud_in" to="$(arg name)/points" />
-
     </node>
 </launch>