Skip to content
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

Jump to bottom

Feature/tf2 ros #46

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Feature/tf2 ros #46

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
32345d3
Bump to tf2ros.
rikba Mar 31, 2021
7ea895b
Compilation.
rikba Mar 31, 2021
846a335
Merge branch 'master' into feature/tf2_ros
rikba Mar 31, 2021
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
13 changes: 8 additions & 5 deletions geotf/include/geotf/geodetic_converter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,12 @@
#include <vector>
#include <memory>
#include <map>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_eigen/tf2_eigen.h>
#include <ros/ros.h>
#include <tf_conversions/tf_eigen.h>

namespace geotf {
class GeodeticConverter {
Expand Down Expand Up @@ -135,8 +137,9 @@ class GeodeticConverter {
// Note: Geoframe must be a cartesian frame.
boost::optional< std::pair<std::string, std::string>> tf_mapping_;

std::shared_ptr<tf::TransformListener> listener_;
std::shared_ptr<tf::TransformBroadcaster> broadcaster_;
std::shared_ptr<tf2_ros::Buffer> buffer_;
std::shared_ptr<tf2_ros::TransformListener> listener_;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;

};
}
Expand Down
3 changes: 2 additions & 1 deletion geotf/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,8 @@
<!-- Dependencies. -->
<depend>roslib</depend>
<depend>roscpp</depend>
<depend>tf</depend>
<depend>tf2_ros</depend>
<depend>tf2_eigen</depend>

<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
Expand Down
44 changes: 21 additions & 23 deletions geotf/src/geodetic_converter.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include <geotf/geodetic_converter.h>

namespace geotf {
// Initialize frame definitions from rosparams
void GeodeticConverter::initFromRosParam(const std::string& prefix) {
Expand Down Expand Up @@ -78,8 +79,9 @@ void GeodeticConverter::initFromRosParam(const std::string& prefix) {
ROS_WARN_STREAM("[GeoTF] No TF connection specified.");
}

listener_ = std::make_shared<tf::TransformListener>();
broadcaster_ = std::make_shared<tf::TransformBroadcaster>();
buffer_ = std::make_shared<tf2_ros::Buffer>();
listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
}

// Adds a coordinate frame by its EPSG identifier
Expand Down Expand Up @@ -324,22 +326,21 @@ bool GeodeticConverter::convertToTf(const std::string& geo_input_frame,
if (!result) { return false; }

// Convert from tf_connection_frame to tf_output_frame.
if (!listener_->canTransform(tf_output_frame, tf_connection_frame, time)) {
if (!buffer_->canTransform(tf_output_frame, tf_connection_frame, time)) {
return false;
}

tf::StampedTransform tf_T_O_C; // transform connection to output.
Eigen::Affine3d eigen_T_O_C;
geometry_msgs::TransformStamped tf_T_O_C; // transform connection to output.
try {
listener_->lookupTransform(tf_output_frame,
tf_connection_frame,
time, tf_T_O_C);
tf_T_O_C = buffer_->lookupTransform(tf_output_frame,
tf_connection_frame,
time);
} catch (std::exception& ex) {
ROS_WARN_STREAM("[GeoTF] Error in tf connection" << ex.what());
return false;
}

tf::transformTFToEigen(tf_T_O_C, eigen_T_O_C);
auto eigen_T_O_C = tf2::transformToEigen(tf_T_O_C);

*output = eigen_T_O_C * tf_connection_value;
return true;
Expand Down Expand Up @@ -376,11 +377,10 @@ bool GeodeticConverter::publishAsTf(const std::string& geo_input_frame,
return false;
}

tf::StampedTransform tf_input;
tf::transformEigenToTF(input_connection, tf_input);
tf_input.stamp_ = ros::Time::now();
tf_input.frame_id_ = tf_connection_frame;
tf_input.child_frame_id_ = frame_name;
auto tf_input = tf2::eigenToTransform(input_connection);
tf_input.header.stamp = ros::Time::now();
tf_input.header.frame_id = tf_connection_frame;
tf_input.child_frame_id = frame_name;
broadcaster_->sendTransform(tf_input);
return true;
}
Expand All @@ -402,24 +402,22 @@ bool GeodeticConverter::convertFromTf(const std::string& tf_input_frame,
std::string geotf_connection_frame = tf_mapping_->first;

//Convert from tf_input_frame to tf_connection_frame
if (!listener_->canTransform(tf_connection_frame, tf_input_frame, time)) {
if (!buffer_->canTransform(tf_connection_frame, tf_input_frame, time)) {
return false;
}


// add exception handling etc.
tf::StampedTransform tf_T_C_I; // transform input to connection
Eigen::Affine3d eigen_T_C_I;

geometry_msgs::TransformStamped tf_T_C_I; // transform input to connection
try {
listener_->lookupTransform(tf_connection_frame,
tf_input_frame,
time, tf_T_C_I);
tf_T_C_I = buffer_->lookupTransform(tf_connection_frame,
tf_input_frame,
time);
} catch (std::exception& ex) {
ROS_WARN_STREAM("[GeoTF] Error in tf connection" << ex.what());
return false;
}
tf::transformTFToEigen(tf_T_C_I, eigen_T_C_I);
auto eigen_T_C_I = tf2::transformToEigen(tf_T_C_I);

tf_connection_value = eigen_T_C_I * input;

Expand Down Expand Up @@ -470,4 +468,4 @@ bool GeodeticConverter::checkTransform(const std::string& input_frame,
transforms_.insert(std::make_pair(tf_id, transform));
return true;
}
}
}