From 76605f1262a648c7e7ee754bda7c0150253407d8 Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Wed, 25 Aug 2021 15:36:00 +0200
Subject: [PATCH] Add sensor config files for perception tutorial

https://ros-planning.github.io/moveit_tutorials/doc/perception_pipeline/perception_pipeline_tutorial.html
---
 config/sensors_3d.yaml                |  2 --
 config/sensors_kinect_depthmap.yaml   | 11 +++++++++++
 config/sensors_kinect_pointcloud.yaml |  9 +++++++++
 launch/sensor_manager.launch.xml      |  6 ++++--
 4 files changed, 24 insertions(+), 4 deletions(-)
 delete mode 100644 config/sensors_3d.yaml
 create mode 100644 config/sensors_kinect_depthmap.yaml
 create mode 100644 config/sensors_kinect_pointcloud.yaml

diff --git a/config/sensors_3d.yaml b/config/sensors_3d.yaml
deleted file mode 100644
index 51010a3..0000000
--- a/config/sensors_3d.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-sensors:
-  []
\ No newline at end of file
diff --git a/config/sensors_kinect_depthmap.yaml b/config/sensors_kinect_depthmap.yaml
new file mode 100644
index 0000000..9538fe0
--- /dev/null
+++ b/config/sensors_kinect_depthmap.yaml
@@ -0,0 +1,11 @@
+sensors:
+  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+    image_topic: /camera/depth_registered/image_raw
+    queue_size: 5
+    near_clipping_plane_distance: 0.3
+    far_clipping_plane_distance: 5.0
+    shadow_threshold: 0.2
+    padding_scale: 4.0
+    padding_offset: 0.03
+    max_update_rate: 1.0
+    filtered_cloud_topic: filtered_cloud
diff --git a/config/sensors_kinect_pointcloud.yaml b/config/sensors_kinect_pointcloud.yaml
new file mode 100644
index 0000000..92e7095
--- /dev/null
+++ b/config/sensors_kinect_pointcloud.yaml
@@ -0,0 +1,9 @@
+sensors:
+  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    point_cloud_topic: /camera/depth_registered/points
+    max_range: 5.0
+    point_subsample: 1
+    padding_offset: 0.1
+    padding_scale: 1.0
+    max_update_rate: 1.0
+    filtered_cloud_topic: filtered_cloud
diff --git a/launch/sensor_manager.launch.xml b/launch/sensor_manager.launch.xml
index 17279dd..61db9d9 100644
--- a/launch/sensor_manager.launch.xml
+++ b/launch/sensor_manager.launch.xml
@@ -2,11 +2,13 @@
 
   <!-- This file makes it easy to include the settings for sensor managers -->
 
-  <!-- Params for 3D sensors config -->
-  <rosparam command="load" file="$(find panda_moveit_config)/config/sensors_3d.yaml" />
+  <!-- Params for 3D sensors config: '' | pointcloud | depthmap -->
+  <arg name="sensor_type" default="" />
+  <rosparam if="$(eval arg('sensor_type')  != '')" command="load" file="$(find panda_moveit_config)/config/sensors_kinect_$(arg sensor_type).yaml" />
 
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
+  <param name="octomap_frame" type="string" value="camera_rgb_optical_frame" />
   <param name="octomap_resolution" type="double" value="0.025" />
   <param name="max_range" type="double" value="5.0" />
 
-- 
GitLab