From 7d1f31d399f59efabbcbe074af38823610b7960b Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 14 Dec 2021 22:09:18 +0100
Subject: [PATCH] wip
---
CMakeLists.txt | 3 ++
include/publisher_tf.h | 31 ++++++++++------
src/publisher_tf.cpp | 81 ++++++++++++++++++++++++++++--------------
3 files changed, 78 insertions(+), 37 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9de45f7..52bf097 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,6 +13,9 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
sensor_msgs
std_msgs
+ geometry_msgs
+ tf2
+ tf2_ros
tf
tf_conversions
# dynamic_reconfigure
diff --git a/include/publisher_tf.h b/include/publisher_tf.h
index c3e4ce7..ddc09b6 100644
--- a/include/publisher_tf.h
+++ b/include/publisher_tf.h
@@ -33,38 +33,49 @@
* ROS includes *
**************************/
#include <ros/ros.h>
-#include <tf/transform_listener.h>
-#include <tf/transform_broadcaster.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf2/utils.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
namespace wolf
{
-tf::Transform stateToTfTransform(const VectorComposite& state, const int& dim)
+tf2::Transform stateToTfTransform(const VectorComposite& state, const int& dim)
{
assert(state.includesStructure("PO"));
+ tf2::Transform t;
+
// 2D
if (dim == 2)
{
- return tf::Transform (tf::createQuaternionFromYaw(state.at('O')(0)),
- tf::Vector3(state.at('P')(0), state.at('P')(1), 0) );
+ t.getRotation().setRPY(0, 0, state.at('O')(0));
+ t.setOrigin( tf2::Vector3(state.at('P')(0), state.at('P')(1), 0) );
}
// 3D
else
{
- return tf::Transform (tf::Quaternion(state.at('O')(0), state.at('O')(1), state.at('O')(2), state.at('O')(3)),
- tf::Vector3(state.at('P')(0), state.at('P')(1), state.at('P')(2)) );
+ t.setRotation(tf2::Quaternion(state.at('O')(0), state.at('O')(1), state.at('O')(2), state.at('O')(3)) );
+ t.setOrigin( tf2::Vector3(state.at('P')(0), state.at('P')(1), state.at('P')(2)) );
}
+
+ return t;
}
class PublisherTf: public Publisher
{
protected:
std::string base_frame_id_, odom_frame_id_, map_frame_id_;
- tf::TransformBroadcaster tfb_;
- tf::TransformListener tfl_;
+ tf2_ros::StaticTransformBroadcaster stfb_;
+ tf2_ros::TransformBroadcaster tfb_;
+ tf2_ros::TransformListener tfl_;
+ tf2_ros::Buffer tf_buffer_;
- tf::StampedTransform T_odom2base_, T_map2odom_;
+ geometry_msgs::TransformStamped Tmsg_map2odom_, Tmsg_odom2base_;
+ tf2::Transform T_map2odom_, T_odom2base_;
bool publish_odom_tf_;
bool state_available_; // used to not repeat warnings regarding availability of state
diff --git a/src/publisher_tf.cpp b/src/publisher_tf.cpp
index b210ad8..1e4b748 100644
--- a/src/publisher_tf.cpp
+++ b/src/publisher_tf.cpp
@@ -25,8 +25,10 @@
* ROS includes *
**************************/
#include <ros/ros.h>
-#include "tf/transform_datatypes.h"
-#include "tf_conversions/tf_eigen.h"
+//#include "tf/transform_datatypes.h"
+//#include "tf_conversions/tf_eigen.h"
+#include "tf2_msgs/TF2Error.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
namespace wolf
{
@@ -35,6 +37,8 @@ PublisherTf::PublisherTf(const std::string& _unique_name,
const ParamsServer& _server,
const ProblemPtr _problem) :
Publisher(_unique_name, _server, _problem),
+ tf_buffer_(),
+ tfl_(tf_buffer_),
state_available_(true)
{
map_frame_id_ = _server.getParam<std::string>(prefix_ + "/map_frame_id");
@@ -44,13 +48,15 @@ PublisherTf::PublisherTf(const std::string& _unique_name,
// initialize TF transforms
T_odom2base_.setIdentity();
- T_odom2base_.frame_id_ = odom_frame_id_;
- T_odom2base_.child_frame_id_ = base_frame_id_;
- T_odom2base_.stamp_ = ros::Time::now();
T_map2odom_.setIdentity();
- T_map2odom_.frame_id_ = map_frame_id_;
- T_map2odom_.child_frame_id_ = odom_frame_id_;
- T_map2odom_.stamp_ = ros::Time::now();
+ Tmsg_odom2base_.header.frame_id = odom_frame_id_;
+ Tmsg_odom2base_.child_frame_id = base_frame_id_;
+ Tmsg_odom2base_.header.stamp = ros::Time::now();
+ Tmsg_odom2base_.transform = tf2::toMsg(T_odom2base_);
+ Tmsg_map2odom_.header.frame_id = map_frame_id_;
+ Tmsg_map2odom_.child_frame_id = odom_frame_id_;
+ Tmsg_map2odom_.header.stamp = ros::Time::now();
+ Tmsg_map2odom_.transform = tf2::toMsg(T_map2odom_);
}
void PublisherTf::initialize(ros::NodeHandle& nh, const std::string& topic)
@@ -65,7 +71,7 @@ void PublisherTf::publishDerived()
// MAP - BASE
//Get map2base from wolf result, and builds base2map pose
- tf::Transform T_map2base;
+ tf2::Transform T_map2base;
if (current_state.count('P') == 0 or
current_state.count('O') == 0 or
not current_ts.ok())
@@ -93,20 +99,29 @@ void PublisherTf::publishDerived()
{
ros::Time transform_time;
std::string error_msg;
- if (tfl_.getLatestCommonTime(odom_frame_id_, base_frame_id_, transform_time, &error_msg) == tf::ErrorValues::NO_ERROR and
- transform_time != T_odom2base_.stamp_)
+ if (tf_buffer_._getLatestCommonTime(tf_buffer_._lookupFrameNumber(odom_frame_id_),
+ tf_buffer_._lookupFrameNumber(base_frame_id_),
+ transform_time,
+ &error_msg) == tf2_msgs::TF2Error::NO_ERROR and
+ transform_time != Tmsg_odom2base_.header.stamp)
{
- WOLF_ERROR("PublisherTf: option 'publish_odom_tf' enabled but a transform between ",
- base_frame_id_, " and ", odom_frame_id_,
- " was found published by a third party. Not publishing this transformation.");
+ ROS_WARN("PublisherTf: option 'publish_odom_tf' enabled but a transform between %s and %s was found published by a third party. Changing 'publish_odom_tf' to false.",
+ base_frame_id_.c_str(), odom_frame_id_.c_str());
+
+ Tmsg_odom2base_ = tf_buffer_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0));
+ tf2::fromMsg(Tmsg_odom2base_.transform, T_odom2base_);
+
+ publish_odom_tf_=false;
}
else
{
VectorComposite odom = problem_->getOdometry("PO");
- T_odom2base_.setData(stateToTfTransform(odom, problem_->getDim()));
- T_odom2base_.stamp_ = ros::Time::now();
- tfb_.sendTransform(T_odom2base_);
+ T_odom2base_ = stateToTfTransform(odom, problem_->getDim());
+ Tmsg_odom2base_.transform = tf2::toMsg(T_odom2base_);
+ Tmsg_odom2base_.header.stamp = ros::Time::now();
+
+ tfb_.sendTransform(Tmsg_odom2base_);
}
}
// TF odometry
@@ -114,22 +129,34 @@ void PublisherTf::publishDerived()
{
try
{
- tfl_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0), T_odom2base_);
+ Tmsg_odom2base_ = tf_buffer_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0));
+ tf2::fromMsg(Tmsg_odom2base_.transform, T_odom2base_);
}
- catch(...)
+ catch (tf2::TransformException &ex)
{
- ROS_WARN("No %s to %s frame received. Assuming identity.", base_frame_id_.c_str(), odom_frame_id_.c_str());
+ ROS_WARN("No tf between %s to %s received, error: %s. Assuming identity.",
+ base_frame_id_.c_str(),
+ odom_frame_id_.c_str(),
+ ex.what());
T_odom2base_.setIdentity();
- T_odom2base_.stamp_ = ros::Time::now();
- }
+ }
}
-
// Broadcast transform ---------------------------------------------------------------------------
- T_map2odom_.setData(T_map2base * T_odom2base_.inverse());
- T_map2odom_.stamp_ = ros::Time::now();
- //std::cout << "T_map2odom: " << T_map2odom.getOrigin().getX() << " " << T_map2odom.getOrigin().getY() << " " << T_map2odom.getRotation().getAngle() << std::endl;
- tfb_.sendTransform(T_map2odom_);
+ tf2::Transform T_map2odom = T_map2base * T_odom2base_.inverse();
+
+ Tmsg_map2odom_.transform.translation.x = T_map2odom.getOrigin().getX();
+ Tmsg_map2odom_.transform.translation.y = T_map2odom.getOrigin().getY();
+ Tmsg_map2odom_.transform.translation.z = T_map2odom.getOrigin().getZ();
+
+ Tmsg_map2odom_.transform.rotation.x = T_map2odom.getRotation().getX();
+ Tmsg_map2odom_.transform.rotation.y = T_map2odom.getRotation().getY();
+ Tmsg_map2odom_.transform.rotation.z = T_map2odom.getRotation().getZ();
+ Tmsg_map2odom_.transform.rotation.w = T_map2odom.getRotation().getW();
+
+ Tmsg_map2odom_.header.stamp = ros::Time::now();
+
+ stfb_.sendTransform(Tmsg_map2odom_);
}
}
--
GitLab