From b341c75bd340c6286ea34e411bbfca21ca4b2e7e Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 5 Mar 2020 12:36:27 +0100
Subject: [PATCH] Added the node and nodelet versions of the launch files for
 3D navigation.

---
 launch/include/camera_nodelets.launch | 159 ++++++++++++++++++++++++++
 launch/include/camera_nodes.launch    | 146 +++++++++++++++++++++++
 launch/include/lidar_nodelets.launch  |  48 ++++++++
 launch/include/lidar_nodes.launch     |  35 ++++++
 4 files changed, 388 insertions(+)
 create mode 100644 launch/include/camera_nodelets.launch
 create mode 100644 launch/include/camera_nodes.launch
 create mode 100644 launch/include/lidar_nodelets.launch
 create mode 100644 launch/include/lidar_nodes.launch

diff --git a/launch/include/camera_nodelets.launch b/launch/include/camera_nodelets.launch
new file mode 100644
index 0000000..caeff32
--- /dev/null
+++ b/launch/include/camera_nodelets.launch
@@ -0,0 +1,159 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <arg name="ns" default="ana"/>
+
+  <arg name="obstacle_nodelet_manager" default="obstacle_nodelet_manager" />
+  <arg name="hole_nodelet_manager" default="hole_nodelet_manager" />
+  <arg name="camera_cloud_in" default="sensors/nav_cam/depth_registered/points"/>
+
+  <arg name="average_node_name" default="average_point_cloud"/>
+  <arg name="average_config_file" default="$(find iri_average_point_cloud)/config/default_config.yaml"/>
+
+  <arg name="normals_node_name" default="obstacle_detection_normals"/>
+  <arg name="normals_config_file" default="$(find iri_nav_obstacle_detection_normals)/config/default_config.yaml"/>
+
+  <arg name="near_hole_detection_node_name" default="near_hole_detection"/>
+  <arg name="near_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/near_config.yaml"/>
+  <arg name="hole_detection_cloud_in" default="radius_outlier_removal/output"/>
+
+  <arg name="far_hole_detection_node_name" default="far_hole_detection"/>
+  <arg name="far_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/far_config.yaml"/>
+
+  <arg name="output" default="screen"/>
+  <arg name="launch_prefix" default=""/>
+
+<!-- *******************  OBSTACLE DETECTION  **********************************  -->
+  <group ns="$(arg ns)">
+
+    <!-- Obstacle nodelets manager -->
+    <node name="$(arg obstacle_nodelet_manager)"
+      pkg="nodelet"
+      type="nodelet"
+      args="manager"
+      output="$(arg output)"/> 
+
+    <node pkg="nodelet" 
+          type="nodelet"
+          name="pc_throttle" args="load iri_average_point_cloud/NodeletThrottlePC $(arg obstacle_nodelet_manager)" output="$(arg output)">
+        <remap from="topic_in"  to="$(arg camera_cloud_in)"/>
+        <remap from="topic_out" to="$(arg camera_cloud_in)_throttle"/>
+        <param name="update_rate" value="15"/>
+    </node>
+  </group>
+
+  <include file="$(find iri_average_point_cloud)/launch/nodelet.launch">
+    <arg name="ns" value="$(arg ns)"/>
+    <arg name="node_name" value="$(arg average_node_name)"/>
+    <arg name="camera_nodelet_manager" value="$(arg obstacle_nodelet_manager)"/>
+    <arg name="config_file" value="$(arg average_config_file)"/>
+    <arg name="output" value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="cloud_in" value="$(arg camera_cloud_in)_throttle"/>
+  </include>
+
+  <group ns="$(arg ns)">
+    <!-- Downsampling points  -->
+    <node name="voxel_grid_obs"
+      pkg="nodelet"
+      type="nodelet"
+      args="load pcl/VoxelGrid $(arg obstacle_nodelet_manager)"
+      output="$(arg output)">
+      <remap from="~input" to="average_point_cloud/output" />
+      <rosparam>  
+        input_frame: 'ana/base_footprint'
+        output_frame: 'ana/base_footprint'
+        leaf_size: 0.03
+        filter_field_name: z
+        filter_limit_min: -0.5
+        filter_limit_max: 5.0
+        filter_limit_negative: False
+      </rosparam>
+    </node>
+
+    <!-- Run a passthrough filter  -->
+    <node name="passthrough"
+      pkg="nodelet" 
+      type="nodelet"  
+      args="load pcl/PassThrough $(arg obstacle_nodelet_manager)" 
+      output="$(arg output)">
+      <remap from="~input" to="voxel_grid_obs/output" />
+      <rosparam>
+        filter_field_name: x
+        filter_limit_min: 0.01
+        filter_limit_max: 1.5
+        filter_limit_negative: False
+      </rosparam>
+    </node>
+
+    <!-- Statistical outlier removal  -->
+    <node name="outlier_removal" 
+      pkg="nodelet"
+      type="nodelet"
+      args="load pcl/StatisticalOutlierRemoval $(arg obstacle_nodelet_manager)"
+      output="$(arg output)" >
+      <remap from="~input" to="passthrough/output" />
+      <rosparam>
+        mean_k: 20
+        stddev: 2.0
+        negative: False 
+      </rosparam>
+    </node> 
+
+    <!-- Radius outlier removal  -->
+    <node name="radius_outlier_removal" 
+      pkg="nodelet"
+      type="nodelet"
+      args="load pcl/RadiusOutlierRemoval $(arg obstacle_nodelet_manager)"
+      output="$(arg output)" >
+      <remap from="~input" to="outlier_removal/output" />
+      <rosparam>
+        radius_search: 0.1
+        min_neighbors: 15
+      </rosparam>
+    </node> 
+  </group>
+
+  <include file="$(find iri_nav_obstacle_detection_normals)/launch/nodelet.launch">
+    <arg name="ns" value="$(arg ns)"/>
+    <arg name="node_name" value="$(arg normals_node_name)"/>
+    <arg name="camera_nodelet_manager" value="$(arg obstacle_nodelet_manager)"/>
+    <arg name="config_file" value="$(arg normals_config_file)"/>
+    <arg name="output" value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="cloud_in" value="radius_outlier_removal/output"/>
+  </include>
+
+<!-- **************************** HOLE DETECTION ******************************* -->
+
+  <group ns="$(arg ns)">
+    <node name="$(arg hole_nodelet_manager)"
+      pkg="nodelet"
+      type="nodelet"
+      args="manager"
+      output="$(arg output)"/>
+  </group>
+
+  <include file="$(find iri_point_cloud_hole_detection)/launch/nodelet.launch">
+         <arg name="ns" value="$(arg ns)"/>
+         <arg name="node_name" value="$(arg near_hole_detection_node_name)"/>
+         <arg name="camera_nodelet_manager" value="$(arg hole_nodelet_manager)"/>
+         <arg name="config_file" value="$(arg near_hole_detection_config_file)"/>
+         <arg name="output" value="$(arg output)"/>
+         <arg name="launch_prefix" default="$(arg launch_prefix)"/>
+         <arg name="cloud_in" default="$(arg hole_detection_cloud_in)"/>
+  </include>
+
+  <include file="$(find iri_point_cloud_hole_detection)/launch/nodelet.launch">
+         <arg name="ns" value="$(arg ns)"/>
+         <arg name="node_name" value="$(arg far_hole_detection_node_name)"/>
+         <arg name="camera_nodelet_manager" value="$(arg hole_nodelet_manager)"/>
+         <arg name="config_file" value="$(arg far_hole_detection_config_file)"/>
+         <arg name="output" value="$(arg output)"/>
+         <arg name="launch_prefix" default="$(arg launch_prefix)"/>
+         <arg name="cloud_in" default="$(arg hole_detection_cloud_in)"/>
+  </include>
+
+</launch>
+
diff --git a/launch/include/camera_nodes.launch b/launch/include/camera_nodes.launch
new file mode 100644
index 0000000..339f92f
--- /dev/null
+++ b/launch/include/camera_nodes.launch
@@ -0,0 +1,146 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <arg name="ns" default="ana"/>
+
+  <arg name="obstacle_nodelet_manager" default="obstacle_nodelet_manager" />
+
+  <arg name="camera_cloud_in" default="sensors/nav_cam/depth_registered/points"/>
+
+  <arg name="average_node_name" default="average_point_cloud"/>
+  <arg name="average_config_file" default="$(find iri_average_point_cloud)/config/default_config.yaml"/>
+  <arg name="average_cloud_in" default="sensors/nav_cam/depth_registered/points_throttle"/>
+
+  <arg name="normals_node_name" default="obstacle_detection_normals"/>
+  <arg name="normals_config_file" default="$(find iri_nav_obstacle_detection_normals)/config/default_config.yaml"/>
+  <arg name="normals_cloud_in" default="radius_outlier_removal/output"/>
+
+  <arg name="near_hole_detection_node_name" default="near_hole_detection"/>
+  <arg name="near_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/near_config.yaml"/>
+  <arg name="hole_detection_cloud_in" default="radius_outlier_removal/output"/>
+
+  <arg name="far_hole_detection_node_name" default="far_hole_detection"/>
+  <arg name="far_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/far_config.yaml"/>
+
+  <arg name="output" default="screen"/>
+  <arg name="launch_prefix" default=""/>
+
+<!-- *******************  OBSTACLE DETECTION  **********************************  -->
+
+  <group ns="$(arg ns)">
+    <node name="pc_throttler" 
+          type="throttle" 
+          pkg="topic_tools" 
+          args="messages $(arg camera_cloud_in) 15 $(arg camera_cloud_in)_throttle" />
+  </group>
+
+  <include file="$(find iri_average_point_cloud)/launch/node.launch">
+	 <arg name="ns" value="$(arg ns)"/>
+         <arg name="node_name" value="$(arg average_node_name)"/>
+         <arg name="config_file" value="$(arg average_config_file)"/>
+         <arg name="output" value="$(arg output)"/>
+         <arg name="launch_prefix" default="$(arg launch_prefix)"/>
+         <arg name="cloud_in" default="$(arg average_cloud_in)"/>
+  </include>
+
+  <group ns="$(arg ns)">
+
+    <node name="$(arg obstacle_nodelet_manager)"
+      pkg="nodelet"
+      type="nodelet"
+      args="manager"
+      output="screen"/>
+
+    <!-- Downsampling points  -->
+    <node name="voxel_grid_obs"
+      pkg="nodelet"
+      type="nodelet"
+      args="load pcl/VoxelGrid $(arg obstacle_nodelet_manager)"
+      output="$(arg output)">
+      <remap from="~input" to="average_point_cloud/output" />
+      <rosparam>  
+        input_frame: 'ana/base_footprint'
+        output_frame: 'ana/base_footprint'
+        leaf_size: 0.03
+        filter_field_name: z
+        filter_limit_min: -0.5
+        filter_limit_max: 5.0
+        filter_limit_negative: False
+      </rosparam>
+    </node>
+
+    <!-- Run a passthrough filter  -->
+    <node name="passthrough"
+      pkg="nodelet" 
+      type="nodelet"  
+      args="load pcl/PassThrough $(arg obstacle_nodelet_manager)" 
+      output="$(arg output)">
+      <remap from="~input" to="voxel_grid_obs/output" />
+      <rosparam>
+        filter_field_name: x
+        filter_limit_min: 0.01
+        filter_limit_max: 1.5
+        filter_limit_negative: False
+      </rosparam>
+    </node>
+
+    <!-- Statistical outlier removal  -->
+    <node name="outlier_removal" 
+      pkg="nodelet"
+      type="nodelet"
+      args="load pcl/StatisticalOutlierRemoval $(arg obstacle_nodelet_manager)"
+      output="$(arg output)" >
+      <remap from="~input" to="passthrough/output" />
+      <rosparam>
+        mean_k: 20
+        stddev: 2.0
+        negative: False 
+      </rosparam>
+    </node> 
+
+    <!-- Radius outlier removal  -->
+    <node name="radius_outlier_removal" 
+      pkg="nodelet"
+      type="nodelet"
+      args="load pcl/RadiusOutlierRemoval $(arg obstacle_nodelet_manager)"
+      output="$(arg output)" >
+      <remap from="~input" to="outlier_removal/output" />
+      <rosparam>
+        radius_search: 0.1
+        min_neighbors: 15
+      </rosparam>
+    </node> 
+  </group>
+
+  <include file="$(find iri_nav_obstacle_detection_normals)/launch/node.launch">
+	 <arg name="ns" value="$(arg ns)"/>
+         <arg name="node_name" value="$(arg normals_node_name)"/>
+         <arg name="config_file" value="$(arg normals_config_file)"/>
+         <arg name="output" value="$(arg output)"/>
+         <arg name="launch_prefix" default="$(arg launch_prefix)"/>
+         <arg name="cloud_in" default="$(arg normals_cloud_in)"/>
+  </include>
+
+<!-- **************************** HOLE DETECTION ******************************* -->
+
+  <include file="$(find iri_point_cloud_hole_detection)/launch/node.launch">
+	 <arg name="ns" value="$(arg ns)"/>
+         <arg name="node_name" value="$(arg near_hole_detection_node_name)"/>
+         <arg name="config_file" value="$(arg near_hole_detection_config_file)"/>
+         <arg name="output" value="$(arg output)"/>
+         <arg name="launch_prefix" default="$(arg launch_prefix)"/>
+         <arg name="cloud_in" default="$(arg hole_detection_cloud_in)"/>
+  </include>
+
+  <include file="$(find iri_point_cloud_hole_detection)/launch/node.launch">
+	 <arg name="ns" value="$(arg ns)"/>
+         <arg name="node_name" value="$(arg far_hole_detection_node_name)"/>
+         <arg name="config_file" value="$(arg far_hole_detection_config_file)"/>
+         <arg name="output" value="$(arg output)"/>
+         <arg name="launch_prefix" default="$(arg launch_prefix)"/>
+         <arg name="cloud_in" default="$(arg hole_detection_cloud_in)"/>
+  </include>
+
+</launch>
+
diff --git a/launch/include/lidar_nodelets.launch b/launch/include/lidar_nodelets.launch
new file mode 100644
index 0000000..49fa71a
--- /dev/null
+++ b/launch/include/lidar_nodelets.launch
@@ -0,0 +1,48 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+  <arg name="ns" default="ana" />
+  <arg name="output" default="log"/>
+  <arg name="launch_prefix" default=""/>
+
+  <arg name="lidar_nodelet_manager" default="lidar_nodelet_manager"/>
+
+  <arg name="lidar_filter_node_name" default="lidar_filter"/>
+  <arg name="lidar_filter_config_file" default="$(find iri_point_cloud_angle_filter)/config/default_config.yaml"/>
+  <arg name="lidar_filter_cloud_in" default="/pointcloud_in" />
+
+  <arg name="lidar_detector_node_name" default="lidar_detector"/>
+  <arg name="lidar_detector_config_file" default="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
+
+  <!-- Obstacle nodelets manager -->
+  <group ns="$(arg ns)">
+    <node name="$(arg lidar_nodelet_manager)"
+          pkg="nodelet"
+          type="nodelet"
+          args="manager"
+          output="screen"/>
+  </group>
+
+  <include file="$(find iri_point_cloud_angle_filter)/launch/nodelet.launch">
+    <arg name="ns" value="$(arg ns)"/>
+    <arg name="node_name" value="$(arg lidar_filter_node_name)"/>
+    <arg name="lidar_nodelet_manager" value="$(arg lidar_nodelet_manager)"/>
+    <arg name="config_file" value="$(arg lidar_filter_config_file)"/>
+    <arg name="output" value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="cloud_in" value="$(arg lidar_filter_cloud_in)" />
+  </include>
+
+  <include file="$(find iri_lidar_obstacle_detector)/launch/nodelet.launch">
+    <arg name="ns" value="$(arg ns)"/>
+    <arg name="node_name" value="$(arg lidar_detector_node_name)"/>
+    <arg name="lidar_nodelet_manager" value="$(arg lidar_nodelet_manager)"/>
+    <arg name="config_file" value="$(arg lidar_detector_config_file)"/>
+    <arg name="output" value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="cloud_in" value="/$(arg ns)/$(arg lidar_filter_node_name)/cloud_out"/>
+  </include>
+  
+</launch>
+
diff --git a/launch/include/lidar_nodes.launch b/launch/include/lidar_nodes.launch
new file mode 100644
index 0000000..f1673f4
--- /dev/null
+++ b/launch/include/lidar_nodes.launch
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+  <arg name="ns" default="ana" />
+  <arg name="output" default="log"/>
+  <arg name="launch_prefix" default=""/>
+
+  <arg name="lidar_filter_node_name" default="lidar_filter"/>
+  <arg name="lidar_filter_config_file" default="$(find iri_point_cloud_angle_filter)/config/default_config.yaml"/>
+  <arg name="lidar_filter_cloud_in" default="/pointcloud_in" />
+
+  <arg name="lidar_detector_node_name" default="lidar_detector"/>
+  <arg name="lidar_detector_config_file" default="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
+
+  <include file="$(find iri_point_cloud_angle_filter)/launch/node.launch">
+    <arg name="ns" value="$(arg ns)"/>
+    <arg name="node_name" value="$(arg lidar_filter_node_name)"/>
+    <arg name="config_file" value="$(arg lidar_filter_config_file)"/>
+    <arg name="output" value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="cloud_in" value="$(arg lidar_filter_cloud_in)" />
+  </include>
+
+  <include file="$(find iri_lidar_obstacle_detector)/launch/node.launch">
+    <arg name="ns" value="$(arg ns)"/>
+    <arg name="node_name" value="$(arg lidar_detector_node_name)"/>
+    <arg name="config_file" value="$(arg lidar_detector_config_file)"/>
+    <arg name="output" value="$(arg output)"/>
+    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="cloud_in" value="/$(arg ns)/$(arg lidar_filter_node_name)/cloud_out"/>
+  </include>
+  
+</launch>
+
-- 
GitLab