Velodyne VLP-16 / Ouster OS-1-16

[bj-neilson]

Hi Mathieu,

I’ll be ordering a 3D lidar to add to my rover. Looking at Velodyne or Ouster 16 channel 360deg rotating lidar.

I’m trying to achieve long term SLAM / GPS denied navigation under a tree orchard canopy.  I have been using Zed so far with limited success.

A problem I see is that foliage will change rapidly. A positive is that tree trunks change very slowly over time.  Creating pointclouds out to a longer range to capture lots of tree trunks, while narrowing the pointcloud to ignore most foliage should improve long term SLAM. Compared with doing that with the ZED I think it would be useful.

My question is how well would you expect rtabmap to perform in this case? I’m receiving IMU data via MavRos from a Cube. I’ve just added 2 x F9P GPS to the rover for great heading data.  I can retain the Zed.  I’ll be removing my RPLidarA2.

Any thoughts on an optimal setup for this configuration?    Sorry for the big question, but GPS denied mapping and localisation is critical to me.  Any other suggestions would be warmly welcomed!

Thanks,
Ben

[matlabbe]

Do you plan to reproduce the same path with the same orientation? Or that the robot has to navigate in any directions, independently how the environment was mapped?

For the ZED setup, another problem I see is the variation of the lighting during the day that could influence visual localization. Lidar could be more robust to that.

If you manage to filter the foliage out of the lidar (maybe the lidar intensity will be different for the foliage than the trunks, making easier to filter it) while keeping the trunks and the ground, a lidar-based approach may "work". With rtabmap, when combining camera and lidar, the zed would be used for global localization (like initializing the pose on the field), then the lidar would be used to refine the pose with being independent of the lighting and orientation of the robot. The IMU can be used as guess for lidar localization, which can help if the field is bumpy. When you will receive the lidar, it could be great if you can share a rosbag of the scans in the orchard environment.

cheers,
Mathieu

[bj-neilson]

Hi Mathieu,

Thanks for your thoughts.

I've just done a big re-build of everything so there's a lot to test again all of a sudden, but I'll run through what I've got and what I'd like to do.

I was using the Zed with a Jetson TX2 and RpLidarA2.  I was able to drive a loop around a row of trees, coming back to the start point.  There was a fair bit of odometry drift.  As the rover started doing a second loop of the same row, if the drift was not too big then it would start to find loop closures.  It was a bit dependent on foliage though, as the Zed has a shorter range and if the rover was in the sun and the tree trunks were shaded, they would not be detected well.

I have a Velodyne VLP-16 arriving next week.  I have setup with some other new hardware.  I have 2 Ublox F9P GPS units for moving baseline on the rover, an F9P base station, still the Zed, pixhawk Cube (for it's IMUs via MavRos) all now running on a Jetson Xavier.

I have used one of your example launch files to subscribe to RGBD, IMU and GPS.  Switching between Zed odom, rtabmap VIO and combining ICP for a bit of testing.  Once the new lidar is fitted I'll add that and remove the RPLidarA2.

As you suggest, yes I'll try my best to filter out the foliage and concentrate on trunks for long term operation.

Once I get basic mapping and localisation working well I'll need to find a few to write a path planner to suit my needs.  I need to mow along a loop around a row of trees at a small offset from the trees, then for a second pass add a mower width to the offset, then move to the next row.  For other operations like spraying I'll just want a repeatable path down the middle of tree rows in a snake path through the orchard.  Nothing like a challenge!

Thanks, yes I'll get a nice rosbag recorded as soon as I can and link that here to take a look at.

Ta
Ben

[denzle]

@Ben, i'm trying something similar but having a not so fun time with the launch file. Would you consider sharing yours for comparison.

Regards
Dan

[bj-neilson]

Hi Dan,

I'll paste it below.  This is just a lightly modified version of one of Mathieu's examples.  I launch the Velodyne and other sensors separately.

Cheers,
Ben


<launch>
 

 
           
 
     
       
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
 
 
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
 
 
  <arg name="localization"            default="false"/>
 
 
  <arg name="use_sim_time"            default="false"/>
 
 
 
  <arg name="cfg"                     default="" /> 
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
 
  <arg name="frame_id"                default="base_link"/>     
  <arg name="odom_frame_id"           default="odom"/>               
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     
  <arg name="ground_truth_base_frame_id" default=""/> 
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/Documents/RTAB-Map/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="2"/>
  <arg name="args"                    default=""/>             
  <arg name="rtabmap_args"            default="$(arg args)"/>   
  <arg name="launch_prefix"           default=""/>             
  <arg name="output"                  default="screen"/>       
  <arg name="publish_tf_map"          default="true"/>

 
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>         
   
 
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth/depth_registered" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
 
 
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />     
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
 
 
  <arg name="rgbd_sync"               default="true"/>         
  <arg name="approx_rgbd_sync"        default="true"/>         
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />
 
  <arg name="compressed"              default="false"/>         
  <arg name="rgb_image_transport"     default="compressed"/>   
  <arg name="depth_image_transport"   default="compressedDepth"/> 
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="true"/>
  <arg name="scan_cloud_topic"        default="/velodyne_points"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="false"/> 
   
  <arg name="visual_odometry"          default="true"/>         
  <arg name="icp_odometry"             default="true"/>         
  <arg name="odom_topic"               default="/camera/odom"/>         
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> 
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="1"/>             
  <arg name="odom_tf_linear_variance"  default="1"/>             
  <arg name="odom_args"                default=""/>             
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/> 
  <arg name="imu_topic"                default="/mavros/imu/data"/>         
  <arg name="wait_imu_to_init"         default="false"/>
 
  <arg name="subscribe_user_data"      default="false"/>             
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> 
 
  <arg name="gps_topic"                default="/mavros/global_position/global" />         
 
  <arg name="tag_topic"                default="/tag_detections" /> 
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             
 
 
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>

 
  <group ns="$(arg namespace)">
     
   
    <group if="$(arg depth)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
         
         
         
        </node>
      </group>
    </group>
    <group if="$(arg stereo)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
          <remap from="left/image_rect"   to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"  to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
         
         
        </node>
      </group>
    </group>
   
    <group unless="$(arg rgbd_sync)">
       <group if="$(arg subscribe_rgbd)">
         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
           <remap     if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
           <remap     if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
           <remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
           
         </node>
      </group>
    </group>

   
    <group unless="$(arg icp_odometry)">
      <group if="$(arg visual_odometry)">
     
       
        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"            to="$(arg odom_topic)"/>
          <remap from="imu"             to="$(arg imu_topic)"/>
     
         
         
         
         
         
         
         
         
         
         
         
         
         
         
        </node>

       
        <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"             to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"                   to="$(arg odom_topic)"/>
          <remap from="imu"                    to="$(arg imu_topic)"/>
     
         
         
         
         
         
         
         
         
         
         
         
         
         
         
        </node>
      </group>
    </group>
   
   
    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
     
     
     
     
     
     
     
     
     
     
     
     
     
     
    </node>
 
   
   
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
           
     
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
     
      <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
       
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
     
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap     if="$(arg scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap from="gps/fix"                to="$(arg gps_topic)"/>
      <remap from="tag_detections"         to="$(arg tag_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
     
     
     
     
     
    </node>
 
   
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
     
     
     
     
     
     
     
     
     
     
     
     
     
   
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
     
      <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
     
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
     
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap     if="$(arg scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>
 
  </group>
 
 
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
    <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
    <remap from="cloud"             to="voxel_cloud" />

   
   
   
  </node>

</launch>

[denzle]

Many thanks Ben. I'm hoping to find where i'm going wrong or could tweek parameters so this will be a great help.

Dan