Fully integrated with sequence_reader

nestormh · Apr 28, 2015 · 7d511e6 · 7d511e6
1 parent a0389c7
commit 7d511e6
Show file tree

Hide file tree

Showing 11 changed files with 168 additions and 156 deletions.
diff --git a/config/VoxelGridTrackingVerdino.rviz b/config/VoxelGridTrackingVerdino.rviz
@@ -49,7 +49,7 @@ Panels:
         - /Navigation1/VerdinoModel1
         - /WinnerPath1
       Splitter Ratio: 0.298137
-    Tree Height: 529
+    Tree Height: 341
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -95,42 +95,18 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        back_panel_link:
-          Value: true
         base_footprint:
           Value: true
-        base_link:
-          Value: true
-        chassis:
-          Value: true
-        control_box_link:
-          Value: true
-        fake_chassis_link:
-          Value: true
-        front_panel_link:
-          Value: true
-        gps:
+        base_left_cam:
           Value: true
-        imu:
+        base_link:
           Value: true
-        left_laser_link:
+        left_cam:
           Value: true
         map:
           Value: true
         odom:
           Value: true
-        rear_laser_link:
-          Value: true
-        right_laser_link:
-          Value: true
-        roof_link:
-          Value: true
-        seats_link:
-          Value: true
-        stereo_ps4:
-          Value: true
-        utm:
-          Value: true
         velodyne:
           Value: true
       Marker Scale: 1
@@ -143,35 +119,11 @@ Visualization Manager:
           odom:
             base_footprint:
               base_link:
-                chassis:
-                  {}
-                gps:
-                  {}
-                imu:
-                  {}
-                left_laser_link:
-                  {}
-                rear_laser_link:
-                  {}
-                right_laser_link:
-                  {}
-                roof_link:
-                  back_panel_link:
-                    {}
-                  control_box_link:
+                base_left_cam:
+                  left_cam:
                     {}
-                  fake_chassis_link:
-                    {}
-                  front_panel_link:
-                    {}
-                  seats_link:
-                    {}
-                stereo_ps4:
-                  {}
                 velodyne:
                   {}
-        utm:
-          {}
       Update Interval: 0
       Value: true
     - Class: rviz/Group
@@ -338,9 +290,9 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 41
+          Max Intensity: 0.68
           Min Color: 0; 0; 0
-          Min Intensity: 1
+          Min Intensity: 0
           Name: ObstaclesPointCloud
           Position Transformer: XYZ
           Queue Size: 10
@@ -518,14 +470,14 @@ Visualization Manager:
         - Alpha: 0.1
           Autocompute Intensity Bounds: false
           Autocompute Value Bounds:
-            Max Value: 22.6035
-            Min Value: -4.77445
+            Max Value: 4.319
+            Min Value: -8.926
             Value: true
           Axis: Z
           Channel Name: intensity
           Class: rviz/PointCloud2
           Color: 255; 255; 255
-          Color Transformer: Intensity
+          Color Transformer: AxisColor
           Decay Time: 0
           Enabled: true
           Invert Rainbow: false
@@ -546,7 +498,7 @@ Visualization Manager:
           Value: true
         - Class: rviz/Image
           Enabled: true
-          Image Topic: /stereo/right/image_rect_eyefish
+          Image Topic: /verdino/sequence_reader/left/image_raw
           Max Value: 1
           Median window: 5
           Min Value: 0
@@ -679,7 +631,13 @@ Visualization Manager:
           Class: rviz/Path
           Color: 25; 255; 0
           Enabled: true
+          Line Style: Lines
+          Line Width: 0.03
           Name: globalPlan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
           Topic: /verdino/move_base/TrajectoryPlannerROS/global_plan
           Value: true
         - Alpha: 1
@@ -1082,7 +1040,13 @@ Visualization Manager:
           Class: rviz/Path
           Color: 25; 255; 0
           Enabled: true
+          Line Style: Lines
+          Line Width: 0.03
           Name: global_plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
           Topic: /verdino/move_base/GrullNavfnROS/plan
           Value: true
         - Alpha: 0.7
@@ -1164,22 +1128,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 4.10056
+      Distance: 8.06278
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.00890449
-        Y: -0.328575
-        Z: -0.403068
+        X: -3.1571
+        Y: -0.806058
+        Z: 1.04693
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.195204
-      Target Frame: velodyne
+      Pitch: 0.419797
+      Target Frame: base_footprint
       Value: VideoBahnhoffAbove
-      Yaw: 1.50492
+      Yaw: 3.14992
     Saved:
       - Class: rviz/XYOrbit
         Distance: 20.0678
@@ -1354,7 +1318,7 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: true
-  Height: 748
+  Height: 975
   Hide Left Dock: true
   Hide Right Dock: true
   ImageGT:
@@ -1369,7 +1333,7 @@ Window Geometry:
     collapsed: false
   Left_rectified:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Right:
     collapsed: false
   RightImg:
@@ -1386,6 +1350,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1366
-  X: -2
+  Width: 1280
+  X: 1364
   Y: -3
diff --git a/launch/launch.launch b/launch/launch.launch
@@ -2,16 +2,18 @@
     <arg name="namespace" default="verdino/voxel_grid_tracking"/>
     <arg name="env_name" default="verdino"/>
     <arg name="show_rviz" default="false"/>
-    <arg name="just_velodyne_obstacles" default="false" />
-    <arg name="input_topic" default="/verdino/velodyne_points"/>
+
+    <arg name="input_topic" default="/$(arg namespace)/velodyne_points"/>
 <!--     Debug output for rviz -->
     <arg name="do_publish_intermediate_info" value="false" />
 
     <arg name="voxel_tracking_params_file"
          default="$(find voxel_grid_tracking)/params/voxel_tracking_verdino_params.yaml"/>
 
     <group ns="$(arg namespace)">
-        <node unless="$(arg just_velodyne_obstacles)" name="voxel_grid_tracking" pkg="voxel_grid_tracking" type="voxel_grid_tracking" output="screen" required="true" >
+        <!--         <node launch-prefix="gdb &#45;&#45;args" name="voxel_grid_tracking" pkg="voxel_grid_tracking" type="voxel_grid_tracking" output="screen" required="true" > -->
+        <!--         <node launch-prefix="gdb -ex run &#45;&#45;args" name="voxel_grid_tracking" pkg="voxel_grid_tracking" type="voxel_grid_tracking" output="screen" required="true" > -->
+        <node name="voxel_grid_tracking" pkg="voxel_grid_tracking" type="voxel_grid_tracking" output="screen" required="true" >
             <remap from="~/deltaTime"
                    to="/$(arg namespace)/stereo_and_odom/deltaTime" />
             <remap from="~/pointCloud"
@@ -24,58 +26,67 @@
                    to="/$(arg namespace)/stereo_and_odom/right/camera_info" />
             <remap from="~/dbg/image_rect_color"
                    to="/$(arg namespace)/stereo_and_odom/left/image_rect_color" />
-           <remap from="~fakePointCloud"
+            <remap from="~fakePointCloud"
                    to="/verdino/fakePointCloud" />
 
             <rosparam file="$(arg voxel_tracking_params_file)" command="load" ns="" />
 
             <param name="publish_intermediate_info" value="$(arg do_publish_intermediate_info)" />
         </node>
-    </group>
-
-    <!--     I must place the remap here. For some reason, it is ignored in other location -->
-    <remap from="/filters/filter_manager/disparity_input" to="/$(arg namespace)/disparity" />
-    <remap from="/filters/filter_manager/left/camera_info" to="/$(arg namespace)/stereo_and_odom/left/camera_info" />
-    <remap from="/filters/filter_manager/right/camera_info" to="/$(arg namespace)/stereo_and_odom/right/camera_info" />
-    <group ns="filters">
+
         <!-- PCL Manager -->
-        <node pkg="nodelet" type="nodelet" name="box_filter_manager" args="manager" output="screen" required="true" />
+        <node pkg="nodelet" type="nodelet" name="box_filter_manager" args="manager" output="screen" respawn="true" />
+
+        <node pkg="nodelet" type="nodelet" name="cam2footprint_pc_transform" 
+              args="load pcl/PassThrough box_filter_manager" output="screen" required="true">
+            <remap from="~input" to="$(arg input_topic)" />
+            <remap from="~output" to="/$(arg namespace)/point_cloud_footprint" />
+            <param name="filter_field_name" value="z" />
+            <param name="filter_limit_min" value="-1000.0" />
+            <param name="filter_limit_max" value="1000.0" />
+            <param name="filter_limit_negative" value="false" />
+            <param name="input_frame" value="velodyne" />
+            <param name="output_frame" value="base_footprint" />
+        </node>
 
-        <include file="$(find grull_transform_pointcloud)/launch/launch.launch">
-            <arg name="nodelet_name"     value="cam2footprint_pc_transform" />
-            <arg name="input_topic"     value="$(arg input_topic)" />
-            <arg name="output_topic"    value="/$(arg namespace)/point_cloud_footprint"/>
-            <arg name="target_frame"    value="base_footprint"/>
-            <arg name="nodelet_manager" value="box_filter_manager"/>
-        </include>
-
         <!-- Run a passthrough filter to delimit in x direction -->
-        <node pkg="nodelet" type="nodelet" name="psx" args="load pcl/PassThrough box_filter_manager" output="screen" required="true">
+        <node pkg="nodelet" type="nodelet" name="psx" args="load pcl/PassThrough box_filter_manager" 
+              output="screen" respawn="true">
             <remap from="~input" to="/$(arg namespace)/point_cloud_footprint" />
             <param name="filter_field_name" value="x" />
         </node>
 
         <!-- Run a passthrough filter to delimit in y direction -->
-        <node pkg="nodelet" type="nodelet" name="psy" args="load pcl/PassThrough box_filter_manager" output="screen" required="true">
+        <node pkg="nodelet" type="nodelet" name="psy" args="load pcl/PassThrough box_filter_manager" 
+              output="screen" respawn="true">
             <remap from="~input" to="psx/output" />
             <param name="filter_field_name" value="y" />
         </node>
 
         <!-- Run a passthrough filter to delimit in z direction -->
-        <node pkg="nodelet" type="nodelet" name="psz" args="load pcl/PassThrough box_filter_manager" output="screen" required="true">
+        <node pkg="nodelet" type="nodelet" name="psz" args="load pcl/PassThrough box_filter_manager" 
+              output="screen" respawn="true">
             <remap from="~input" to="psy/output" />
             <param name="filter_field_name" value="z" />
         </node>
 
-       <rosparam file="$(find voxel_grid_tracking)/params/$(arg env_name).yaml" command="load" ns="" />
+        <rosparam file="$(find voxel_grid_tracking)/params/$(arg env_name).yaml" command="load" ns="" />
 
-        <include file="$(find grull_transform_pointcloud)/launch/launch.launch">
-            <arg name="nodelet_name"    value="footprint2map_pc_transform" />
-            <arg name="input_topic"     value="psz/output" />
-            <arg name="output_topic"    value="/$(arg namespace)/obstaclesPointCloud"/>
-            <arg name="target_frame"    value="map"/>
-            <arg name="nodelet_manager" value="box_filter_manager"/>
-        </include>
+        <node pkg="nodelet" type="nodelet" name="footprint2map_pc_transform" 
+              args="load pcl/PassThrough box_filter_manager" output="screen" respawn="true">
+            <remap from="~input" to="/$(arg namespace)/psz/output" />
+            <remap from="~output" to="/$(arg namespace)/obstaclesPointCloud" />
+
+            <param name="approximate_sync" value="false" />
+            <param name="max_queue_size" value="10" />
+
+            <param name="filter_field_name" value="x" />
+            <param name="filter_limit_min" value="-1000.0" />
+            <param name="filter_limit_max" value="1000.0" />
+            <param name="filter_limit_negative" value="false" />
+            <param name="input_frame" value="base_footprint" />
+            <param name="output_frame" value="odom" />
+        </node>
     </group>
 
     <group if="$(arg show_rviz)">

diff --git a/launch/voxel_tracking_sequence_reader.launch b/launch/voxel_tracking_sequence_reader.launch
@@ -0,0 +1,35 @@
+<launch>
+    <arg name="namespace" default="verdino"/>
+    <arg name="env_name" default="verdino"/>
+    <arg name="show_rviz" default="true"/>
+
+    <arg name="input_topic" default="/$(arg namespace)/velodyne_points"/>
+
+    <arg name="do_publish_intermediate_info" value="false" />
+
+    <arg name="voxel_tracking_params_file"
+         default="$(find voxel_grid_tracking)/params/voxel_tracking_verdino_params.yaml"/>
+
+    <include file="$(find sequence_reader)/launch/launch.launch">
+        <arg name="namespace" value="$(arg namespace)"/>
+        <arg name="wait_time_sequence_reader" default="200"/>
+        <arg name="use_rviz" value="false"/>
+        <arg name="use_elas" value="false"/>
+        <arg name="use_rsgm" value="false"/>
+
+        <arg name="static_transforms" value="true" />
+        <arg name="static_odom2basefootprint" value="false" />
+        <arg name="visual_odom" value="false" />
+
+        <arg name="sequence_file" 
+             value="$(find sequence_reader)/params/kitti_sequence_campus.yaml" />
+    </include>
+
+    <include file="$(find voxel_grid_tracking)/launch/launch.launch">
+        <arg name="namespace" value="$(arg namespace)"/>
+        <arg name="env_name" value="$(arg namespace)"/>
+        <arg name="show_rviz" value="$(arg show_rviz)"/>
+        <arg name="input_topic" value="/$(arg namespace)/velodyne_points"/>
+    </include>
+
+</launch>
diff --git a/params/verdino.yaml b/params/verdino.yaml
@@ -1,12 +1,12 @@
 psx:
     filter_limit_min: 0.0
-    filter_limit_max: 50.0
+    filter_limit_max: 20.0
 
 psy:
-    filter_limit_min: -2.0
-    filter_limit_max: 2.0
+    filter_limit_min: -4.0
+    filter_limit_max: 4.0
 
 psz:
-    filter_limit_min: 0.5
+    filter_limit_min: 0.75
     filter_limit_max: 4.0