From 3d2a84644cde17b65bd5fdb96c1818bebf2ce4ac Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 27 Nov 2019 13:38:10 +0100
Subject: [PATCH] Switch to TF2
---
CMakeLists.txt | 12 ++++++------
include/dynamic_graph_bridge/sot_loader.hh | 6 +++---
package.xml | 4 ++--
src/ros_tf_listener.cpp | 16 +++++++++-------
src/ros_tf_listener.hh | 22 ++++++++++++++--------
src/sot_loader.cpp | 21 ++++++++++++++-------
6 files changed, 48 insertions(+), 33 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index db75b43..eb88c79 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,7 +30,7 @@ include(cmake/test.cmake)
project(dynamic_graph_bridge)
-SET(CATKIN_REQUIRED_COMPONENTS roscpp std_msgs message_generation std_srvs geometry_msgs sensor_msgs tf)
+SET(CATKIN_REQUIRED_COMPONENTS roscpp std_msgs message_generation std_srvs geometry_msgs sensor_msgs tf2_ros)
SET(CATKIN_DEPENDS_LIBRARIES ros_bridge sot_loader)
## LAAS cmake submodule part
@@ -89,7 +89,7 @@ SET(SOT_PKGNAMES
dynamic_graph_bridge_msgs)
add_required_dependency(roscpp)
-add_required_dependency(tf)
+add_required_dependency(tf2_ros)
add_required_dependency("realtime_tools >= 1.8")
add_required_dependency(tf2_bullet)
@@ -178,16 +178,16 @@ ENDIF(BUILD_PYTHON_INTERFACE)
# Stand alone embedded intepreter with a robot controller.
add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp src/sot_loader_basic.cpp)
-pkg_config_use_dependency(geometric_simu tf)
+pkg_config_use_dependency(geometric_simu tf2_ros)
pkg_config_use_dependency(geometric_simu roscpp)
pkg_config_use_dependency(geometric_simu dynamic-graph)
-target_link_libraries(geometric_simu ros_bridge tf ${Boost_LIBRARIES} ${CMAKE_DL_LIBS})
+target_link_libraries(geometric_simu ros_bridge tf2_ros ${Boost_LIBRARIES} ${CMAKE_DL_LIBS})
# Sot loader library
add_library(sot_loader src/sot_loader.cpp src/sot_loader_basic.cpp)
pkg_config_use_dependency(sot_loader dynamic-graph)
pkg_config_use_dependency(sot_loader sot-core)
-target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp ros_bridge tf)
+target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp ros_bridge tf2_ros)
install(TARGETS sot_loader DESTINATION lib)
add_subdirectory(src)
@@ -200,7 +200,7 @@ generate_messages( DEPENDENCIES std_msgs )
# This is necessary so that the pc file generated by catking is similar to the on
# done directly by jrl-cmake-modules
-catkin_package(CATKIN_DEPENDS message_runtime roscpp realtime_tools tf2_bullet ${SOT_PKGNAMES} tf
+catkin_package(CATKIN_DEPENDS message_runtime roscpp realtime_tools tf2_bullet ${SOT_PKGNAMES} tf2_ros
LIBRARIES ${CATKIN_DEPENDS_LIBRARIES}
)
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index e70ac48..60840a0 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -39,7 +39,7 @@
#include "ros/ros.h"
#include "std_srvs/Empty.h"
#include <sensor_msgs/JointState.h>
-#include <tf/transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
// Sot Framework includes
#include <sot/core/debug.hh>
@@ -83,8 +83,8 @@ class SotLoader : public SotLoaderBasic {
virtual void startControlLoop();
// Robot Pose Publisher
- tf::TransformBroadcaster freeFlyerPublisher_;
- tf::Transform freeFlyerPose_;
+ tf2_ros::TransformBroadcaster freeFlyerPublisher_;
+ geometry_msgs::TransformStamped freeFlyerPose_;
public:
SotLoader();
diff --git a/package.xml b/package.xml
index 0dfcefc..52ec228 100644
--- a/package.xml
+++ b/package.xml
@@ -23,7 +23,7 @@
<build_depend>roscpp</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
- <build_depend>tf</build_depend>
+ <build_depend>tf2_ros</build_depend>
<build_depend>realtime_tools</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>message_runtime</build_depend>
@@ -39,7 +39,7 @@
<exec_depend>roscpp</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
- <exec_depend>tf</exec_depend>
+ <exec_depend>tf2_ros</exec_depend>
<exec_depend>realtime_tools</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>message_runtime</exec_depend>
diff --git a/src/ros_tf_listener.cpp b/src/ros_tf_listener.cpp
index da89ca3..8733249 100644
--- a/src/ros_tf_listener.cpp
+++ b/src/ros_tf_listener.cpp
@@ -9,19 +9,21 @@ sot::MatrixHomogeneous& TransformListenerData::getTransform(
sot::MatrixHomogeneous& res, int time) {
static const ros::Time rosTime(0);
try {
- listener.lookupTransform(toFrame, fromFrame, rosTime, transform);
- } catch (const tf::TransformException& ex) {
+ transform = buffer.lookupTransform(toFrame, fromFrame, rosTime);
+ } catch (const tf2::TransformException& ex) {
res.setIdentity();
std::ostringstream oss;
oss << "Enable to get transform at time " << time << ": " << ex.what();
entity->SEND_WARNING_STREAM_MSG(oss.str());
return res;
}
- for (int r = 0; r < 3; ++r) {
- for (int c = 0; c < 3; ++c)
- res.linear()(r, c) = transform.getBasis().getRow(r)[c];
- res.translation()[r] = transform.getOrigin()[r];
- }
+
+ const geometry_msgs::TransformStamped::_transform_type::_rotation_type&
+ quat = transform.transform.rotation;
+ const geometry_msgs::TransformStamped::_transform_type::_translation_type&
+ trans = transform.transform.translation;
+ res.linear() = sot::Quaternion(quat.w, quat.x, quat.y, quat.z).matrix();
+ res.translation() << trans.x, trans.y, trans.z;
return res;
}
} // namespace internal
diff --git a/src/ros_tf_listener.hh b/src/ros_tf_listener.hh
index 6f0b457..adf4b5d 100644
--- a/src/ros_tf_listener.hh
+++ b/src/ros_tf_listener.hh
@@ -3,7 +3,8 @@
#include <boost/bind.hpp>
-#include <tf/transform_listener.h>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/TransformStamped.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal.h>
@@ -19,15 +20,15 @@ struct TransformListenerData {
typedef Signal<sot::MatrixHomogeneous, int> signal_t;
RosTfListener* entity;
- tf::TransformListener& listener;
+ tf2_ros::Buffer& buffer;
const std::string toFrame, fromFrame;
- tf::StampedTransform transform;
+ geometry_msgs::TransformStamped transform;
signal_t signal;
- TransformListenerData(RosTfListener* e, tf::TransformListener& l,
+ TransformListenerData(RosTfListener* e, tf2_ros::Buffer& b,
const std::string& to, const std::string& from,
const std::string& signame)
- : entity(e), listener(l), toFrame(to), fromFrame(from), signal(signame) {
+ : entity(e), buffer(b), toFrame(to), fromFrame(from), signal(signame) {
signal.setFunction(
boost::bind(&TransformListenerData::getTransform, this, _1, _2));
}
@@ -42,7 +43,11 @@ class RosTfListener : public Entity {
public:
typedef internal::TransformListenerData TransformListenerData;
- RosTfListener(const std::string& name) : Entity(name) {
+ RosTfListener(const std::string& name)
+ : Entity(name)
+ , buffer()
+ , listener(buffer, rosInit(), false)
+ {
std::string docstring =
"\n"
" Add a signal containing the transform between two frames.\n"
@@ -73,7 +78,7 @@ class RosTfListener : public Entity {
signalName % getName() % signame;
TransformListenerData* tld =
- new TransformListenerData(this, listener, to, from, signalName.str());
+ new TransformListenerData(this, buffer, to, from, signalName.str());
signalRegistration(tld->signal);
listenerDatas[signame] = tld;
}
@@ -81,7 +86,8 @@ class RosTfListener : public Entity {
private:
typedef std::map<std::string, TransformListenerData*> Map_t;
Map_t listenerDatas;
- tf::TransformListener listener;
+ tf2_ros::Buffer buffer;
+ tf2_ros::TransformListener listener;
};
} // end of namespace dynamicgraph.
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 422d233..6dcf031 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -126,6 +126,9 @@ SotLoader::SotLoader()
thread_() {
readSotVectorStateParam();
initPublication();
+
+ freeFlyerPose_.header.frame_id = "odom";
+ freeFlyerPose_.child_frame_id = "base_link";
}
SotLoader::~SotLoader() {
@@ -198,14 +201,18 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
// get the robot pose values
std::vector<double> poseValue_ = controlValues["baseff"].getValues();
- freeFlyerPose_.setOrigin(
- tf::Vector3(poseValue_[0], poseValue_[1], poseValue_[2]));
- tf::Quaternion poseQ_(poseValue_[4], poseValue_[5], poseValue_[6],
- poseValue_[3]);
- freeFlyerPose_.setRotation(poseQ_);
+ freeFlyerPose_.transform.translation.x = poseValue_[0];
+ freeFlyerPose_.transform.translation.y = poseValue_[1];
+ freeFlyerPose_.transform.translation.z = poseValue_[2];
+
+ freeFlyerPose_.transform.rotation.w = poseValue_[3];
+ freeFlyerPose_.transform.rotation.x = poseValue_[4];
+ freeFlyerPose_.transform.rotation.y = poseValue_[5];
+ freeFlyerPose_.transform.rotation.z = poseValue_[6];
+
+ freeFlyerPose_.header.stamp = joint_state_.header.stamp;
// Publish
- freeFlyerPublisher_.sendTransform(tf::StampedTransform(
- freeFlyerPose_, ros::Time::now(), "odom", "base_link"));
+ freeFlyerPublisher_.sendTransform(freeFlyerPose_);
}
void SotLoader::setup() {
--
GitLab