add paerception node for pr2 navigation

k-okada · Nov 6, 2019 · a3502ef · a3502ef
1 parent 2517fbb
commit a3502ef
Showing 1 changed file with 45 additions and 0 deletions.
diff --git a/pr2eus/test/pr2-ri-test.launch b/pr2eus/test/pr2-ri-test.launch
@@ -25,6 +25,43 @@
 
   <!-- for :move-to and :go-velocity tests -->
   <include file="$(find pr2_navigation_slam)/slam_gmapping.xml" />
+  <!-- Filter for tilt laser shadowing/veiling -->
+  <node pkg="laser_filters" type="scan_to_cloud_filter_chain" name="tilt_shadow_filter">
+    <remap from="scan" to="tilt_scan" />
+    <remap from="cloud_filtered" to="tilt_scan_shadow_filtered" />
+    <param name="target_frame" value="base_footprint" />
+    <param name="high_fidelity" value="true" />
+    <rosparam command="load" file="$(find pr2_navigation_perception)/config/tilt_laser_filters.yaml" />
+    <rosparam command="load" file="$(find pr2_navigation_perception)/config/point_cloud_footprint_filter.yaml" />
+  </node>
+  <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="interpolate_missing_tilt_laser_data_filter">
+    <rosparam command="load" file="$(find pr2_navigation_perception)/config/laser_interpolation.yaml" />
+    <remap from="scan" to="tilt_scan" />
+    <remap from="scan_filtered" to="tilt_scan_interpolated" />
+  </node>
+  <!-- Filter for tilt laser scans that hit the body of the robot -->
+  <node pkg="pr2_navigation_self_filter" type="self_filter" name="tilt_laser_self_filter">
+    <remap from="cloud_in" to="tilt_scan_shadow_filtered" />
+    <remap from="cloud_out" to="tilt_scan_filtered" />
+    <rosparam command="load" file="$(find pr2_navigation_perception)/config/tilt_self_filter.yaml" />
+    <param name="sensor_frame" type="string" value="laser_tilt_link" />
+  </node>
+  <!-- Filter for base laser shadowing/veiling -->
+  <node pkg="laser_filters" type="scan_to_cloud_filter_chain" name="base_shadow_filter" >
+    <remap from="scan" to="base_scan" />
+    <remap from="cloud_filtered" to="base_scan_shadow_filtered" />
+    <param name="target_frame" value="base_footprint" />
+    <rosparam command="load" file="$(find pr2_navigation_perception)/config/shadow_filter.yaml" />
+    <rosparam command="load" file="$(find pr2_navigation_perception)/config/point_cloud_footprint_filter.yaml" />
+  </node>
+  <!-- Filter for base laser scans that hit the body of the robot -->
+  <node pkg="pr2_navigation_self_filter" type="self_filter" name="base_laser_self_filter">
+    <remap from="cloud_in" to="base_scan_shadow_filtered" />
+    <remap from="cloud_out" to="base_scan_marking" />
+    <param name="sensor_frame" type="string" value="base_laser_link" />
+    <rosparam command="load" file="$(find pr2_navigation_perception)/config/base_self_filter.yaml" />
+  </node>
+
   <node pkg="move_base" type="move_base" name="move_base_node" >
     <remap from="odom" to="base_odometry/odom" />
     <remap from="cmd_vel" to="base_controller/command" />
@@ -44,6 +81,14 @@
     <rosparam file="$(find pr2_navigation_slam)/config/global_costmap_params.yaml" command="load" />
     <rosparam file="$(find pr2_navigation_slam)/config/local_costmap_params.yaml" command="load"  />
   </node>
+  <node pkg="semantic_point_annotator" type="sac_inc_ground_removal_node" name="sac_ground_removal">
+    <remap from="tilt_laser_cloud_filtered" to="tilt_scan_filtered" />
+    <remap from="cloud_ground_filtered" to="ground_object_cloud" />
+    <param name="z_threshold" value="0.15" />
+    <param name="sac_min_points_per_model" value="40" />
+    <param name="sac_distance_threshold" value="0.05" />
+    <param name="planar_refine" value="1" />
+  </node>
 
   <!-- start test -->
   <test test-name="pr2_ri_test" pkg="roseus" type="roseus" retry="1"