diff --git a/README.md b/README.md index 1a27d519..2023a98f 100644 --- a/README.md +++ b/README.md @@ -145,7 +145,7 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens The robot pose and covariance. -* **`/tf`** ([tf/tfMessage]) +* **`/tf`** ([tf2_msgs/TFMessage]) The transformation tree. @@ -400,7 +400,7 @@ Please report bugs and request features using the [Issue Tracker](https://github [grid_map_msgs/GridMap]: https://github.com/anybotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg [sensor_msgs/PointCloud2]: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html [geometry_msgs/PoseWithCovarianceStamped]: http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html -[tf/tfMessage]: http://docs.ros.org/kinetic/api/tf/html/msg/tfMessage.html +[tf2_msgs/TFMessage]: http://docs.ros.org/en/noetic/api/tf2_msgs/html/msg/TFMessage.html [std_srvs/Empty]: http://docs.ros.org/api/std_srvs/html/srv/Empty.html [grid_map_msgs/GetGridMap]: https://github.com/anybotics/grid_map/blob/master/grid_map_msgs/srv/GetGridMap.srv [grid_map_msgs/ProcessFile]: https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/srv/ProcessFile.srv diff --git a/elevation_mapping/CMakeLists.txt b/elevation_mapping/CMakeLists.txt index 121099ca..2d9efc05 100644 --- a/elevation_mapping/CMakeLists.txt +++ b/elevation_mapping/CMakeLists.txt @@ -7,7 +7,7 @@ add_compile_options(-Wall -Wextra -Wpedantic) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set( - CATKIN_PACKAGE_DEPENDENCIES + CATKIN_PACKAGE_DEPENDENCIES eigen_conversions grid_map_core grid_map_ros @@ -19,11 +19,13 @@ set( roscpp sensor_msgs std_msgs - tf - tf_conversions + tf2 + tf2_ros + tf2_eigen + tf2_geometry_msgs ) -find_package(catkin REQUIRED +find_package(catkin REQUIRED COMPONENTS ${CATKIN_PACKAGE_DEPENDENCIES} ) @@ -102,7 +104,7 @@ target_link_libraries(${PROJECT_NAME} ############# install( - TARGETS + TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_pcl_types ${PROJECT_NAME}_library diff --git a/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp b/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp index 9ca5cd4f..51b0e9af 100644 --- a/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp +++ b/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp @@ -20,7 +20,8 @@ #include #include #include -#include +#include +#include // Eigen #include @@ -291,8 +292,9 @@ class ElevationMapping { //! Frame ID of the elevation map std::string mapFrameId_; - //! TF listener and broadcaster. - tf::TransformListener transformListener_; + //! TF listener and buffer. + tf2_ros::Buffer transformBuffer_; + tf2_ros::TransformListener transformListener_; //! Point which the elevation map follows. kindr::Position3D trackPoint_; diff --git a/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp b/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp index 54fe221c..d7271754 100644 --- a/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp +++ b/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp @@ -10,7 +10,8 @@ // ROS #include -#include +#include +#include // Eigen #include @@ -138,8 +139,9 @@ class SensorProcessorBase { //! ROS nodehandle. ros::NodeHandle& nodeHandle_; - //! TF transform listener. - tf::TransformListener transformListener_; + //! TF transform listener and buffer. + tf2_ros::Buffer transformBuffer_; + tf2_ros::TransformListener transformListener_; //! Rotation from Base to Sensor frame (C_SB) kindr::RotationMatrixD rotationBaseToSensor_; diff --git a/elevation_mapping/package.xml b/elevation_mapping/package.xml index b006fc57..853cadcf 100644 --- a/elevation_mapping/package.xml +++ b/elevation_mapping/package.xml @@ -27,8 +27,10 @@ roscpp sensor_msgs std_msgs - tf - tf_conversions + tf2 + tf2_ros + tf2_eigen + tf2_geometry_msgs grid_map_filters diff --git a/elevation_mapping/src/ElevationMapping.cpp b/elevation_mapping/src/ElevationMapping.cpp index a548259e..9c30f911 100644 --- a/elevation_mapping/src/ElevationMapping.cpp +++ b/elevation_mapping/src/ElevationMapping.cpp @@ -20,6 +20,10 @@ #include #include #include +#include +#include +#include +#include #include "elevation_mapping/ElevationMap.hpp" #include "elevation_mapping/ElevationMapping.hpp" @@ -35,6 +39,7 @@ ElevationMapping::ElevationMapping(ros::NodeHandle& nodeHandle) : nodeHandle_(nodeHandle), inputSources_(nodeHandle_), robotPoseCacheSize_(200), + transformListener_(transformBuffer_), map_(nodeHandle), robotMotionMapUpdater_(nodeHandle), ignoreRobotMotionUpdates_(false), @@ -531,8 +536,8 @@ bool ElevationMapping::updateMapLocation() { geometry_msgs::PointStamped trackPointTransformed; try { - transformListener_.transformPoint(map_.getFrameId(), trackPoint, trackPointTransformed); - } catch (tf::TransformException& ex) { + trackPointTransformed = transformBuffer_.transform(trackPoint, map_.getFrameId()); + } catch (tf2::TransformException& ex) { ROS_ERROR("%s", ex.what()); return false; } @@ -613,12 +618,14 @@ bool ElevationMapping::initializeElevationMap() { if (initializeElevationMap_) { if (static_cast(initializationMethod_) == elevation_mapping::InitializationMethods::PlanarFloorInitializer) { - tf::StampedTransform transform; + geometry_msgs::TransformStamped transform_msg; + tf2::Stamped transform; // Listen to transform between mapFrameId_ and targetFrameInitSubmap_ and use z value for initialization try { - transformListener_.waitForTransform(mapFrameId_, targetFrameInitSubmap_, ros::Time(0), ros::Duration(5.0)); - transformListener_.lookupTransform(mapFrameId_, targetFrameInitSubmap_, ros::Time(0), transform); + transform_msg = transformBuffer_.lookupTransform(mapFrameId_, targetFrameInitSubmap_, ros::Time(0), ros::Duration(5.0)); + tf2::fromMsg(transform_msg, transform); + ROS_DEBUG_STREAM("Initializing with x: " << transform.getOrigin().x() << " y: " << transform.getOrigin().y() << " z: " << transform.getOrigin().z()); @@ -631,7 +638,7 @@ bool ElevationMapping::initializeElevationMap() { map_.setRawSubmapHeight(positionRobot, transform.getOrigin().z() + initSubmapHeightOffset_, lengthInXInitSubmap_, lengthInYInitSubmap_, marginInitSubmap_); return true; - } catch (tf::TransformException& ex) { + } catch (tf2::TransformException& ex) { ROS_DEBUG("%s", ex.what()); ROS_WARN("Could not initialize elevation map with constant height. (This warning can be ignored if TF tree is not available.)"); return false; diff --git a/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp b/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp index 536c91f5..5081bfc6 100644 --- a/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp +++ b/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp @@ -8,6 +8,9 @@ #include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp" +// ROS +#include + // PCL #include #include @@ -17,7 +20,7 @@ #include // TF -#include +#include // STL #include @@ -30,6 +33,7 @@ namespace elevation_mapping { SensorProcessorBase::SensorProcessorBase(ros::NodeHandle& nodeHandle, const GeneralParameters& generalConfig) : nodeHandle_(nodeHandle), + transformListener_(transformBuffer_), ignorePointsUpperThreshold_(std::numeric_limits::infinity()), ignorePointsLowerThreshold_(-std::numeric_limits::infinity()), applyVoxelGridFilter_(false), @@ -90,31 +94,33 @@ bool SensorProcessorBase::process(const PointCloudType::ConstPtr pointCloudInput bool SensorProcessorBase::updateTransformations(const ros::Time& timeStamp) { try { - transformListener_.waitForTransform(sensorFrameId_, generalParameters_.mapFrameId_, timeStamp, ros::Duration(1.0)); - - tf::StampedTransform transformTf; - transformListener_.lookupTransform(generalParameters_.mapFrameId_, sensorFrameId_, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationSensorToMap_); - transformListener_.lookupTransform(generalParameters_.robotBaseFrameId_, sensorFrameId_, timeStamp, - transformTf); // TODO(max): Why wrong direction? - Eigen::Affine3d transform; - poseTFToEigen(transformTf, transform); - rotationBaseToSensor_.setMatrix(transform.rotation().matrix()); - translationBaseToSensorInBaseFrame_.toImplementation() = transform.translation(); - - transformListener_.lookupTransform(generalParameters_.mapFrameId_, generalParameters_.robotBaseFrameId_, timeStamp, - transformTf); // TODO(max): Why wrong direction? - poseTFToEigen(transformTf, transform); - rotationMapToBase_.setMatrix(transform.rotation().matrix()); - translationMapToBaseInMapFrame_.toImplementation() = transform.translation(); + geometry_msgs::TransformStamped transformGeom; + transformGeom = transformBuffer_.lookupTransform(generalParameters_.mapFrameId_, sensorFrameId_, timeStamp, ros::Duration(1.0)); + transformationSensorToMap_ = tf2::transformToEigen(transformGeom); + + transformGeom = transformBuffer_.lookupTransform(generalParameters_.robotBaseFrameId_, sensorFrameId_, timeStamp, + ros::Duration(1.0)); // TODO(max): Why wrong direction? + Eigen::Quaterniond rotationQuaternion; + tf2::fromMsg(transformGeom.transform.rotation, rotationQuaternion); + rotationBaseToSensor_.setMatrix(rotationQuaternion.toRotationMatrix()); + Eigen::Vector3d translationVector; + tf2::fromMsg(transformGeom.transform.translation, translationVector); + translationBaseToSensorInBaseFrame_.toImplementation() = translationVector; + + transformGeom = transformBuffer_.lookupTransform(generalParameters_.mapFrameId_, generalParameters_.robotBaseFrameId_, + timeStamp, ros::Duration(1.0)); // TODO(max): Why wrong direction? + tf2::fromMsg(transformGeom.transform.rotation, rotationQuaternion); + rotationMapToBase_.setMatrix(rotationQuaternion.toRotationMatrix()); + tf2::fromMsg(transformGeom.transform.translation, translationVector); + translationMapToBaseInMapFrame_.toImplementation() = translationVector; if (!firstTfAvailable_) { firstTfAvailable_ = true; } return true; - } catch (tf::TransformException& ex) { + } catch (tf2::TransformException& ex) { if (!firstTfAvailable_) { return false; } @@ -129,22 +135,20 @@ bool SensorProcessorBase::transformPointCloud(PointCloudType::ConstPtr pointClou timeStamp.fromNSec(1000 * pointCloud->header.stamp); const std::string inputFrameId(pointCloud->header.frame_id); - tf::StampedTransform transformTf; try { - transformListener_.waitForTransform(targetFrame, inputFrameId, timeStamp, ros::Duration(1.0), ros::Duration(0.001)); - transformListener_.lookupTransform(targetFrame, inputFrameId, timeStamp, transformTf); - } catch (tf::TransformException& ex) { + geometry_msgs::TransformStamped transformGeom; + transformGeom = transformBuffer_.lookupTransform(targetFrame, inputFrameId, timeStamp, ros::Duration(1.0)); // FIXME: missing 0.001 retry duration + Eigen::Affine3d transform = tf2::transformToEigen(transformGeom); + pcl::transformPointCloud(*pointCloud, *pointCloudTransformed, transform.cast()); + pointCloudTransformed->header.frame_id = targetFrame; + + ROS_DEBUG_THROTTLE(5, "Point cloud transformed to frame %s for time stamp %f.", targetFrame.c_str(), + pointCloudTransformed->header.stamp / 1000.0); + } catch (tf2::TransformException& ex) { ROS_ERROR("%s", ex.what()); return false; } - Eigen::Affine3d transform; - poseTFToEigen(transformTf, transform); - pcl::transformPointCloud(*pointCloud, *pointCloudTransformed, transform.cast()); - pointCloudTransformed->header.frame_id = targetFrame; - - ROS_DEBUG_THROTTLE(5, "Point cloud transformed to frame %s for time stamp %f.", targetFrame.c_str(), - pointCloudTransformed->header.stamp / 1000.0); return true; } diff --git a/elevation_mapping_demos/launch/simple_demo.launch b/elevation_mapping_demos/launch/simple_demo.launch index 8a06a583..9fc6f8c6 100644 --- a/elevation_mapping_demos/launch/simple_demo.launch +++ b/elevation_mapping_demos/launch/simple_demo.launch @@ -16,7 +16,7 @@ - + diff --git a/elevation_mapping_demos/launch/turtlesim3_waffle_demo.launch b/elevation_mapping_demos/launch/turtlesim3_waffle_demo.launch index f80cc178..85c5c869 100644 --- a/elevation_mapping_demos/launch/turtlesim3_waffle_demo.launch +++ b/elevation_mapping_demos/launch/turtlesim3_waffle_demo.launch @@ -15,6 +15,7 @@ + diff --git a/elevation_mapping_demos/package.xml b/elevation_mapping_demos/package.xml index 1e5e2ec2..540f506d 100644 --- a/elevation_mapping_demos/package.xml +++ b/elevation_mapping_demos/package.xml @@ -20,7 +20,7 @@ pcl_ros point_cloud_io robot_state_publisher - tf + tf2_ros turtlebot3_gazebo xacro diff --git a/elevation_mapping_demos/scripts/tf_to_pose_publisher.py b/elevation_mapping_demos/scripts/tf_to_pose_publisher.py index 8a4c2f1a..d53a87cc 100755 --- a/elevation_mapping_demos/scripts/tf_to_pose_publisher.py +++ b/elevation_mapping_demos/scripts/tf_to_pose_publisher.py @@ -2,33 +2,25 @@ import rospy import geometry_msgs.msg -import tf +import tf2_ros def callback(newPose): """Listens to a transform between from_frame and to_frame and publishes it as a pose with a zero covariance.""" - global publisher, tf_listener, from_frame, to_frame + global publisher, tf_buffer, tf_listener, from_frame, to_frame # Listen to transform and throw exception if the transform is not # available. try: - (trans, rot) = tf_listener.lookupTransform( - from_frame, to_frame, rospy.Time(0)) - except (tf.LookupException, tf.ConnectivityException, - tf.ExtrapolationException): + trans = tf_buffer.lookup_transform(from_frame, to_frame, rospy.Time()) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): return # Create and fill pose message for publishing pose = geometry_msgs.msg.PoseWithCovarianceStamped() - pose.header.stamp = rospy.Time(0) - pose.header.frame_id = from_frame - pose.pose.pose.position.x = trans[0] - pose.pose.pose.position.y = trans[1] - pose.pose.pose.position.z = trans[2] - pose.pose.pose.orientation.x = rot[0] - pose.pose.pose.orientation.y = rot[1] - pose.pose.pose.orientation.z = rot[2] - pose.pose.pose.orientation.w = rot[3] + pose.header = trans.header + pose.pose.pose.position = trans.transform.translation + pose.pose.pose.orientation = trans.transform.rotation # Since tf transforms do not have a covariance, pose is filled with # a zero covariance. @@ -45,14 +37,15 @@ def callback(newPose): def main_program(): """ Main function initializes node and subscribers and starts the ROS loop. """ - global publisher, tf_listener, from_frame, to_frame + global publisher, tf_buffer, tf_listener, from_frame, to_frame rospy.init_node('tf_to_pose_publisher') # Read frame id's for tf listener from_frame = rospy.get_param("~from_frame") to_frame = rospy.get_param("~to_frame") pose_name = str(to_frame) + "_pose" - tf_listener = tf.TransformListener() + tf_buffer = tf2_ros.Buffer() + tf_listener = tf2_ros.TransformListener(tf_buffer) publisher = rospy.Publisher( pose_name, geometry_msgs.msg.PoseWithCovarianceStamped, queue_size=10)