Skip to content

Upgrade to tf2 #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
tenzincrouch merged 2 commits into from
Jan 8, 2025
Merged

Upgrade to tf2 #1

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
deec4a5
Replace tf with tf2
ecthelion99 Sep 27, 2024
4dea726
Replace tf with tf2 (demos)
SivertHavso Feb 17, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 7 additions & 5 deletions elevation_mapping/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -20,11 +20,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}
)
Expand Down Expand Up @@ -111,7 +113,7 @@ target_link_libraries(${PROJECT_NAME}
#############

install(
TARGETS
TARGETS
${PROJECT_NAME}
${PROJECT_NAME}_pcl_types
${PROJECT_NAME}_library
Expand Down
8 changes: 5 additions & 3 deletions elevation_mapping/include/elevation_mapping/ElevationMapping.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,8 @@
#include <sensor_msgs/PointCloud2.h>
#include <std_srvs/Empty.h>
#include <std_srvs/Trigger.h>
#include <tf/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

// Eigen
#include <Eigen/Core>
Expand Down Expand Up @@ -298,8 +299,9 @@ class ElevationMapping {
//! Cache for the robot pose messages.
message_filters::Cache<geometry_msgs::PoseWithCovarianceStamped> robotPoseCache_;

//! TF listener and broadcaster.
tf::TransformListener transformListener_;
//! TF listener and buffer.
tf2_ros::Buffer transformBuffer_;
tf2_ros::TransformListener transformListener_;

struct Parameters {
//! Size of the cache for the robot pose messages.
Expand Down
8 changes: 5 additions & 3 deletions elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,8 @@

// ROS
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

// Eigen
#include <Eigen/Core>
Expand Down Expand Up @@ -139,8 +140,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_;
Expand Down
6 changes: 4 additions & 2 deletions elevation_mapping/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,10 @@
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>tf</depend>
<depend>tf_conversions</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_eigen</depend>
<depend>tf2_geometry_msgs</depend>

<!-- <test_depend>cmake_code_coverage</test_depend> -->
<test_depend>grid_map_filters</test_depend>
Expand Down
19 changes: 13 additions & 6 deletions elevation_mapping/src/ElevationMapping.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,10 @@
#include <grid_map_ros/grid_map_ros.hpp>
#include <kindr/Core>
#include <kindr_ros/kindr_ros.hpp>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include "elevation_mapping/ElevationMap.hpp"
#include "elevation_mapping/ElevationMapping.hpp"
Expand All @@ -35,6 +39,7 @@ namespace elevation_mapping {
ElevationMapping::ElevationMapping(ros::NodeHandle& nodeHandle)
: nodeHandle_(nodeHandle),
inputSources_(nodeHandle_),
transformListener_(transformBuffer_),
map_(nodeHandle),
robotMotionMapUpdater_(nodeHandle),
receivedFirstMatchingPointcloudAndPose_(false) {
Expand Down Expand Up @@ -542,8 +547,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;
}
Expand Down Expand Up @@ -627,12 +632,14 @@ bool ElevationMapping::initializeElevationMap() {
if (parameters.initializeElevationMap_) {
if (static_cast<elevation_mapping::InitializationMethods>(parameters.initializationMethod_) ==
elevation_mapping::InitializationMethods::PlanarFloorInitializer) {
tf::StampedTransform transform;
geometry_msgs::TransformStamped transform_msg;
tf2::Stamped<tf2::Transform> transform;

// Listen to transform between mapFrameId_ and targetFrameInitSubmap_ and use z value for initialization
try {
transformListener_.waitForTransform(parameters.mapFrameId_, parameters.targetFrameInitSubmap_, ros::Time(0), ros::Duration(5.0));
transformListener_.lookupTransform(parameters.mapFrameId_, parameters.targetFrameInitSubmap_, ros::Time(0), transform);
transform_msg = transformBuffer_.lookupTransform(parameters.mapFrameId_, parameters.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());

Expand All @@ -645,7 +652,7 @@ bool ElevationMapping::initializeElevationMap() {
map_.setRawSubmapHeight(positionRobot, transform.getOrigin().z() + parameters.initSubmapHeightOffset_,
parameters.initSubmapVariance_, parameters.lengthInXInitSubmap_, parameters.lengthInYInitSubmap_);
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;
Expand Down
67 changes: 36 additions & 31 deletions elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,9 @@

#include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp"

// ROS
#include <geometry_msgs/TransformStamped.h>

// PCL
#include <pcl/common/io.h>
#include <pcl/common/transforms.h>
Expand All @@ -17,7 +20,7 @@
#include <pcl/pcl_base.h>

// TF
#include <tf_conversions/tf_eigen.h>
#include <tf2_eigen/tf2_eigen.h>

// STL
#include <cmath>
Expand All @@ -29,7 +32,9 @@
namespace elevation_mapping {

SensorProcessorBase::SensorProcessorBase(ros::NodeHandle& nodeHandle, const GeneralParameters& generalConfig)
: nodeHandle_(nodeHandle), firstTfAvailable_(false) {
: nodeHandle_(nodeHandle),
transformListener_(transformBuffer_),
firstTfAvailable_(false) {
pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
transformationSensorToMap_.setIdentity();
generalParameters_ = generalConfig;
Expand Down Expand Up @@ -92,31 +97,33 @@ bool SensorProcessorBase::process(const PointCloudType::ConstPtr pointCloudInput
bool SensorProcessorBase::updateTransformations(const ros::Time& timeStamp) {
const Parameters parameters{parameters_.getData()};
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?
Copy link

@tenzincrouch tenzincrouch Oct 6, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ecthelion99 - What is this TODO? Did you put it here or was it in existing code?

Copy link
Collaborator Author

@ecthelion99 ecthelion99 Oct 6, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These were all existing

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?
Copy link

@tenzincrouch tenzincrouch Oct 6, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ecthelion99 - same question.

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;
}
Expand All @@ -131,22 +138,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
Copy link

@tenzincrouch tenzincrouch Oct 6, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What about this one?

Eigen::Affine3d transform = tf2::transformToEigen(transformGeom);
pcl::transformPointCloud(*pointCloud, *pointCloudTransformed, transform.cast<float>());
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<float>());
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;
}

Expand Down