dump pcds and processing

launch/bringup.launch

@@ -4,23 +4,35 @@
     xsi:noNamespaceSchemaLocation="https://gist.githubusercontent.com/nalt/dfa2abc9d2e3ae4feb82ca5608090387/raw/roslaunch.xsd">
 
     <!-- ARGS -->
-    <arg name="no_rgb" default="false" />
+    <arg
+        name="no_rgb" default="false" />
     <arg name="no_rgbd" default="false" />
-    <arg name="no_robots" default="false" />
+    <arg name="no_robots"
+        default="false" />
     <arg name="no_rrc" default="false" />
     <arg name="no_rws" default="false" />
-    <arg name="no_record" default="false" />
+    <arg
+        name="no_record" default="false" />
 
     <!-- INCLUDES -->
-    <include file="$(dirname)/bringup/rgbd.launch" pass_all_args="true" unless="$(arg no_rgbd)" />
+    <include file="$(dirname)/bringup/rgbd.launch"
+        pass_all_args="true" unless="$(arg no_rgbd)" />
 
-    <include file="$(dirname)/bringup/rgb.launch" pass_all_args="true" unless="$(arg no_rgb)" />
+    <include file="$(dirname)/bringup/rgb.launch"
+        pass_all_args="true" unless="$(arg no_rgb)" />
 
-    <include file="$(dirname)/bringup/robots.launch" pass_all_args="true" unless="$(arg no_robots)" />
+    <include
+        file="$(dirname)/bringup/robots.launch" pass_all_args="true" unless="$(arg no_robots)" />
 
     <!-- RECORD -->
-    <node name="record" pkg="rosbag" type="record"
-        args="--all --publish --output-prefix=$(env HOME)/rosbags/biodigitalmatter --split
-    --size=50000 --lz4"
+    <node
+        name="record" pkg="rosbag" type="record"
+        args="--all --publish --output-prefix=$(env HOME)/rosbags/$(optenv HOSTNAME no_env_hostname) --split
+    --duration=2h --lz4"
         unless="$(arg no_record)" />
+    <node name="store_pointclouds" pkg="pcl_ros" type="pointcloud">
+        <remap from="input" to="/points" />
+        <param name="fixed_frame" value="world" />
+        <param name="prefix" value="$(env HOME)/pcds/" />
+    </node>
 </launch>

launch/bringup/robots.launch

@@ -12,6 +12,7 @@
     <arg name="namespace" default="/"
         doc="Allows to define a namespace in order to connect to multiple robots. Defaults to root (ie /)." />
 
+
     <group ns="$(arg namespace)">
 
         <!-- TF -->
@@ -34,7 +35,7 @@
         </include>
 
         <node pkg="robot_state_publisher" type="robot_state_publisher"
-            name="rob_st_pub"
+        name="rob_st_pub"
             unless="$(arg no_rws)">
             <remap from="joint_states" to="/rws/joint_states" />
         </node>

launch/process_pointcloud.launch

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+
+<launch>
+
+    <arg name="name" default="oak" />
+    <arg name="params_file" default="$(find depthai_ros_driver)/config/rgbd.yaml" />
+    <arg name="rectify_rgb" default="true" />
+    <node pkg="nodelet" type="nodelet" name="rectify_color"
+        args="load image_proc/rectify $(arg  name)_nodelet_manager" if="$(arg rectify_rgb)">
+        <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">
+        <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" />
+
+    </node>
+</launch>

ros_catkin_entrypoint.sh

@@ -15,5 +15,6 @@ if [ -f $CATKIN_WS/src/biodigitalmatter_ros/.env ] ; then
 fi
 
 mkdir -p ~/rosbags
+mkdir -p ~/pcds
 
 exec "$@"