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.

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

Feature/tf2 ros #46

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open
Changes from 1 commit
Commits
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
Prev Previous commit
Next Next commit
Compilation.
  • Loading branch information
rikba committed Mar 31, 2021
commit 7ea895b496888fa9b47fe893127828dd40ef051f
6 changes: 4 additions & 2 deletions geotf/include/geotf/geodetic_converter.h
Original file line number Diff line number Diff line change
@@ -13,10 +13,12 @@
#include <vector>
#include <memory>
#include <map>
#include <tf2_ros/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 {
1 change: 1 addition & 0 deletions geotf/package.xml
Original file line number Diff line number Diff line change
@@ -16,6 +16,7 @@
<depend>roslib</depend>
<depend>roscpp</depend>
<depend>tf2_ros</depend>
<depend>tf2_eigen</depend>

<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
33 changes: 15 additions & 18 deletions geotf/src/geodetic_converter.cc
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) {
@@ -321,18 +322,17 @@ bool GeodeticConverter::convertToTf(const std::string& geo_input_frame,
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 {
buffer_->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;
@@ -369,11 +369,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;
}
@@ -401,18 +400,16 @@ bool GeodeticConverter::convertFromTf(const std::string& tf_input_frame,


// 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 {
buffer_->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;