Switched to tf2, cleanup transforms, added option for time offset.

elevation_mapping/CMakeLists.txt

@@ -13,9 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
   message_filters
   sensor_msgs
   std_msgs
-  tf
-  tf_conversions
-  eigen_conversions
+  tf2_ros
+  tf2_geometry_msgs
   kindr
   kindr_ros
 )
@@ -48,9 +47,8 @@ catkin_package(
     message_filters
     sensor_msgs
     std_msgs
-    tf
-    tf_conversions
-    eigen_conversions
+    tf2_ros
+    tf2_geometry_msgs
     kindr
     kindr_ros
   DEPENDS

elevation_mapping/include/elevation_mapping/ElevationMap.hpp

@@ -8,24 +8,16 @@
 
 #pragma once
 
-// Grid Map
-#include <grid_map_ros/grid_map_ros.hpp>
+#include "elevation_mapping/typedefs.hpp"
 
-// Eigen
+#include <grid_map_ros/grid_map_ros.hpp>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-
-// PCL
+#include <kindr/Core>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
-// Kindr
-#include <kindr/Core>
-
-// Boost
 #include <boost/thread/recursive_mutex.hpp>
-
-// ROS
 #include <ros/ros.h>
 
 namespace elevation_mapping {
@@ -65,7 +57,7 @@ class ElevationMap
    * @return true if successful.
    */
   bool add(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud, Eigen::VectorXf& pointCloudVariances,
-           const ros::Time& timeStamp, const Eigen::Affine3d& transformationSensorToMap);
+           const ros::Time& timeStamp, const Transform& transformationSensorToMap);
 
   /*!
    * Update the elevation map with variance update data.

elevation_mapping/include/elevation_mapping/ElevationMapping.hpp

@@ -10,6 +10,7 @@
 
 // Elevation Mapping
 #include "elevation_mapping/ElevationMap.hpp"
+#include "elevation_mapping/typedefs.hpp"
 #include "elevation_mapping/RobotMotionMapUpdater.hpp"
 #include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp"
 #include "elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp"
@@ -200,11 +201,12 @@ class ElevationMapping
   //! Size of the cache for the robot pose messages.
   int robotPoseCacheSize_;
 
-  //! TF listener and broadcaster.
-  tf::TransformListener transformListener_;
+  //! TF listener and buffer.
+  tf2_ros::TransformListener tfListener_;
+  tf2_ros::Buffer tfBuffer_;
 
   //! Point which the elevation map follows.
-  kindr::Position3D trackPoint_;
+  Position3 trackPoint_;
   std::string trackPointFrameId_;
 
   //! ROS topics for subscriptions.
@@ -232,6 +234,9 @@ class ElevationMapping
   //! Maximum time that the map will not be updated.
   ros::Duration maxNoUpdateDuration_;
 
+  //! Time offset between pose and range measurements.
+  ros::Duration timeOffsetForPointCloud_;
+
   //! Time tolerance for updating the map with data before the last update.
   //! This is useful when having multiple sensors adding data to the map.
   ros::Duration timeTolerance_;

elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp

@@ -25,7 +25,7 @@ class LaserSensorProcessor : public SensorProcessorBase
    * @param nodeHandle the ROS node handle.
    * @param transformListener the ROS transform listener.
    */
-  LaserSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+  LaserSensorProcessor(ros::NodeHandle& nodeHandle, tf2_ros::Buffer& tfBuffer);
 
   /*!
    * Destructor.

elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp

@@ -25,7 +25,7 @@ class PerfectSensorProcessor : public SensorProcessorBase
    * @param nodeHandle the ROS node handle.
    * @param transformListener the ROS transform listener.
    */
-  PerfectSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+  PerfectSensorProcessor(ros::NodeHandle& nodeHandle, tf2_ros::Buffer& tfBuffer);
 
   /*!
    * Destructor.

elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp

@@ -8,21 +8,14 @@
 
 #pragma once
 
-// ROS
-#include <ros/ros.h>
-#include <tf/transform_listener.h>
+#include "elevation_mapping/typedefs.hpp"
 
-// PCL
+#include <Eigen/Core>
+#include <ros/ros.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <tf2_ros/buffer.h>
 
-// Eigen
-#include <Eigen/Core>
-
-// Kindr
-#include <kindr/Core>
-
-// STL
 #include <unordered_map>
 #include <string>
 #include <memory>
@@ -46,13 +39,19 @@ class SensorProcessorBase
    * @param nodeHandle the ROS node handle.
    * @param transformListener the ROS transform listener.
    */
-	SensorProcessorBase(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+	SensorProcessorBase(ros::NodeHandle& nodeHandle, tf2_ros::Buffer& tfBuffer);
 
 	/*!
 	 * Destructor.
 	 */
 	virtual ~SensorProcessorBase();
 
+  /*!
+   * Reads and verifies the parameters.
+   * @return true if successful, false otherwise.
+   */
+  virtual bool readParameters();
+
 	/*!
 	 * Processes the point cloud.
 	 * @param[in] pointCloudInput the input point cloud.
@@ -70,13 +69,6 @@ class SensorProcessorBase
 	friend class ElevationMapping;
 
  protected:
-
-  /*!
-   * Reads and verifies the parameters.
-   * @return true if successful.
-   */
-  virtual bool readParameters();
-
   /*!
    * Cleans the point cloud.
    * @param pointCloud the point cloud to clean.
@@ -124,26 +116,23 @@ class SensorProcessorBase
   //! ROS nodehandle.
   ros::NodeHandle& nodeHandle_;
 
-  //! TF transform listener.
-  tf::TransformListener& transformListener_;
-
-  //! The timeout duration for the lookup of the transformation between sensor frame and target frame.
-  ros::Duration transformListenerTimeout_;
+  /// TF buffer.
+  tf2_ros::Buffer& tfBuffer_;
 
-  //! Rotation from Base to Sensor frame (C_SB)
-  kindr::RotationMatrixD rotationBaseToSensor_;
+  //! Rotation from Sensor to Base frame (C_BS).
+  RotationMatrix rotationSensorToBase_;
 
-  //! Translation from Base to Sensor in Base frame (B_r_BS)
-  kindr::Position3D translationBaseToSensorInBaseFrame_;
+  //! Translation from Base to Sensor in Base frame (B_r_BS).
+  Position3 translationBaseToSensorInBaseFrame_;
 
-  //! Rotation from (elevation) Map to Base frame (C_BM)
-  kindr::RotationMatrixD rotationMapToBase_;
+  //! Rotation from Base to (elevation) Map frame (C_MB).
+  RotationMatrix rotationBaseToMap_;
 
-  //! Translation from Map to Base in Map frame (M_r_MB)
-  kindr::Position3D translationMapToBaseInMapFrame_;
+  //! Translation from Map to Base in Map frame (M_r_MB).
+  Position3 translationMapToBaseInMapFrame_;
 
-  //! Transformation from Sensor to Map frame
-  Eigen::Affine3d transformationSensorToMap_;
+  //! Transformation from Sensor to Map frame.
+  Transform transformationSensorToMap_;
 
   //! TF frame id of the map.
   std::string mapFrameId_;

elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp

@@ -29,7 +29,7 @@ class StereoSensorProcessor: public SensorProcessorBase
    * @param nodeHandle the ROS node handle.
    * @param transformListener the ROS transform listener.
    */
-  StereoSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+  StereoSensorProcessor(ros::NodeHandle& nodeHandle, tf2_ros::Buffer& tfBuffer);
 
   /*!
    * Destructor.

elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp

@@ -25,7 +25,7 @@ class StructuredLightSensorProcessor : public SensorProcessorBase
    * @param nodeHandle the ROS node handle.
    * @param transformListener the ROS transform listener.
    */
-  StructuredLightSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+  StructuredLightSensorProcessor(ros::NodeHandle& nodeHandle, tf2_ros::Buffer& tfBuffer);
 
   /*!
    * Destructor.

elevation_mapping/include/elevation_mapping/typedefs.hpp

@@ -0,0 +1,21 @@
+/*
+ * typedefs.hpp
+ *
+ *  Created on: Sep 26, 2017
+ *      Author: Péter Fankhauser
+ *   Institute: ETH Zurich
+ */
+
+#pragma once
+
+#include <kindr/Core>
+#include <grid_map_core/TypeDefs.hpp>
+
+namespace elevation_mapping {
+
+using Position2 = grid_map::Position;
+using Position3 = kindr::Position3D;
+using RotationMatrix = kindr::RotationMatrixPD;
+using Transform = kindr::HomTransformQuatD;
+
+}

elevation_mapping/package.xml

@@ -19,9 +19,8 @@
   <depend>grid_map_msgs</depend>
   <depend>kindr</depend>
   <depend>kindr_ros</depend>
-  <depend>tf</depend>
-  <depend>tf_conversions</depend>
-  <depend>eigen_conversions</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>boost</depend>
   <depend>eigen</depend>
 </package>

elevation_mapping/src/ElevationMap.cpp

@@ -63,7 +63,8 @@ void ElevationMap::setGeometry(const grid_map::Length& length, const double& res
   ROS_INFO_STREAM("Elevation map grid resized to " << rawMap_.getSize()(0) << " rows and "  << rawMap_.getSize()(1) << " columns.");
 }
 
-bool ElevationMap::add(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud, Eigen::VectorXf& pointCloudVariances, const ros::Time& timestamp, const Eigen::Affine3d& transformationSensorToMap)
+bool ElevationMap::add(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud, Eigen::VectorXf& pointCloudVariances,
+                       const ros::Time& timestamp, const Transform& transformationSensorToMap)
 {
   if (pointCloud->size() != pointCloudVariances.size()) {
     ROS_ERROR("ElevationMap::add: Size of point cloud (%i) and variances (%i) do not agree.",
@@ -84,7 +85,7 @@ bool ElevationMap::add(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud,
   for (unsigned int i = 0; i < pointCloud->size(); ++i) {
     auto& point = pointCloud->points[i];
     Index index;
-    Position position(point.x, point.y);
+    Position2 position(point.x, point.y);
     if (!rawMap_.getIndex(position, index)) continue; // Skip this point if it does not lie within the elevation map.
 
     auto& elevation = rawMap_.at("elevation", index);
@@ -133,7 +134,7 @@ bool ElevationMap::add(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud,
     const float pointHeightPlusUncertainty = point.z + 3.0 * sqrt(pointVariance); // 3 sigma.
     if (std::isnan(lowestScanPoint) || pointHeightPlusUncertainty < lowestScanPoint){
       lowestScanPoint = pointHeightPlusUncertainty;
-      const Position3 sensorTranslation(transformationSensorToMap.translation());
+      const Position3 sensorTranslation(transformationSensorToMap.getPosition());
       sensorXatLowestScan = sensorTranslation.x();
       sensorYatLowestScan = sensorTranslation.y();
       sensorZatLowestScan = sensorTranslation.z();
@@ -202,7 +203,7 @@ bool ElevationMap::fuseArea(const Eigen::Vector2d& position, const Eigen::Array2
   Index submapBufferSize;
 
   // These parameters are not used in this function.
-  Position submapPosition;
+  Position2 submapPosition;
   Length submapLength;
   Index requestedIndexInSubmap;
 
@@ -277,7 +278,7 @@ bool ElevationMap::fuse(const grid_map::Index& topLeftIndex, const grid_map::Ind
     const double ellipseRotation(atan2(solver.eigenvectors().col(maxEigenvalueIndex).real()(1), solver.eigenvectors().col(maxEigenvalueIndex).real()(0)));
 
     // Requested length and position (center) of submap in map.
-    Position requestedSubmapPosition;
+    Position2 requestedSubmapPosition;
     rawMapCopy.getPosition(*areaIterator, requestedSubmapPosition);
     EllipseIterator ellipseIterator(rawMapCopy, requestedSubmapPosition, ellipseLength, ellipseRotation);
 
@@ -312,7 +313,7 @@ bool ElevationMap::fuse(const grid_map::Index& topLeftIndex, const grid_map::Ind
       means[i] = rawMapCopy.at("elevation", *ellipseIterator);
 
       // Compute weight from probability.
-      Position absolutePosition;
+      Position2 absolutePosition;
       rawMapCopy.getPosition(*ellipseIterator, absolutePosition);
       Eigen::Vector2d distanceToCenter = (rotationMatrix * (absolutePosition - requestedSubmapPosition)).cwiseAbs();
 
@@ -410,8 +411,8 @@ void ElevationMap::visibilityCleanup(const ros::Time& updatedTime)
     const auto& sensorZatLowestScan = visibilityCleanupMap_.at("sensor_z_at_lowest_scan", *iterator);
     if (std::isnan(lowestScanPoint)) continue;
     Index indexAtSensor;
-    if(!visibilityCleanupMap_.getIndex(Position(sensorXatLowestScan, sensorYatLowestScan), indexAtSensor)) continue;
-    Position point;
+    if(!visibilityCleanupMap_.getIndex(Position2(sensorXatLowestScan, sensorYatLowestScan), indexAtSensor)) continue;
+    Position2 point;
     visibilityCleanupMap_.getPosition(*iterator, point);
     float pointDiffX = point.x() - sensorXatLowestScan;
     float pointDiffY = point.y() - sensorYatLowestScan;