diff --git a/elevation_mapping/CMakeLists.txt b/elevation_mapping/CMakeLists.txt index e867f0c0..63b0ab96 100644 --- a/elevation_mapping/CMakeLists.txt +++ b/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 diff --git a/elevation_mapping/include/elevation_mapping/ElevationMap.hpp b/elevation_mapping/include/elevation_mapping/ElevationMap.hpp index ee521214..da7b2e29 100644 --- a/elevation_mapping/include/elevation_mapping/ElevationMap.hpp +++ b/elevation_mapping/include/elevation_mapping/ElevationMap.hpp @@ -8,24 +8,16 @@ #pragma once -// Grid Map -#include +#include "elevation_mapping/typedefs.hpp" -// Eigen +#include #include #include - -// PCL +#include #include #include -// Kindr -#include - -// Boost #include - -// ROS #include namespace elevation_mapping { @@ -65,7 +57,7 @@ class ElevationMap * @return true if successful. */ bool add(const pcl::PointCloud::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. diff --git a/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp b/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp index ea40989b..58e9b579 100644 --- a/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp +++ b/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_; diff --git a/elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp b/elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp index 50e82e39..5a8c6040 100644 --- a/elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp +++ b/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. diff --git a/elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp b/elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp index 484e32a1..1cd60b49 100644 --- a/elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp +++ b/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. diff --git a/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp b/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp index 2760d4b1..0dab4b36 100644 --- a/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp +++ b/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp @@ -8,21 +8,14 @@ #pragma once -// ROS -#include -#include +#include "elevation_mapping/typedefs.hpp" -// PCL +#include +#include #include #include +#include -// Eigen -#include - -// Kindr -#include - -// STL #include #include #include @@ -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_; diff --git a/elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp b/elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp index cc5b3d2e..1a562749 100644 --- a/elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp +++ b/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. diff --git a/elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp b/elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp index 256d4149..3d4f9530 100644 --- a/elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp +++ b/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. diff --git a/elevation_mapping/include/elevation_mapping/typedefs.hpp b/elevation_mapping/include/elevation_mapping/typedefs.hpp new file mode 100644 index 00000000..610a9f66 --- /dev/null +++ b/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 +#include + +namespace elevation_mapping { + +using Position2 = grid_map::Position; +using Position3 = kindr::Position3D; +using RotationMatrix = kindr::RotationMatrixPD; +using Transform = kindr::HomTransformQuatD; + +} diff --git a/elevation_mapping/package.xml b/elevation_mapping/package.xml index f7026a75..8849f065 100644 --- a/elevation_mapping/package.xml +++ b/elevation_mapping/package.xml @@ -19,9 +19,8 @@ grid_map_msgs kindr kindr_ros - tf - tf_conversions - eigen_conversions + tf2_ros + tf2_geometry_msgs boost eigen diff --git a/elevation_mapping/src/ElevationMap.cpp b/elevation_mapping/src/ElevationMap.cpp index bab7fd0f..982d2899 100644 --- a/elevation_mapping/src/ElevationMap.cpp +++ b/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::Ptr pointCloud, Eigen::VectorXf& pointCloudVariances, const ros::Time& timestamp, const Eigen::Affine3d& transformationSensorToMap) +bool ElevationMap::add(const pcl::PointCloud::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::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::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; diff --git a/elevation_mapping/src/ElevationMapping.cpp b/elevation_mapping/src/ElevationMapping.cpp index 96cd6989..660f599f 100644 --- a/elevation_mapping/src/ElevationMapping.cpp +++ b/elevation_mapping/src/ElevationMapping.cpp @@ -6,35 +6,26 @@ * Institute: ETH Zurich, Autonomous Systems Lab */ #include "elevation_mapping/ElevationMapping.hpp" - -// Elevation Mapping #include "elevation_mapping/ElevationMap.hpp" #include "elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp" #include "elevation_mapping/sensor_processors/StereoSensorProcessor.hpp" #include "elevation_mapping/sensor_processors/LaserSensorProcessor.hpp" #include "elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp" -// Grid Map #include #include - -// PCL #include #include #include #include #include #include - -// Kindr #include #include +#include -// Boost #include #include - -// STL #include #include #include @@ -53,6 +44,7 @@ ElevationMapping::ElevationMapping(ros::NodeHandle& nodeHandle) : nodeHandle_(nodeHandle), map_(nodeHandle), robotMotionMapUpdater_(nodeHandle), + tfListener_(tfBuffer_), isContinouslyFusing_(false), ignoreRobotMotionUpdates_(false) { @@ -130,6 +122,10 @@ bool ElevationMapping::readParameters() maxNoUpdateDuration_.fromSec(1.0 / minUpdateRate); ROS_ASSERT(!maxNoUpdateDuration_.isZero()); + double timeOffset; + nodeHandle_.param("time_offset_for_point_cloud", timeOffset, 0.0); + timeOffsetForPointCloud_.fromSec(timeOffset); + double timeTolerance; nodeHandle_.param("time_tolerance", timeTolerance, 0.0); timeTolerance_.fromSec(timeTolerance); @@ -187,13 +183,13 @@ bool ElevationMapping::readParameters() string sensorType; nodeHandle_.param("sensor_processor/type", sensorType, string("structured_light")); if (sensorType == "structured_light") { - sensorProcessor_.reset(new StructuredLightSensorProcessor(nodeHandle_, transformListener_)); + sensorProcessor_.reset(new StructuredLightSensorProcessor(nodeHandle_, tfBuffer_)); } else if (sensorType == "stereo") { - sensorProcessor_.reset(new StereoSensorProcessor(nodeHandle_, transformListener_)); + sensorProcessor_.reset(new StereoSensorProcessor(nodeHandle_, tfBuffer_)); } else if (sensorType == "laser") { - sensorProcessor_.reset(new LaserSensorProcessor(nodeHandle_, transformListener_)); + sensorProcessor_.reset(new LaserSensorProcessor(nodeHandle_, tfBuffer_)); } else if (sensorType == "perfect") { - sensorProcessor_.reset(new PerfectSensorProcessor(nodeHandle_, transformListener_)); + sensorProcessor_.reset(new PerfectSensorProcessor(nodeHandle_, tfBuffer_)); } else { ROS_ERROR("The sensor type %s is not available.", sensorType.c_str()); } @@ -249,6 +245,8 @@ void ElevationMapping::pointCloudCallback( PointCloud::Ptr pointCloud(new PointCloud); pcl::fromPCLPointCloud2(pcl_pc, *pointCloud); lastPointCloudUpdateTime_.fromNSec(1000 * pointCloud->header.stamp); + lastPointCloudUpdateTime_ += timeOffsetForPointCloud_; + pointCloud->header.stamp = lastPointCloudUpdateTime_.toNSec() / 1000; ROS_INFO("ElevationMap received a point cloud (%i points) for elevation mapping.", static_cast(pointCloud->size())); @@ -271,6 +269,7 @@ void ElevationMapping::pointCloudCallback( ROS_ERROR("Could not get pose information from robot for time %f. Buffer empty?", lastPointCloudUpdateTime_.toSec()); return; } + ROS_DEBUG_STREAM("Time error for robot pose from buffer: " << (lastPointCloudUpdateTime_ - poseMessage->header.stamp).toSec() << " s."); robotPoseCovariance = Eigen::Map(poseMessage->pose.covariance.data(), 6, 6); } @@ -285,7 +284,7 @@ void ElevationMapping::pointCloudCallback( } // Add point cloud to elevation map. - if (!map_.add(pointCloudProcessed, measurementVariances, lastPointCloudUpdateTime_, Eigen::Affine3d(sensorProcessor_->transformationSensorToMap_))) { + if (!map_.add(pointCloudProcessed, measurementVariances, lastPointCloudUpdateTime_, sensorProcessor_->transformationSensorToMap_)) { ROS_ERROR("Adding point cloud to elevation map failed."); resetMapUpdateTimer(); return; @@ -394,10 +393,15 @@ bool ElevationMapping::updateMapLocation() convertToRosGeometryMsg(trackPoint_, trackPoint.point); geometry_msgs::PointStamped trackPointTransformed; + std::string errorMessage; try { - transformListener_.transformPoint(map_.getFrameId(), trackPoint, trackPointTransformed); - } catch (TransformException &ex) { - ROS_ERROR("%s", ex.what()); + if (!tfBuffer_.canTransform(map_.getFrameId(), trackPoint.header.frame_id, trackPoint.header.stamp, &errorMessage)) { + ROS_WARN("Could not lookup TF transform from %s to %s.", trackPoint.header.frame_id.c_str(), map_.getFrameId().c_str()); + return false; + } + tfBuffer_.transform(trackPoint, trackPointTransformed, map_.getFrameId()); + } catch (tf2::TransformException &errorMessage) { + ROS_ERROR("%s", errorMessage.what()); return false; } diff --git a/elevation_mapping/src/elevation_mapping_node.cpp b/elevation_mapping/src/elevation_mapping_node.cpp index 56b51f10..e9d63c15 100644 --- a/elevation_mapping/src/elevation_mapping_node.cpp +++ b/elevation_mapping/src/elevation_mapping_node.cpp @@ -16,7 +16,7 @@ int main(int argc, char** argv) elevation_mapping::ElevationMapping elevationMap(nodeHandle); // Spin - ros::AsyncSpinner spinner(1); // Use n threads + ros::AsyncSpinner spinner(2); // Use n threads. spinner.start(); ros::waitForShutdown(); return 0; diff --git a/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp b/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp index d92897e3..bc9cf0eb 100644 --- a/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp +++ b/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp @@ -25,8 +25,8 @@ namespace elevation_mapping { * International Conference on Applied Robotics for the Power Industry (CARPI), 2012. */ -LaserSensorProcessor::LaserSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener) - : SensorProcessorBase(nodeHandle, transformListener) +LaserSensorProcessor::LaserSensorProcessor(ros::NodeHandle& nodeHandle, tf2_ros::Buffer& tfBuffer) + : SensorProcessorBase(nodeHandle, tfBuffer) { } @@ -67,15 +67,15 @@ bool LaserSensorProcessor::computeVariances( const Eigen::RowVector3f projectionVector = Eigen::RowVector3f::UnitZ(); // Sensor Jacobian (J_s). - const Eigen::RowVector3f sensorJacobian = projectionVector * (rotationMapToBase_.transposed() * rotationBaseToSensor_.transposed()).toImplementation().cast(); + const Eigen::RowVector3f sensorJacobian = projectionVector * (rotationBaseToMap_ * rotationSensorToBase_).toImplementation().cast(); // Robot rotation covariance matrix (Sigma_q). Eigen::Matrix3f rotationVariance = robotPoseCovariance.bottomRightCorner(3, 3).cast(); // Preparations for robot rotation Jacobian (J_q) to minimize computation for every point in point cloud. - const Eigen::Matrix3f C_BM_transpose = rotationMapToBase_.transposed().toImplementation().cast(); + const Eigen::Matrix3f C_BM_transpose = rotationBaseToMap_.toImplementation().cast(); const Eigen::RowVector3f P_mul_C_BM_transpose = projectionVector * C_BM_transpose; - const Eigen::Matrix3f C_SB_transpose = rotationBaseToSensor_.transposed().toImplementation().cast(); + const Eigen::Matrix3f C_SB_transpose = rotationSensorToBase_.toImplementation().cast(); const Eigen::Matrix3f B_r_BS_skew = kindr::getSkewMatrixFromVector(Eigen::Vector3f(translationBaseToSensorInBaseFrame_.toImplementation().cast())); for (size_t i = 0; i < pointCloud->size(); ++i) { diff --git a/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp b/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp index 2532250a..e61a339c 100644 --- a/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp +++ b/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp @@ -22,8 +22,8 @@ namespace elevation_mapping { * Noiseless, perfect sensor. */ -PerfectSensorProcessor::PerfectSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener) - : SensorProcessorBase(nodeHandle, transformListener) +PerfectSensorProcessor::PerfectSensorProcessor(ros::NodeHandle& nodeHandle, tf2_ros::Buffer& tfBuffer) + : SensorProcessorBase(nodeHandle, tfBuffer) { } @@ -60,15 +60,15 @@ bool PerfectSensorProcessor::computeVariances( const Eigen::RowVector3f projectionVector = Eigen::RowVector3f::UnitZ(); // Sensor Jacobian (J_s). - const Eigen::RowVector3f sensorJacobian = projectionVector * (rotationMapToBase_.transposed() * rotationBaseToSensor_.transposed()).toImplementation().cast(); + const Eigen::RowVector3f sensorJacobian = projectionVector * (rotationBaseToMap_ * rotationSensorToBase_).toImplementation().cast(); // Robot rotation covariance matrix (Sigma_q). Eigen::Matrix3f rotationVariance = robotPoseCovariance.bottomRightCorner(3, 3).cast(); // Preparations for robot rotation Jacobian (J_q) to minimize computation for every point in point cloud. - const Eigen::Matrix3f C_BM_transpose = rotationMapToBase_.transposed().toImplementation().cast(); + const Eigen::Matrix3f C_BM_transpose = rotationBaseToMap_.toImplementation().cast(); const Eigen::RowVector3f P_mul_C_BM_transpose = projectionVector * C_BM_transpose; - const Eigen::Matrix3f C_SB_transpose = rotationBaseToSensor_.transposed().toImplementation().cast(); + const Eigen::Matrix3f C_SB_transpose = rotationSensorToBase_.toImplementation().cast(); const Eigen::Matrix3f B_r_BS_skew = kindr::getSkewMatrixFromVector(Eigen::Vector3f(translationBaseToSensorInBaseFrame_.toImplementation().cast())); for (unsigned int i = 0; i < pointCloud->size(); ++i) diff --git a/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp b/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp index 215e97a7..f34229b4 100644 --- a/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp +++ b/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp @@ -8,32 +8,28 @@ #include -//PCL +#include #include #include #include #include #include -//TF -#include - -// STL #include #include #include namespace elevation_mapping { -SensorProcessorBase::SensorProcessorBase(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener) +SensorProcessorBase::SensorProcessorBase(ros::NodeHandle& nodeHandle, tf2_ros::Buffer& tfBuffer) : nodeHandle_(nodeHandle), - transformListener_(transformListener), + tfBuffer_(tfBuffer), ignorePointsUpperThreshold_(std::numeric_limits::infinity()), ignorePointsLowerThreshold_(-std::numeric_limits::infinity()) { pcl::console::setVerbosityLevel(pcl::console::L_ERROR); + tfBuffer_.setUsingDedicatedThread(true); transformationSensorToMap_.setIdentity(); - transformListenerTimeout_.fromSec(1.0); } SensorProcessorBase::~SensorProcessorBase() {} @@ -43,12 +39,6 @@ bool SensorProcessorBase::readParameters() nodeHandle_.param("sensor_frame_id", sensorFrameId_, std::string("/sensor")); // TODO Fail if parameters are not found. nodeHandle_.param("robot_base_frame_id", robotBaseFrameId_, std::string("/robot")); nodeHandle_.param("map_frame_id", mapFrameId_, std::string("/map")); - - double minUpdateRate; - nodeHandle_.param("min_update_rate", minUpdateRate, 2.0); - transformListenerTimeout_.fromSec(1.0 / minUpdateRate); - ROS_ASSERT(!transformListenerTimeout_.isZero()); - nodeHandle_.param("sensor_processor/ignore_points_above", ignorePointsUpperThreshold_, std::numeric_limits::infinity()); nodeHandle_.param("sensor_processor/ignore_points_below", ignorePointsLowerThreshold_, -std::numeric_limits::infinity()); return true; @@ -79,28 +69,36 @@ bool SensorProcessorBase::process( bool SensorProcessorBase::updateTransformations(const ros::Time& timeStamp) { try { - transformListener_.waitForTransform(sensorFrameId_, mapFrameId_, timeStamp, ros::Duration(1.0)); - - tf::StampedTransform transformTf; - transformListener_.lookupTransform(mapFrameId_, sensorFrameId_, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationSensorToMap_); - - transformListener_.lookupTransform(robotBaseFrameId_, sensorFrameId_, timeStamp, transformTf); // TODO Why wrong direction? - Eigen::Affine3d transform; - poseTFToEigen(transformTf, transform); - rotationBaseToSensor_.setMatrix(transform.rotation().matrix()); - translationBaseToSensorInBaseFrame_.toImplementation() = transform.translation(); - - transformListener_.lookupTransform(mapFrameId_, robotBaseFrameId_, timeStamp, transformTf); // TODO Why wrong direction? - poseTFToEigen(transformTf, transform); - rotationMapToBase_.setMatrix(transform.rotation().matrix()); - translationMapToBaseInMapFrame_.toImplementation() = transform.translation(); - - return true; - } catch (tf::TransformException &ex) { - ROS_ERROR("%s", ex.what()); + std::string errorMessage; + if (!tfBuffer_.canTransform(sensorFrameId_, mapFrameId_, timeStamp, &errorMessage)) { + ROS_WARN("Could not lookup TF transform from %s to %s.", mapFrameId_.c_str(), sensorFrameId_.c_str()); + return false; + } + + // Sensor to base. + geometry_msgs::TransformStamped transformStamped; + transformStamped = tfBuffer_.lookupTransform(robotBaseFrameId_, sensorFrameId_, timeStamp); + Transform transform; + kindr_ros::convertFromRosGeometryMsg(transformStamped.transform, transform); + translationBaseToSensorInBaseFrame_ = transform.getPosition(); + rotationSensorToBase_ = transform.getRotation(); + + // Base to map. + transformStamped = tfBuffer_.lookupTransform(mapFrameId_, robotBaseFrameId_, timeStamp); + kindr_ros::convertFromRosGeometryMsg(transformStamped.transform, transform); + translationMapToBaseInMapFrame_ = transform.getPosition(); + rotationBaseToMap_ = transform.getRotation(); + + // Map to sensor. + transformStamped = tfBuffer_.lookupTransform(mapFrameId_, sensorFrameId_, timeStamp); + kindr_ros::convertFromRosGeometryMsg(transformStamped.transform, transformationSensorToMap_); + + } catch (tf2::TransformException &errorMessage) { + ROS_ERROR("%s", errorMessage.what()); return false; } + + return true; } bool SensorProcessorBase::transformPointCloud( @@ -112,18 +110,22 @@ bool SensorProcessorBase::transformPointCloud( timeStamp.fromNSec(1000 * pointCloud->header.stamp); const std::string inputFrameId(pointCloud->header.frame_id); - tf::StampedTransform transformTf; + geometry_msgs::TransformStamped transformStamped; try { - transformListener_.waitForTransform(targetFrame, inputFrameId, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(targetFrame, inputFrameId, timeStamp, transformTf); - } catch (tf::TransformException &ex) { - ROS_ERROR("%s", ex.what()); + std::string errorMessage; + if (!tfBuffer_.canTransform(targetFrame, inputFrameId, timeStamp, &errorMessage)) { + ROS_WARN("Could not lookup TF transform from %s to %s.", inputFrameId.c_str(), targetFrame.c_str()); + return false; + } + transformStamped = tfBuffer_.lookupTransform(targetFrame, inputFrameId, timeStamp); + } catch (tf2::TransformException &errorMessage) { + ROS_ERROR("%s", errorMessage.what()); return false; } - Eigen::Affine3d transform; - poseTFToEigen(transformTf, transform); - pcl::transformPointCloud(*pointCloud, *pointCloudTransformed, transform.cast()); + Transform transform; + kindr_ros::convertFromRosGeometryMsg(transformStamped.transform, transform); + pcl::transformPointCloud(*pointCloud, *pointCloudTransformed, transform.getTransformationMatrix().cast()); pointCloudTransformed->header.frame_id = targetFrame; ROS_DEBUG("Point cloud transformed to frame %s for time stamp %f.", targetFrame.c_str(), diff --git a/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp b/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp index b0a81099..c10f3690 100644 --- a/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp +++ b/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp @@ -12,8 +12,8 @@ namespace elevation_mapping { -StereoSensorProcessor::StereoSensorProcessor(ros::NodeHandle & nodeHandle, tf::TransformListener & transformListener) - : SensorProcessorBase(nodeHandle, transformListener) +StereoSensorProcessor::StereoSensorProcessor(ros::NodeHandle & nodeHandle, tf2_ros::Buffer& tfBuffer) + : SensorProcessorBase(nodeHandle, tfBuffer) { } @@ -58,15 +58,15 @@ bool StereoSensorProcessor::computeVariances( const Eigen::RowVector3f projectionVector = Eigen::RowVector3f::UnitZ(); // Sensor Jacobian (J_s). - const Eigen::RowVector3f sensorJacobian = projectionVector * (rotationMapToBase_.transposed() * rotationBaseToSensor_.transposed()).toImplementation().cast(); + const Eigen::RowVector3f sensorJacobian = projectionVector * (rotationBaseToMap_ * rotationSensorToBase_).toImplementation().cast(); // Robot rotation covariance matrix (Sigma_q). Eigen::Matrix3f rotationVariance = robotPoseCovariance.bottomRightCorner(3, 3).cast(); // Preparations for#include robot rotation Jacobian (J_q) to minimize computation for every point in point cloud. - const Eigen::Matrix3f C_BM_transpose = rotationMapToBase_.transposed().toImplementation().cast(); + const Eigen::Matrix3f C_BM_transpose = rotationBaseToMap_.toImplementation().cast(); const Eigen::RowVector3f P_mul_C_BM_transpose = projectionVector * C_BM_transpose; - const Eigen::Matrix3f C_SB_transpose = rotationBaseToSensor_.transposed().toImplementation().cast(); + const Eigen::Matrix3f C_SB_transpose = rotationSensorToBase_.toImplementation().cast(); const Eigen::Matrix3f B_r_BS_skew = kindr::getSkewMatrixFromVector(Eigen::Vector3f(translationBaseToSensorInBaseFrame_.toImplementation().cast())); for (unsigned int i = 0; i < pointCloud->size(); ++i) diff --git a/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp b/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp index 6687620d..6d9ecc76 100644 --- a/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp +++ b/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp @@ -22,8 +22,8 @@ namespace elevation_mapping { * Taken from: Nguyen, C. V., Izadi, S., & Lovell, D., Modeling Kinect Sensor Noise for Improved 3D Reconstruction and Tracking, 2012. */ -StructuredLightSensorProcessor::StructuredLightSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener) - : SensorProcessorBase(nodeHandle, transformListener) +StructuredLightSensorProcessor::StructuredLightSensorProcessor(ros::NodeHandle& nodeHandle, tf2_ros::Buffer& tfBuffer) + : SensorProcessorBase(nodeHandle, tfBuffer) { } @@ -75,15 +75,15 @@ bool StructuredLightSensorProcessor::computeVariances( const Eigen::RowVector3f projectionVector = Eigen::RowVector3f::UnitZ(); // Sensor Jacobian (J_s). - const Eigen::RowVector3f sensorJacobian = projectionVector * (rotationMapToBase_.transposed() * rotationBaseToSensor_.transposed()).toImplementation().cast(); + const Eigen::RowVector3f sensorJacobian = projectionVector * (rotationBaseToMap_ * rotationSensorToBase_).toImplementation().cast(); // Robot rotation covariance matrix (Sigma_q). Eigen::Matrix3f rotationVariance = robotPoseCovariance.bottomRightCorner(3, 3).cast(); // Preparations for robot rotation Jacobian (J_q) to minimize computation for every point in point cloud. - const Eigen::Matrix3f C_BM_transpose = rotationMapToBase_.transposed().toImplementation().cast(); + const Eigen::Matrix3f C_BM_transpose = rotationBaseToMap_.toImplementation().cast(); const Eigen::RowVector3f P_mul_C_BM_transpose = projectionVector * C_BM_transpose; - const Eigen::Matrix3f C_SB_transpose = rotationBaseToSensor_.transposed().toImplementation().cast(); + const Eigen::Matrix3f C_SB_transpose =rotationSensorToBase_.toImplementation().cast(); const Eigen::Matrix3f B_r_BS_skew = kindr::getSkewMatrixFromVector(Eigen::Vector3f(translationBaseToSensorInBaseFrame_.toImplementation().cast())); for (unsigned int i = 0; i < pointCloud->size(); ++i) { @@ -98,12 +98,12 @@ bool StructuredLightSensorProcessor::computeVariances( float measurementDistance = pointVector.z(); // Compute sensor covariance matrix (Sigma_S) with sensor model. - float deviationNormal = sensorParameters_.at("normal_factor_a") - + sensorParameters_.at("normal_factor_b") - * (measurementDistance - sensorParameters_.at("normal_factor_c")) * (measurementDistance - sensorParameters_.at("normal_factor_c")) - + sensorParameters_.at("normal_factor_d") * pow(measurementDistance, sensorParameters_.at("normal_factor_e")); - float varianceNormal = deviationNormal * deviationNormal; - float deviationLateral = sensorParameters_.at("lateral_factor") * measurementDistance; + float deviationNormal = sensorParameters_.at("normal_factor_a") + + sensorParameters_.at("normal_factor_b") * (measurementDistance - sensorParameters_.at("normal_factor_c")) + * (measurementDistance - sensorParameters_.at("normal_factor_c")) + + sensorParameters_.at("normal_factor_d") * pow(measurementDistance, sensorParameters_.at("normal_factor_e")); + float varianceNormal = deviationNormal * deviationNormal; + float deviationLateral = sensorParameters_.at("lateral_factor") * measurementDistance; float varianceLateral = deviationLateral * deviationLateral; Eigen::Matrix3f sensorVariance = Eigen::Matrix3f::Zero(); sensorVariance.diagonal() << varianceLateral, varianceLateral, varianceNormal;