Lidars - FontysAtWork/ESA-PROJ GitHub Wiki

The Light Detection And Ranging (LiDAR) sensors are the main source of localization for the Youbot. There are two LiDARs fitted on the Youbot (one on the front and one on the back). The UBG-04LX-F01 LiDAR sensors from Hokuyo are used. Relative to the center of the Youbot (or baselink in ROS terms) these LiDARs have the following offsets.

In order for the LiDAR localization to work, ROS needs to know the position of the LiDARs in respect to the base_link transform frame. This is done by creating static transform publishers. They are created in the lasers.template file.

<!-- front laser -->
<node pkg="tf" type="static_transform_publisher"
   name="hokuyo_front_broadcaster"
   args="0.338 0 -0.025 0 0 0 base_link hokuyo_front_link 100" />

<!-- back laser -->
<node pkg="tf" type="static_transform_publisher"
   name="hokuyo_back_broadcaster"
   args="-0.335 0 -0.025 0 3.14 3.14 base_link hokuyo_back_link 100" />

Note that the back laser has a rotation in the roll and pitch angles but not in the yaw (which would be the more logical and obvious choice). This however, resulted in strange behaviour (the back LiDAR data had a slight offset in the yaw direction). It was possible to correct this by adjusting the yaw angle, but that would create a setting which would be confusing others. After examining the Youbot examples we noticed they rotated the pitch and roll rotation instead.

The hokuyo_node package is deprecated and only supported up until ROS jade. That's why, if the switch to ROS kinetic is made, a replacement package will need to be integrated. urg_node seems to be a suitable replacement but this hasn't been tested yet.

Update: We've since moved to ROS Kinetic. We now use the urg_node for reading the LiDAR output. It is indeed a drop-in replacement for the hokuyo_node package.

The driver nodes for the LiDAR sensors publish the information on separate ROS topics (/scan_front and /scan_back). However AMCL only expects one source for laser scans. This means they need to be combined. We first tried this by using the laserscan_multi_merger node from the ira_laser_tools package.

As shown in the image the /scan_front and /scan_back topics are interpolated into the /scan_multi topic. This node was declared in the lasers.template file.

<node pkg="ira_laser_tools" name="laserscan_multi_merger" type="laserscan_multi_merger" >
   <param name="destination_frame" value="/base_link"/>
   <param name="cloud_destination_topic" value="/merged_cloud"/>
   <param name="scan_destination_topic" value="/scan_multi"/>
   <param name="laserscan_topics" value ="/scan_front /scan_back" />
</node>

Though this seemed to work at first we noticed that over time a non-correctable drift in the rotation was accumulated. This drift was between the static map (from the map server) and the actual LiDAR scans. This means the Youbot gradually lost its position in the map. It was most noticeable when rotating the Youbot in place. After a lot of troubleshooting, we discovered that the interpolation algorithm in the laserscan_multi_merger node was the culprit. We decided to inspect the codebase of other teams for inspiration. When we looked at the hokuyo.launch file from the Swarmlab@Work team we noticed they combined the topics with the relay node.

The difference is that this technique alternately copies the /scan_front and /scan_back LiDAR data to the same topic (/scan_combined). This can work because the messages are stamped with a header that uniquely identifies them. Apparently AMCL is able to differentiate the data on this header (this isn't documented on the ROS wiki). After trying this approach we noticed that the drift was gone. It still drifts a little when rotating in place, however it corrects itself when moving more linear. The relay nodes are created in the lasers.template file using the following code:

<node pkg="topic_tools" type="relay" name="relay_hokuyo_front" 
      args="/scan_front /scan_combined" />
<node pkg="topic_tools" type="relay" name="relay_hokuyo_rear" 
      args="/scan_back /scan_combined" />