From cc98786ff26017ad56b3d6a5d408ce540603f90e Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Fri, 26 Apr 2019 14:45:53 +0200 Subject: [PATCH 01/12] refactor component to be implemented based on tf2::BufferCore --- rtt_tf/rtt_tf-component.cpp | 162 +++++++++++++++++++++++++----------- rtt_tf/rtt_tf-component.hpp | 38 +++++---- 2 files changed, 135 insertions(+), 65 deletions(-) diff --git a/rtt_tf/rtt_tf-component.cpp b/rtt_tf/rtt_tf-component.cpp index 96cd35e5..1644da3d 100644 --- a/rtt_tf/rtt_tf-component.cpp +++ b/rtt_tf/rtt_tf-component.cpp @@ -49,16 +49,72 @@ #include #include +#include + +#include + +#include + +namespace +{ + // local replacement for tf::resolve, which is not present in tf2. + // this helper method prepends the tf_prefix to the passed frame name for broadcasting from the component. + std::string prepend_prefix(const std::string& prefix, const std::string& frame_name) + { + if (!frame_name.empty() && frame_name[0] == '/') + { + if (frame_name.size() > 1) + { + return frame_name.substr(1); + } + return std::string(""); + } + + if (prefix.empty()) + { + return frame_name; + } + + std::string composite; + if (prefix[0] == '/' && prefix.size() > 1) + { + composite = prefix.substr(1) + "/"; + } + else + { + composite = prefix + "/"; + } + return composite + frame_name; + } +} + namespace rtt_tf { + // Functor for resolving the tf prefix when broadcasting multiple transforms. + class PrefixResolver + { + public: + PrefixResolver(const std::string& prefix) : prefix_(prefix) { } + + geometry_msgs::TransformStamped operator()(const geometry_msgs::TransformStamped& elem) + { + geometry_msgs::TransformStamped result = elem; + result.header.frame_id = prepend_prefix(prefix_, result.header.frame_id); + result.child_frame_id = prepend_prefix(prefix_, result.child_frame_id); + return result; + } + + private: + const std::string& prefix_; + }; + using namespace RTT; - using namespace tf; RTT_TF::RTT_TF(const std::string& name) : - TaskContext(name, PreOperational), - tf::Transformer(true, ros::Duration(Transformer::DEFAULT_CACHE_TIME)), - prop_cache_time(Transformer::DEFAULT_CACHE_TIME), + TaskContext( name, PreOperational ), + tf2::BufferCore( ros::Duration(BufferCore::DEFAULT_CACHE_TIME) ), + prop_cache_time( BufferCore::DEFAULT_CACHE_TIME ), prop_buffer_size(DEFAULT_BUFFER_SIZE) { this->addProperty("cache_time", prop_cache_time); @@ -109,15 +165,16 @@ namespace rtt_tf nh.getParam(tf_prefix_param_key, prop_tf_prefix); } + // no prefix to update in tf2::BufferCore (see #68) // Update the tf::Transformer prefix - tf_prefix_ = prop_tf_prefix; + //tf_prefix_ = prop_tf_prefix; // Connect to tf topic ConnPolicy cp = ConnPolicy::buffer(prop_buffer_size); cp.transport = 3; //3=ROS cp.name_id = "/tf"; - return (port_tf_in.createStream(cp) && port_tf_out.createStream(cp)); + return (port_tf_in.createStream(cp) && port_tf_static_in.createStream(cp) && port_tf_out.createStream(cp)); } void RTT_TF::updateHook() @@ -127,15 +184,17 @@ namespace rtt_tf //log(Debug) << "In update" << endlog(); #endif try { - tf::tfMessage msg_in; + tf2_msgs::TFMessage msg_in; while (port_tf_in.read(msg_in) == NewData) { for (unsigned int i = 0; i < msg_in.transforms.size(); i++) { - StampedTransform trans; - transformStampedMsgToTF(msg_in.transforms[i], trans); try { +// tf2::BufferCore (see #68) has a non-defaulted authority argument, +// but there is no __connection_header to extract it from. +// Workaround: use the same default value as tf::Transform did. #if ROS_VERSION_MINIMUM(1,11,0) - this->setTransform(trans); + const std::string authority = "default_authority"; + this->setTransform(msg_in.transforms[i], authority); #else std::map* msg_header_map = msg_in.__connection_header.get(); @@ -151,7 +210,7 @@ namespace rtt_tf } this->setTransform(trans, authority); #endif - } catch (TransformException& ex) { + } catch (tf2::TransformException& ex) { log(Error) << "Failure to set received transform from " << msg_in.transforms[i].child_frame_id << " to " << msg_in.transforms[i].header.frame_id @@ -164,70 +223,77 @@ namespace rtt_tf } } - geometry_msgs::TransformStamped RTT_TF::lookupTransform( + bool RTT_TF::startHook() + { + return true; + } + + void RTT_TF::stopHook() + { + } + + void RTT_TF::cleanupHook() + { + } + + ros::Time RTT_TF::getCommonTime( const std::string& target, - const std::string& source) + const std::string& source) const { - tf::StampedTransform stamped_tf; ros::Time common_time; - this->getLatestCommonTime(source, target, common_time,NULL); - static_cast(this)->lookupTransform(target, source, common_time, stamped_tf); - geometry_msgs::TransformStamped msg; - tf::transformStampedTFToMsg(stamped_tf,msg); - return msg; + + tf2::CompactFrameID target_id = _lookupFrameNumber(target); + tf2::CompactFrameID source_id = _lookupFrameNumber(source); + + _getLatestCommonTime(source_id, target_id, common_time, NULL); + + return common_time; + } + + geometry_msgs::TransformStamped RTT_TF::lookupTransform( + const std::string& target, + const std::string& source) const + { + return tf2::BufferCore::lookupTransform(target, source, getCommonTime(target, source)); } bool RTT_TF::canTransform( const std::string& target, - const std::string& source) + const std::string& source) const { - tf::StampedTransform stamped_tf; - ros::Time common_time; - this->getLatestCommonTime(source, target, common_time,NULL); - return static_cast(this)->canTransform(target, source, common_time); + return tf2::BufferCore::canTransform(target, source, getCommonTime(target, source)); } geometry_msgs::TransformStamped RTT_TF::lookupTransformAtTime( const std::string& target, const std::string& source, - const ros::Time& common_time) + const ros::Time& common_time) const { - tf::StampedTransform stamped_tf; - static_cast(this)->lookupTransform(target, source, common_time, stamped_tf); - geometry_msgs::TransformStamped msg; - tf::transformStampedTFToMsg(stamped_tf,msg); - return msg; + return tf2::BufferCore::lookupTransform(target, source, common_time); } - void RTT_TF::broadcastTransform( - const geometry_msgs::TransformStamped &tform) + void RTT_TF::broadcastTransform(const geometry_msgs::TransformStamped& tform) { - // Populate the TF message - tf::tfMessage msg_out; + // Populate the TFMessage + tf2_msgs::TFMessage msg_out; msg_out.transforms.push_back(tform); // Resolve names - msg_out.transforms.back().header.frame_id = tf::resolve(prop_tf_prefix, msg_out.transforms.back().header.frame_id); - msg_out.transforms.back().child_frame_id = tf::resolve(prop_tf_prefix, msg_out.transforms.back().child_frame_id); + geometry_msgs::TransformStamped& last_transform = msg_out.transforms.back(); + last_transform.header.frame_id = prepend_prefix(prop_tf_prefix, last_transform.header.frame_id); + last_transform.child_frame_id = prepend_prefix(prop_tf_prefix, last_transform.child_frame_id); port_tf_out.write(msg_out); } - void RTT_TF::broadcastTransforms( - const std::vector &tform) + void RTT_TF::broadcastTransforms(const std::vector& tform) { // Populate the TF message - tf::tfMessage msg_out; + tf2_msgs::TFMessage msg_out; - // Resolve names - for(std::vector::const_iterator it = tform.begin(); - it != tform.end(); - ++it) - { - msg_out.transforms.push_back(*it); - msg_out.transforms.back().header.frame_id = tf::resolve(prop_tf_prefix, msg_out.transforms.back().header.frame_id); - msg_out.transforms.back().child_frame_id = tf::resolve(prop_tf_prefix, msg_out.transforms.back().child_frame_id); - } + // copy and resolve names + msg_out.transforms.reserve(tform.size()); + std::transform(tform.begin(), tform.end(), msg_out.transforms.begin(), PrefixResolver(prop_tf_prefix)); port_tf_out.write(msg_out); } diff --git a/rtt_tf/rtt_tf-component.hpp b/rtt_tf/rtt_tf-component.hpp index c369c4e7..03466ad0 100644 --- a/rtt_tf/rtt_tf-component.hpp +++ b/rtt_tf/rtt_tf-component.hpp @@ -2,36 +2,40 @@ #define OROCOS_RTT_TF_COMPONENT_HPP #include -#include -#include +#include +#include namespace rtt_tf { // Inherit from TaskContext and Transformer, the second is required in order to use tf_prefix - class RTT_TF: public RTT::TaskContext, protected tf::Transformer + class RTT_TF: public RTT::TaskContext, protected tf2::BufferCore { static const int DEFAULT_BUFFER_SIZE = 100; - boost::shared_ptr m_transformer; double prop_cache_time; - double prop_buffer_size; + int prop_buffer_size; std::string prop_tf_prefix; - RTT::InputPort port_tf_in; - RTT::OutputPort port_tf_out; + RTT::InputPort port_tf_in; + RTT::InputPort port_tf_static_in; + RTT::OutputPort port_tf_out; + + ros::Time getCommonTime( + const std::string& target, + const std::string& source) const; bool canTransform( - const std::string& parent, - const std::string& child); + const std::string& target, + const std::string& source) const; geometry_msgs::TransformStamped lookupTransform( - const std::string& parent, - const std::string& child); + const std::string& target, + const std::string& source) const; geometry_msgs::TransformStamped lookupTransformAtTime( - const std::string& parent, - const std::string& child, - const ros::Time& common_time); + const std::string& target, + const std::string& source, + const ros::Time& common_time) const; void broadcastTransform( const geometry_msgs::TransformStamped &tform); @@ -46,13 +50,13 @@ namespace rtt_tf bool configureHook(); - bool startHook(){return true;}; + bool startHook(); void updateHook(); - void stopHook(){}; + void stopHook(); - void cleanupHook(){}; + void cleanupHook(); }; }//namespace rtt_tf #endif From dbbecdc529725abfb9c75e41f156039f36548df7 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Fri, 26 Apr 2019 17:38:15 +0200 Subject: [PATCH 02/12] InputPort port_tf_static_in is connected to /tf_static topic --- rtt_tf/rtt_tf-component.cpp | 87 +++++++++++++++++++++---------------- rtt_tf/rtt_tf-component.hpp | 10 +++-- 2 files changed, 57 insertions(+), 40 deletions(-) diff --git a/rtt_tf/rtt_tf-component.cpp b/rtt_tf/rtt_tf-component.cpp index 1644da3d..2fc3c477 100644 --- a/rtt_tf/rtt_tf-component.cpp +++ b/rtt_tf/rtt_tf-component.cpp @@ -54,7 +54,6 @@ #include #include - namespace { // local replacement for tf::resolve, which is not present in tf2. @@ -121,6 +120,7 @@ namespace rtt_tf this->addProperty("buffer_size", prop_buffer_size); this->addProperty("tf_prefix", prop_tf_prefix); this->addEventPort("tf_in", port_tf_in); + this->addEventPort("tf_static_in", port_tf_static_in); this->addPort("tf_out", port_tf_out); this->addTFOperations(this->provides()); @@ -166,7 +166,6 @@ namespace rtt_tf } // no prefix to update in tf2::BufferCore (see #68) - // Update the tf::Transformer prefix //tf_prefix_ = prop_tf_prefix; // Connect to tf topic @@ -174,51 +173,65 @@ namespace rtt_tf cp.transport = 3; //3=ROS cp.name_id = "/tf"; - return (port_tf_in.createStream(cp) && port_tf_static_in.createStream(cp) && port_tf_out.createStream(cp)); + // Connect to tf_static topic + ConnPolicy cp_static = ConnPolicy::buffer(prop_buffer_size); + cp.transport = 3; + cp.name_id = "/tf_static"; + + return (port_tf_static_in.createStream(cp_static) && port_tf_in.createStream(cp) && port_tf_out.createStream(cp)); } - void RTT_TF::updateHook() + void RTT_TF::internalUpdate(tf2_msgs::TFMessage& msg, RTT::InputPort& port) { - Logger::In(this->getName()); -#ifndef NDEBUG - //log(Debug) << "In update" << endlog(); -#endif - try { - tf2_msgs::TFMessage msg_in; - - while (port_tf_in.read(msg_in) == NewData) { - for (unsigned int i = 0; i < msg_in.transforms.size(); i++) { - try { + while (port.read(msg) == NewData) { + for (std::size_t i = 0; i < msg.transforms.size(); ++i) { + try { // tf2::BufferCore (see #68) has a non-defaulted authority argument, // but there is no __connection_header to extract it from. // Workaround: use the same default value as tf::Transform did. #if ROS_VERSION_MINIMUM(1,11,0) - const std::string authority = "default_authority"; - this->setTransform(msg_in.transforms[i], authority); + const std::string authority = "default_authority"; + this->setTransform(msg.transforms[i], authority); #else - std::map* msg_header_map = - msg_in.__connection_header.get(); - std::string authority; - std::map::iterator it = - msg_header_map->find("callerid"); - - if (it == msg_header_map->end()) { - log(Warning) << "Message received without callerid" << endlog(); - authority = "no callerid"; - } else { - authority = it->second; - } - this->setTransform(trans, authority); -#endif - } catch (tf2::TransformException& ex) { - log(Error) << "Failure to set received transform from " - << msg_in.transforms[i].child_frame_id << " to " - << msg_in.transforms[i].header.frame_id - << " with error: " << ex.what() << endlog(); + std::map* msg_header_map = + msg.__connection_header.get(); + std::string authority; + std::map::iterator it = + msg_header_map->find("callerid"); + + if (it == msg_header_map->end()) { + log(Warning) << "Message received without callerid" << endlog(); + authority = "no callerid"; + } else { + authority = it->second; } + this->setTransform(trans, authority); +#endif + } catch (tf2::TransformException& ex) { + log(Error) << "Failure to set received transform from " + << msg.transforms[i].child_frame_id << " to " + << msg.transforms[i].header.frame_id + << " with error: " << ex.what() << endlog(); } } - } catch (std::exception& ex) { + } + } + + void RTT_TF::updateHook() + { + Logger::In(this->getName()); +#ifndef NDEBUG + //log(Debug) << "In update" << endlog(); +#endif + try + { + tf2_msgs::TFMessage msg_in; + + internalUpdate(msg_in, port_tf_in); + internalUpdate(msg_in, port_tf_static_in); + } + catch (std::exception& ex) + { log(Error) << ex.what() << endlog(); } } @@ -288,7 +301,7 @@ namespace rtt_tf void RTT_TF::broadcastTransforms(const std::vector& tform) { - // Populate the TF message + // Populate the TFMessage tf2_msgs::TFMessage msg_out; // copy and resolve names diff --git a/rtt_tf/rtt_tf-component.hpp b/rtt_tf/rtt_tf-component.hpp index 03466ad0..87dd0795 100644 --- a/rtt_tf/rtt_tf-component.hpp +++ b/rtt_tf/rtt_tf-component.hpp @@ -13,16 +13,20 @@ namespace rtt_tf static const int DEFAULT_BUFFER_SIZE = 100; double prop_cache_time; - int prop_buffer_size; + double prop_buffer_size; std::string prop_tf_prefix; RTT::InputPort port_tf_in; RTT::InputPort port_tf_static_in; RTT::OutputPort port_tf_out; + void internalUpdate( + tf2_msgs::TFMessage& msg, + RTT::InputPort& port); + ros::Time getCommonTime( - const std::string& target, - const std::string& source) const; + const std::string& target, + const std::string& source) const; bool canTransform( const std::string& target, From ae3efb3ee7715f45ca9705d155ebfc6b9db97d58 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Fri, 26 Apr 2019 17:39:01 +0200 Subject: [PATCH 03/12] add rtt_tf2_msgs typekit for support of tf2 types in RTT --- typekits/rtt_tf2_msgs/CMakeLists.txt | 12 +++++++ typekits/rtt_tf2_msgs/package.xml | 52 ++++++++++++++++++++++++++++ 2 files changed, 64 insertions(+) create mode 100644 typekits/rtt_tf2_msgs/CMakeLists.txt create mode 100644 typekits/rtt_tf2_msgs/package.xml diff --git a/typekits/rtt_tf2_msgs/CMakeLists.txt b/typekits/rtt_tf2_msgs/CMakeLists.txt new file mode 100644 index 00000000..e718ab18 --- /dev/null +++ b/typekits/rtt_tf2_msgs/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rtt_tf2_msgs) + +find_package(catkin REQUIRED COMPONENTS rtt_roscomm) + +ros_generate_rtt_typekit(tf2_msgs) +ros_generate_rtt_service_proxies(tf2_msgs) + +orocos_generate_package( + DEPENDS tf2_msgs + DEPENDS_TARGETS rtt_roscomm rtt_std_msgs rtt_geometry_msgs rtt_actionlib_msgs +) diff --git a/typekits/rtt_tf2_msgs/package.xml b/typekits/rtt_tf2_msgs/package.xml new file mode 100644 index 00000000..27088a2d --- /dev/null +++ b/typekits/rtt_tf2_msgs/package.xml @@ -0,0 +1,52 @@ + + rtt_tf2_msgs + 2.9.1 + + + Provides an rtt typekit for ROS nav_msgs messages. + + It allows you to use ROS messages transparently in + RTT components and applications. + + This package was automatically generated by the + create_rtt_msgs generator and should not be manually + modified. + + See the http://ros.org/wiki/nav_msgs documentation + for the documentation of the ROS messages in this + typekit. + + + Orocos Developers + BSD + + catkin + + tf2_msgs + tf2_msgs + + rtt_tf + rtt_tf + + rtt_std_msgs + rtt_std_msgs + + rtt_geometry_msgs + rtt_geometry_msgs + + rtt_actionlib_msgs + rtt_actionlib_msgs + + + + tf2_msgs + rtt_tf + rtt_std_msgs + rtt_geometry_msgs + rtt_actionlib_msgs + + + + + + From a58c49400c15dc0ada2eb7fd39afac6f9c83b8ae Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Fri, 26 Apr 2019 17:39:55 +0200 Subject: [PATCH 04/12] update rtt_tf tests to load any useful plugins automatically --- rtt_tf/test.ops | 4 ++++ rtt_tf/tests/broadcaster_test.cpp | 7 +++++++ rtt_tf/tests/lookup_test.cpp | 7 +++++++ 3 files changed, 18 insertions(+) diff --git a/rtt_tf/test.ops b/rtt_tf/test.ops index 47fa3a1d..8b82eb13 100644 --- a/rtt_tf/test.ops +++ b/rtt_tf/test.ops @@ -1,3 +1,7 @@ +import("rtt_rosnode") +import("rtt_std_msgs") +import("rtt_geometry_msgs") +import("rtt_tf2_msgs") import("rtt_tf") loadComponent("tf","rtt_tf::RTT_TF") tf.configure() diff --git a/rtt_tf/tests/broadcaster_test.cpp b/rtt_tf/tests/broadcaster_test.cpp index 2ea00f23..51c814e8 100644 --- a/rtt_tf/tests/broadcaster_test.cpp +++ b/rtt_tf/tests/broadcaster_test.cpp @@ -63,6 +63,13 @@ int ORO_main(int argc, char** argv) { // Create deployer OCL::DeploymentComponent deployer; + // initialize ROS and load typekits + deployer.import("rtt_rosnode"); + deployer.import("rtt_std_msgs"); + deployer.import("rtt_geometry_msgs"); + deployer.import("rtt_tf2_msgs"); + + // import and load our component deployer.import("rtt_tf"); deployer.loadComponent("tf","rtt_tf::RTT_TF"); diff --git a/rtt_tf/tests/lookup_test.cpp b/rtt_tf/tests/lookup_test.cpp index a5c4a60d..ab62d692 100644 --- a/rtt_tf/tests/lookup_test.cpp +++ b/rtt_tf/tests/lookup_test.cpp @@ -55,6 +55,13 @@ int ORO_main(int argc, char** argv) { // Create deployer OCL::DeploymentComponent deployer; + // initialize ROS and load typekits + deployer.import("rtt_rosnode"); + deployer.import("rtt_std_msgs"); + deployer.import("rtt_geometry_msgs"); + deployer.import("rtt_tf2_msgs"); + + // import and load our component deployer.import("rtt_tf"); deployer.loadComponent("tf","rtt_tf::RTT_TF"); From b66a3040681bc6c632a064eee9fdb47ce247ee77 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Tue, 30 Apr 2019 09:52:53 +0200 Subject: [PATCH 05/12] Update README by referring to tf2. --- rtt_tf/README.md | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/rtt_tf/README.md b/rtt_tf/README.md index c087544a..96a5c6cb 100644 --- a/rtt_tf/README.md +++ b/rtt_tf/README.md @@ -1,13 +1,13 @@ RTT TF ====== -This package provides an Orocos RTT component for utilizing the TF rigid body -transform library from within Orocos. This component provides a "tf" service +This package provides an Orocos RTT component for utilizing the [TF2 rigid body +transform library](http://wiki.ros.org/tf2) from within Orocos. This component provides a "tf" service with operations for requesting and broadcasting sing and batch transforms. ### Usage -In general, a given process only needs a single TF component. This component +In general, a given process only needs a single TF2 component. This component provides the following operations both on its bare interface and also on the provided "tf" RTT service. @@ -31,6 +31,9 @@ and then connect it with whichever components you intend to have use it: ```cpp import("rtt_ros"); +import("rtt_std_msgs"); +import("rtt_geometry_msgs"); +import("rtt_tf2_msgs"); // Import and load the TF component ros.import("rtt_tf"); @@ -47,7 +50,7 @@ connectServices("my_component","tf"); The [rtt\_tf/tf\_interface.h](include/rtt_tf/tf_interface.h) header includes the `rtt_tf::TFInterface` class which adds a required service to a given RTT -component. This can be used for more easily connecting with the TF component. +component. This can be used for more easily connecting with the TF2 component. It can be used to create the RTT service requester "tf" like so: From ac6df7dd688b45cceae5fa699e1a0e9420cc6bda Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Mon, 6 May 2019 17:06:51 +0200 Subject: [PATCH 06/12] rtt_tf2_msgs: update package by using create_rtt_msgs script from rtt_roscomm --- typekits/rtt_tf2_msgs/CMakeLists.txt | 2 +- typekits/rtt_tf2_msgs/package.xml | 21 ++++++++++----------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/typekits/rtt_tf2_msgs/CMakeLists.txt b/typekits/rtt_tf2_msgs/CMakeLists.txt index e718ab18..e45c4fbe 100644 --- a/typekits/rtt_tf2_msgs/CMakeLists.txt +++ b/typekits/rtt_tf2_msgs/CMakeLists.txt @@ -8,5 +8,5 @@ ros_generate_rtt_service_proxies(tf2_msgs) orocos_generate_package( DEPENDS tf2_msgs - DEPENDS_TARGETS rtt_roscomm rtt_std_msgs rtt_geometry_msgs rtt_actionlib_msgs + DEPENDS_TARGETS rtt_roscomm rtt_std_msgs rtt_actionlib_msgs rtt_geometry_msgs ) diff --git a/typekits/rtt_tf2_msgs/package.xml b/typekits/rtt_tf2_msgs/package.xml index 27088a2d..ae6e3a82 100644 --- a/typekits/rtt_tf2_msgs/package.xml +++ b/typekits/rtt_tf2_msgs/package.xml @@ -3,7 +3,7 @@ 2.9.1 - Provides an rtt typekit for ROS nav_msgs messages. + Provides an rtt typekit for ROS tf2_msgs messages. It allows you to use ROS messages transparently in RTT components and applications. @@ -12,7 +12,7 @@ create_rtt_msgs generator and should not be manually modified. - See the http://ros.org/wiki/nav_msgs documentation + See the http://ros.org/wiki/tf2_msgs documentation for the documentation of the ROS messages in this typekit. @@ -23,27 +23,26 @@ catkin tf2_msgs - tf2_msgs + rtt_roscomm - rtt_tf - rtt_tf + tf2_msgs + rtt_roscomm rtt_std_msgs rtt_std_msgs - + rtt_actionlib_msgs + rtt_actionlib_msgs rtt_geometry_msgs rtt_geometry_msgs - rtt_actionlib_msgs - rtt_actionlib_msgs - tf2_msgs - rtt_tf + rtt_roscomm rtt_std_msgs - rtt_geometry_msgs rtt_actionlib_msgs + rtt_geometry_msgs + From 7244f95e61c1d689f5ed6cbf18e2a8da843b2c14 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Mon, 13 May 2019 15:23:46 +0200 Subject: [PATCH 07/12] remove unnecessary import statements from README. The rtt_ros plugin loads a component that allows importing ROS packages following the dependency tree. --- rtt_tf/README.md | 3 --- 1 file changed, 3 deletions(-) diff --git a/rtt_tf/README.md b/rtt_tf/README.md index 96a5c6cb..3a927175 100644 --- a/rtt_tf/README.md +++ b/rtt_tf/README.md @@ -31,9 +31,6 @@ and then connect it with whichever components you intend to have use it: ```cpp import("rtt_ros"); -import("rtt_std_msgs"); -import("rtt_geometry_msgs"); -import("rtt_tf2_msgs"); // Import and load the TF component ros.import("rtt_tf"); From 6f8df098eff8a74535ed778b94899562fa3cf607 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Mon, 13 May 2019 15:48:03 +0200 Subject: [PATCH 08/12] rtt_tf: add dependencies on tf2, rtt_tf2_msgs to rtt_tf --- rtt_tf/CMakeLists.txt | 4 ++-- rtt_tf/package.xml | 5 +++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/rtt_tf/CMakeLists.txt b/rtt_tf/CMakeLists.txt index 2003a7a5..2e4a9c77 100644 --- a/rtt_tf/CMakeLists.txt +++ b/rtt_tf/CMakeLists.txt @@ -14,8 +14,8 @@ orocos_install_headers(include/rtt_tf/tf_interface.h) orocos_generate_package( INCLUDE_DIRS include - DEPENDS tf - DEPENDS_TARGETS rtt_roscomm rtt_geometry_msgs + DEPENDS tf tf2 + DEPENDS_TARGETS rtt_roscomm rtt_geometry_msgs rtt_tf2_msgs ) # Tests diff --git a/rtt_tf/package.xml b/rtt_tf/package.xml index 47233ed2..de493120 100644 --- a/rtt_tf/package.xml +++ b/rtt_tf/package.xml @@ -16,16 +16,21 @@ rtt tf + tf2 rtt_roscomm rtt_geometry_msgs + rtt_tf2_msgs rtt tf + tf2 rtt_roscomm rtt_geometry_msgs + rtt_tf2_msgs + rtt_tf2_msgs rtt_roscomm rtt_geometry_msgs From cf9a741a6206f3f0ff1c466b61162446cc70378a Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Mon, 13 May 2019 15:50:53 +0200 Subject: [PATCH 09/12] rtt_tf: add canTransformAtTime operation, remove code duplication and other minor cleanups --- rtt_tf/include/rtt_tf/tf_interface.h | 8 ++- rtt_tf/rtt_tf-component.cpp | 86 +++++++++++----------------- rtt_tf/rtt_tf-component.hpp | 7 ++- 3 files changed, 44 insertions(+), 57 deletions(-) diff --git a/rtt_tf/include/rtt_tf/tf_interface.h b/rtt_tf/include/rtt_tf/tf_interface.h index 92a66898..f59fff63 100644 --- a/rtt_tf/include/rtt_tf/tf_interface.h +++ b/rtt_tf/include/rtt_tf/tf_interface.h @@ -18,13 +18,15 @@ namespace rtt_tf { lookupTransformAtTime("lookupTransformAtTime"), broadcastTransform("broadcastTransform"), broadcastTransforms("broadcastTransforms"), - canTransform("canTransform") + canTransform("canTransform"), + canTransformAtTime("canTransformAtTime") { owner->requires("tf")->addOperationCaller(lookupTransform); owner->requires("tf")->addOperationCaller(lookupTransformAtTime); owner->requires("tf")->addOperationCaller(broadcastTransform); owner->requires("tf")->addOperationCaller(broadcastTransforms); owner->requires("tf")->addOperationCaller(canTransform); + owner->requires("tf")->addOperationCaller(canTransformAtTime); } //! Check if the operations are ready @@ -34,7 +36,8 @@ namespace rtt_tf { lookupTransformAtTime.ready() && broadcastTransform.ready() && broadcastTransforms.ready() && - canTransform.ready(); + canTransform.ready() && + canTransformAtTime.ready(); } RTT::OperationCaller lookupTransform; @@ -42,6 +45,7 @@ namespace rtt_tf { RTT::OperationCaller broadcastTransform; RTT::OperationCaller&)> broadcastTransforms; RTT::OperationCaller canTransform; + RTT::OperationCaller canTransformAtTime; }; } diff --git a/rtt_tf/rtt_tf-component.cpp b/rtt_tf/rtt_tf-component.cpp index 2fc3c477..4dbfd1da 100644 --- a/rtt_tf/rtt_tf-component.cpp +++ b/rtt_tf/rtt_tf-component.cpp @@ -49,43 +49,12 @@ #include #include +#include #include #include #include -namespace -{ - // local replacement for tf::resolve, which is not present in tf2. - // this helper method prepends the tf_prefix to the passed frame name for broadcasting from the component. - std::string prepend_prefix(const std::string& prefix, const std::string& frame_name) - { - if (!frame_name.empty() && frame_name[0] == '/') - { - if (frame_name.size() > 1) - { - return frame_name.substr(1); - } - return std::string(""); - } - - if (prefix.empty()) - { - return frame_name; - } - - std::string composite; - if (prefix[0] == '/' && prefix.size() > 1) - { - composite = prefix.substr(1) + "/"; - } - else - { - composite = prefix + "/"; - } - return composite + frame_name; - } -} namespace rtt_tf { @@ -98,8 +67,8 @@ namespace rtt_tf geometry_msgs::TransformStamped operator()(const geometry_msgs::TransformStamped& elem) { geometry_msgs::TransformStamped result = elem; - result.header.frame_id = prepend_prefix(prefix_, result.header.frame_id); - result.child_frame_id = prepend_prefix(prefix_, result.child_frame_id); + result.header.frame_id = tf::resolve(prefix_, result.header.frame_id); + result.child_frame_id = tf::resolve(prefix_, result.child_frame_id); return result; } @@ -107,6 +76,14 @@ namespace rtt_tf const std::string& prefix_; }; + tf2_msgs::TFMessage transformsToMessage(const std::vector& tforms, const std::string& prefix) + { + tf2_msgs::TFMessage msg; + // resolve names and copy transforms to message + msg.transforms.reserve(tforms.size()); + std::transform(tforms.begin(), tforms.end(), msg.transforms.begin(), PrefixResolver(prefix)); + return msg; + } using namespace RTT; @@ -149,9 +126,15 @@ namespace rtt_tf .arg("transforms", "[std::vector]"); service->addOperation("canTransform", &RTT_TF::canTransform, this) - .doc("Check if the transform from source to target can be resolved..") + .doc("Check if the transform from source to target can be resolved.") .arg("target", "Target frame") .arg("source", "Source frame"); + + service->addOperation("canTransformAtTime", &RTT_TF::canTransformAtTime, this) + .doc("Check if the transform from source to target can be resolved for a given common time.") + .arg("target", "Target frame") + .arg("source", "Source frame") + .arg("common_time", "[ros::Time] The common time for which the transform would resolve"); } bool RTT_TF::configureHook() @@ -249,7 +232,7 @@ namespace rtt_tf { } - ros::Time RTT_TF::getCommonTime( + ros::Time RTT_TF::getLatestCommonTime( const std::string& target, const std::string& source) const { @@ -267,14 +250,22 @@ namespace rtt_tf const std::string& target, const std::string& source) const { - return tf2::BufferCore::lookupTransform(target, source, getCommonTime(target, source)); + return tf2::BufferCore::lookupTransform(target, source, ros::Time()); } bool RTT_TF::canTransform( const std::string& target, const std::string& source) const { - return tf2::BufferCore::canTransform(target, source, getCommonTime(target, source)); + return tf2::BufferCore::canTransform(target, source, ros::Time()); + } + + bool RTT_TF::canTransformAtTime( + const std::string& target, + const std::string& source, + const ros::Time& common_time) const + { + return tf2::BufferCore::canTransform(target, source, common_time); } geometry_msgs::TransformStamped RTT_TF::lookupTransformAtTime( @@ -287,27 +278,14 @@ namespace rtt_tf void RTT_TF::broadcastTransform(const geometry_msgs::TransformStamped& tform) { - // Populate the TFMessage - tf2_msgs::TFMessage msg_out; - msg_out.transforms.push_back(tform); - - // Resolve names - geometry_msgs::TransformStamped& last_transform = msg_out.transforms.back(); - last_transform.header.frame_id = prepend_prefix(prop_tf_prefix, last_transform.header.frame_id); - last_transform.child_frame_id = prepend_prefix(prop_tf_prefix, last_transform.child_frame_id); - + const std::vector tforms(1, tform); + tf2_msgs::TFMessage msg_out = transformsToMessage(tforms, prop_tf_prefix); port_tf_out.write(msg_out); } void RTT_TF::broadcastTransforms(const std::vector& tform) { - // Populate the TFMessage - tf2_msgs::TFMessage msg_out; - - // copy and resolve names - msg_out.transforms.reserve(tform.size()); - std::transform(tform.begin(), tform.end(), msg_out.transforms.begin(), PrefixResolver(prop_tf_prefix)); - + tf2_msgs::TFMessage msg_out = transformsToMessage(tform, prop_tf_prefix); port_tf_out.write(msg_out); } diff --git a/rtt_tf/rtt_tf-component.hpp b/rtt_tf/rtt_tf-component.hpp index 87dd0795..5497bfa6 100644 --- a/rtt_tf/rtt_tf-component.hpp +++ b/rtt_tf/rtt_tf-component.hpp @@ -24,7 +24,7 @@ namespace rtt_tf tf2_msgs::TFMessage& msg, RTT::InputPort& port); - ros::Time getCommonTime( + ros::Time getLatestCommonTime( const std::string& target, const std::string& source) const; @@ -32,6 +32,11 @@ namespace rtt_tf const std::string& target, const std::string& source) const; + bool canTransformAtTime( + const std::string& target, + const std::string& source, + const ros::Time& common_time) const; + geometry_msgs::TransformStamped lookupTransform( const std::string& target, const std::string& source) const; From 18ad148b2a320a9eed39b3193471e7dad43feea3 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Mon, 13 May 2019 16:08:22 +0200 Subject: [PATCH 10/12] rtt_tf: handle configureHook() failure --- rtt_tf/rtt_tf-component.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/rtt_tf/rtt_tf-component.cpp b/rtt_tf/rtt_tf-component.cpp index 4dbfd1da..0cec48af 100644 --- a/rtt_tf/rtt_tf-component.cpp +++ b/rtt_tf/rtt_tf-component.cpp @@ -88,7 +88,7 @@ namespace rtt_tf using namespace RTT; RTT_TF::RTT_TF(const std::string& name) : - TaskContext( name, PreOperational ), + TaskContext(name, PreOperational), tf2::BufferCore( ros::Duration(BufferCore::DEFAULT_CACHE_TIME) ), prop_cache_time( BufferCore::DEFAULT_CACHE_TIME ), prop_buffer_size(DEFAULT_BUFFER_SIZE) @@ -158,10 +158,18 @@ namespace rtt_tf // Connect to tf_static topic ConnPolicy cp_static = ConnPolicy::buffer(prop_buffer_size); - cp.transport = 3; + cp.transport = 3; //3=ROS cp.name_id = "/tf_static"; - return (port_tf_static_in.createStream(cp_static) && port_tf_in.createStream(cp) && port_tf_out.createStream(cp)); + bool configured = port_tf_static_in.createStream(cp_static) + && port_tf_in.createStream(cp) + && port_tf_out.createStream(cp); + + if (!configured) { + cleanupHook(); + } + + return configured; } void RTT_TF::internalUpdate(tf2_msgs::TFMessage& msg, RTT::InputPort& port) @@ -230,6 +238,9 @@ namespace rtt_tf void RTT_TF::cleanupHook() { + port_tf_in.disconnect(); + port_tf_out.disconnect(); + port_tf_static_in.disconnect(); } ros::Time RTT_TF::getLatestCommonTime( From 9be6c797e2aa12b0a280e3b1cdd13f0a5233bc9f Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Mon, 13 May 2019 16:18:51 +0200 Subject: [PATCH 11/12] rtt_tf: reduce code in RTT_TF::internalUpdate and differentiate static transform case; remove dead code --- rtt_tf/rtt_tf-component.cpp | 45 +++++++------------------------------ rtt_tf/rtt_tf-component.hpp | 7 ++---- 2 files changed, 10 insertions(+), 42 deletions(-) diff --git a/rtt_tf/rtt_tf-component.cpp b/rtt_tf/rtt_tf-component.cpp index 0cec48af..28569217 100644 --- a/rtt_tf/rtt_tf-component.cpp +++ b/rtt_tf/rtt_tf-component.cpp @@ -148,9 +148,6 @@ namespace rtt_tf nh.getParam(tf_prefix_param_key, prop_tf_prefix); } - // no prefix to update in tf2::BufferCore (see #68) - //tf_prefix_ = prop_tf_prefix; - // Connect to tf topic ConnPolicy cp = ConnPolicy::buffer(prop_buffer_size); cp.transport = 3; //3=ROS @@ -172,32 +169,16 @@ namespace rtt_tf return configured; } - void RTT_TF::internalUpdate(tf2_msgs::TFMessage& msg, RTT::InputPort& port) + void RTT_TF::internalUpdate(tf2_msgs::TFMessage& msg, RTT::InputPort& port, bool is_static) { + // tf2::BufferCore::setTransform (see #68) has a non-defaulted authority argument, + // but there is no __connection_header to extract it from. + const std::string authority = "unknown_authority"; + while (port.read(msg) == NewData) { for (std::size_t i = 0; i < msg.transforms.size(); ++i) { try { -// tf2::BufferCore (see #68) has a non-defaulted authority argument, -// but there is no __connection_header to extract it from. -// Workaround: use the same default value as tf::Transform did. -#if ROS_VERSION_MINIMUM(1,11,0) - const std::string authority = "default_authority"; - this->setTransform(msg.transforms[i], authority); -#else - std::map* msg_header_map = - msg.__connection_header.get(); - std::string authority; - std::map::iterator it = - msg_header_map->find("callerid"); - - if (it == msg_header_map->end()) { - log(Warning) << "Message received without callerid" << endlog(); - authority = "no callerid"; - } else { - authority = it->second; - } - this->setTransform(trans, authority); -#endif + this->setTransform(msg.transforms[i], authority, is_static); } catch (tf2::TransformException& ex) { log(Error) << "Failure to set received transform from " << msg.transforms[i].child_frame_id << " to " @@ -217,9 +198,8 @@ namespace rtt_tf try { tf2_msgs::TFMessage msg_in; - - internalUpdate(msg_in, port_tf_in); - internalUpdate(msg_in, port_tf_static_in); + internalUpdate(msg_in, port_tf_in, false); + internalUpdate(msg_in, port_tf_static_in, true); } catch (std::exception& ex) { @@ -227,15 +207,6 @@ namespace rtt_tf } } - bool RTT_TF::startHook() - { - return true; - } - - void RTT_TF::stopHook() - { - } - void RTT_TF::cleanupHook() { port_tf_in.disconnect(); diff --git a/rtt_tf/rtt_tf-component.hpp b/rtt_tf/rtt_tf-component.hpp index 5497bfa6..3cfdf560 100644 --- a/rtt_tf/rtt_tf-component.hpp +++ b/rtt_tf/rtt_tf-component.hpp @@ -22,7 +22,8 @@ namespace rtt_tf void internalUpdate( tf2_msgs::TFMessage& msg, - RTT::InputPort& port); + RTT::InputPort& port, + bool is_static); ros::Time getLatestCommonTime( const std::string& target, @@ -59,12 +60,8 @@ namespace rtt_tf bool configureHook(); - bool startHook(); - void updateHook(); - void stopHook(); - void cleanupHook(); }; }//namespace rtt_tf From 9a864ec901bb9d7532410e4314c5c1eb2e728485 Mon Sep 17 00:00:00 2001 From: Francisco Almeida Date: Mon, 13 May 2019 16:29:04 +0200 Subject: [PATCH 12/12] rtt_tf: add output port for static transform and broadcastStaticTransform(s) operations --- rtt_tf/include/rtt_tf/tf_interface.h | 8 ++++++++ rtt_tf/rtt_tf-component.cpp | 27 +++++++++++++++++++++++++-- rtt_tf/rtt_tf-component.hpp | 7 +++++++ 3 files changed, 40 insertions(+), 2 deletions(-) diff --git a/rtt_tf/include/rtt_tf/tf_interface.h b/rtt_tf/include/rtt_tf/tf_interface.h index f59fff63..809109b0 100644 --- a/rtt_tf/include/rtt_tf/tf_interface.h +++ b/rtt_tf/include/rtt_tf/tf_interface.h @@ -18,6 +18,8 @@ namespace rtt_tf { lookupTransformAtTime("lookupTransformAtTime"), broadcastTransform("broadcastTransform"), broadcastTransforms("broadcastTransforms"), + broadcastStaticTransform("broadcastStaticTransform"), + broadcastStaticTransforms("broadcastStaticTransforms"), canTransform("canTransform"), canTransformAtTime("canTransformAtTime") { @@ -25,6 +27,8 @@ namespace rtt_tf { owner->requires("tf")->addOperationCaller(lookupTransformAtTime); owner->requires("tf")->addOperationCaller(broadcastTransform); owner->requires("tf")->addOperationCaller(broadcastTransforms); + owner->requires("tf")->addOperationCaller(broadcastStaticTransform); + owner->requires("tf")->addOperationCaller(broadcastStaticTransforms); owner->requires("tf")->addOperationCaller(canTransform); owner->requires("tf")->addOperationCaller(canTransformAtTime); } @@ -36,6 +40,8 @@ namespace rtt_tf { lookupTransformAtTime.ready() && broadcastTransform.ready() && broadcastTransforms.ready() && + broadcastStaticTransform.ready() && + broadcastStaticTransforms.ready() && canTransform.ready() && canTransformAtTime.ready(); } @@ -44,6 +50,8 @@ namespace rtt_tf { RTT::OperationCaller lookupTransformAtTime; RTT::OperationCaller broadcastTransform; RTT::OperationCaller&)> broadcastTransforms; + RTT::OperationCaller broadcastStaticTransform; + RTT::OperationCaller&)> broadcastStaticTransforms; RTT::OperationCaller canTransform; RTT::OperationCaller canTransformAtTime; }; diff --git a/rtt_tf/rtt_tf-component.cpp b/rtt_tf/rtt_tf-component.cpp index 28569217..974ec608 100644 --- a/rtt_tf/rtt_tf-component.cpp +++ b/rtt_tf/rtt_tf-component.cpp @@ -122,7 +122,15 @@ namespace rtt_tf .arg("transform", "[geometry_msgs::TransformStamped]"); service->addOperation("broadcastTransforms", &RTT_TF::broadcastTransforms, this, RTT::OwnThread) - .doc("Broadcast a stamped transform immediately.") + .doc("Broadcast stamped transforms immediately.") + .arg("transforms", "[std::vector]"); + + service->addOperation("broadcastStaticTransform", &RTT_TF::broadcastStaticTransform, this, RTT::OwnThread) + .doc("Broadcast a stamped transform as a static transform immediately.") + .arg("transform", "[geometry_msgs::TransformStamped]"); + + service->addOperation("broadcastStaticTransforms", &RTT_TF::broadcastStaticTransforms, this, RTT::OwnThread) + .doc("Broadcast stamped transforms as static transforms immediately.") .arg("transforms", "[std::vector]"); service->addOperation("canTransform", &RTT_TF::canTransform, this) @@ -160,7 +168,8 @@ namespace rtt_tf bool configured = port_tf_static_in.createStream(cp_static) && port_tf_in.createStream(cp) - && port_tf_out.createStream(cp); + && port_tf_out.createStream(cp) + && port_tf_static_out.createStream(cp_static); if (!configured) { cleanupHook(); @@ -212,6 +221,7 @@ namespace rtt_tf port_tf_in.disconnect(); port_tf_out.disconnect(); port_tf_static_in.disconnect(); + port_tf_static_out.disconnect(); } ros::Time RTT_TF::getLatestCommonTime( @@ -271,6 +281,19 @@ namespace rtt_tf port_tf_out.write(msg_out); } + void RTT_TF::broadcastStaticTransform(const geometry_msgs::TransformStamped& tform) + { + const std::vector tforms(1, tform); + tf2_msgs::TFMessage msg_out = transformsToMessage(tforms, prop_tf_prefix); + port_tf_static_out.write(msg_out); + } + + void RTT_TF::broadcastStaticTransforms(const std::vector& tform) + { + tf2_msgs::TFMessage msg_out = transformsToMessage(tform, prop_tf_prefix); + port_tf_static_out.write(msg_out); + } + }//namespace /* diff --git a/rtt_tf/rtt_tf-component.hpp b/rtt_tf/rtt_tf-component.hpp index 3cfdf560..6e7944de 100644 --- a/rtt_tf/rtt_tf-component.hpp +++ b/rtt_tf/rtt_tf-component.hpp @@ -19,6 +19,7 @@ namespace rtt_tf RTT::InputPort port_tf_in; RTT::InputPort port_tf_static_in; RTT::OutputPort port_tf_out; + RTT::OutputPort port_tf_static_out; void internalUpdate( tf2_msgs::TFMessage& msg, @@ -53,6 +54,12 @@ namespace rtt_tf void broadcastTransforms( const std::vector &tforms); + void broadcastStaticTransform( + const geometry_msgs::TransformStamped &tform); + + void broadcastStaticTransforms( + const std::vector &tforms); + void addTFOperations(RTT::Service::shared_ptr service); public: