From 8d321e20e4ac479eec263d192dbd201beb49a117 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 14 Dec 2021 22:23:15 +0100
Subject: [PATCH] using static transform for odom-map and small tf bug
---
CMakeLists.txt | 1 +
include/publisher_tf.h | 6 +++++-
package.xml | 3 +++
src/publisher_tf.cpp | 47 ++++++++++++++++++++++++++++++++----------
4 files changed, 45 insertions(+), 12 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9de45f7..af8c10f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
tf
tf_conversions
+ tf2_ros
# dynamic_reconfigure
)
diff --git a/include/publisher_tf.h b/include/publisher_tf.h
index c3e4ce7..9930c48 100644
--- a/include/publisher_tf.h
+++ b/include/publisher_tf.h
@@ -35,6 +35,7 @@
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
namespace wolf
{
@@ -62,9 +63,12 @@ class PublisherTf: public Publisher
protected:
std::string base_frame_id_, odom_frame_id_, map_frame_id_;
tf::TransformBroadcaster tfb_;
+ tf2_ros::StaticTransformBroadcaster stfb_;
+
tf::TransformListener tfl_;
- tf::StampedTransform T_odom2base_, T_map2odom_;
+ tf::StampedTransform T_odom2base_;//, T_map2odom_;
+ geometry_msgs::TransformStamped Tmsg_map2odom_;
bool publish_odom_tf_;
bool state_available_; // used to not repeat warnings regarding availability of state
diff --git a/package.xml b/package.xml
index cbdaa5d..59bc1f4 100644
--- a/package.xml
+++ b/package.xml
@@ -53,15 +53,18 @@
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
+ <build_depend>tf2_ros</build_depend>
<!-- <build_depend>roslib</build_depend> -->
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>tf</build_export_depend>
+ <build_export_depend>tf2_ros</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
+ <exec_depend>tf2_ros</exec_depend>
<!-- <exec_depend>roslib</exec_depend> -->
<!-- The export tag contains other, unspecified, tags -->
diff --git a/src/publisher_tf.cpp b/src/publisher_tf.cpp
index b210ad8..627aaef 100644
--- a/src/publisher_tf.cpp
+++ b/src/publisher_tf.cpp
@@ -47,10 +47,13 @@ PublisherTf::PublisherTf(const std::string& _unique_name,
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();
+ //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_map2odom_.child_frame_id = odom_frame_id_;
+ Tmsg_map2odom_.header.frame_id = map_frame_id_;
+ Tmsg_map2odom_.header.stamp = ros::Time::now();
}
void PublisherTf::initialize(ros::NodeHandle& nh, const std::string& topic)
@@ -96,9 +99,14 @@ void PublisherTf::publishDerived()
if (tfl_.getLatestCommonTime(odom_frame_id_, base_frame_id_, transform_time, &error_msg) == tf::ErrorValues::NO_ERROR and
transform_time != T_odom2base_.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());
+
+ tfl_.lookupTransform(odom_frame_id_, base_frame_id_, ros::Time(0), T_odom2base_);
+
+ publish_odom_tf_=false;
+
}
else
{
@@ -118,7 +126,9 @@ void PublisherTf::publishDerived()
}
catch(...)
{
- ROS_WARN("No %s to %s frame received. Assuming identity.", base_frame_id_.c_str(), odom_frame_id_.c_str());
+ ROS_WARN("No %s to %s frame received. Assuming identity.",
+ base_frame_id_.c_str(),
+ odom_frame_id_.c_str());
T_odom2base_.setIdentity();
T_odom2base_.stamp_ = ros::Time::now();
}
@@ -126,10 +136,25 @@ void PublisherTf::publishDerived()
// Broadcast transform ---------------------------------------------------------------------------
- T_map2odom_.setData(T_map2base * T_odom2base_.inverse());
- T_map2odom_.stamp_ = ros::Time::now();
+ //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_);
+ //tfb_.sendTransform(T_map2odom_);
+
+ tf::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