From 2aaa01d65491d02ef6c095f7bc7b13a8f52d1be6 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?highlight=perception%20tutorial
---
 config/sensors_3d.yaml                | 23 -----------------------
 config/sensors_kinect_depthmap.yaml   | 11 +++++++++++
 config/sensors_kinect_pointcloud.yaml |  9 +++++++++
 launch/sensor_manager.launch.xml      |  5 ++++-
 4 files changed, 24 insertions(+), 24 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 0713b61..0000000
--- a/config/sensors_3d.yaml
+++ /dev/null
@@ -1,23 +0,0 @@
-# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
-sensors:
-  - filtered_cloud_topic: filtered_cloud
-    max_range: 5.0
-    max_update_rate: 1.0
-    padding_offset: 0.1
-    padding_scale: 1.0
-    point_cloud_topic: /head_mount_kinect/depth_registered/points
-    point_subsample: 1
-    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-  - far_clipping_plane_distance: 5.0
-    filtered_cloud_topic: filtered_cloud
-    image_topic: /head_mount_kinect/depth_registered/image_raw
-    max_range: 5.0
-    max_update_rate: 1.0
-    near_clipping_plane_distance: 0.3
-    padding_offset: 0.03
-    padding_scale: 4.0
-    point_cloud_topic: /head_mount_kinect/depth_registered/points
-    point_subsample: 1
-    queue_size: 5
-    sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
-    shadow_threshold: 0.2
\ 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..dd9d3ad 100644
--- a/launch/sensor_manager.launch.xml
+++ b/launch/sensor_manager.launch.xml
@@ -3,10 +3,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" />
+  <arg name="sensor_type" default="pointcloud"/>
+  <rosparam if="$(eval arg('sensor_type')  == 'point_cloud')" command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
+  <rosparam unless="$(eval arg('sensor_type')  == 'point_cloud')" command="load" file="$(find panda_moveit_config)/config/sensors_kinect_depthmap.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" />