diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index 135c5abc5..1a228a43a 100644 --- a/tf2_ros/CMakeLists.txt +++ b/tf2_ros/CMakeLists.txt @@ -126,6 +126,14 @@ if(BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}_test_message_filter test/message_filter_test.cpp) target_link_libraries(${PROJECT_NAME}_test_message_filter ${PROJECT_NAME}) + ament_add_gtest(${PROJECT_NAME}_test_transform_listener test/test_transform_listener.cpp) + target_link_libraries(${PROJECT_NAME}_test_transform_listener ${PROJECT_NAME}) + + ament_add_gtest(${PROJECT_NAME}_test_static_transform_broadcaster test/test_static_transform_broadcaster.cpp) + target_link_libraries(${PROJECT_NAME}_test_static_transform_broadcaster ${PROJECT_NAME}) + + ament_add_gtest(${PROJECT_NAME}_test_transform_broadcaster test/test_transform_broadcaster.cpp) + target_link_libraries(${PROJECT_NAME}_test_transform_broadcaster ${PROJECT_NAME}) # TODO(tfoote) port tests to use ROS2 instead of ROS1 api. if (false) diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h index 3414c9edb..a0364ee36 100644 --- a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h @@ -43,29 +43,24 @@ namespace tf2_ros { - -/** \brief This class provides an easy way to publish coordinate frame transform information. - * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the +/** \brief This class provides an easy way to publish coordinate frame transform information. + * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the * necessary data needed for each message. */ class StaticTransformBroadcaster{ public: - - TF2_ROS_PUBLIC - StaticTransformBroadcaster(rclcpp::Node::SharedPtr node); - - template> + /** \brief Node interface constructor */ + template> StaticTransformBroadcaster( - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface) + NodeT && node, + const rclcpp::QoS & qos = rclcpp::QoS(100), + const rclcpp::PublisherOptionsWithAllocator & options = + rclcpp::PublisherOptionsWithAllocator()) { - auto qos = rclcpp::QoS(rclcpp::KeepLast(100)); // TODO(tfoote) latched equivalent - - using MessageT = tf2_msgs::msg::TFMessage; - using PublisherT = ::rclcpp::Publisher; - publisher_ = rclcpp::create_publisher( - node_topics_interface, "/tf_static", qos); - } + publisher_ = rclcpp::create_publisher( + node, "/tf_static", qos, options); + }; /** \brief Send a TransformStamped message * The stamped data structure includes frame_id, and time, and parent_id already. */ diff --git a/tf2_ros/include/tf2_ros/transform_broadcaster.h b/tf2_ros/include/tf2_ros/transform_broadcaster.h index 61d6be09c..dc1ba443b 100644 --- a/tf2_ros/include/tf2_ros/transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/transform_broadcaster.h @@ -41,21 +41,29 @@ namespace tf2_ros { -/** \brief This class provides an easy way to publish coordinate frame transform information. - * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the +/** \brief This class provides an easy way to publish coordinate frame transform information. + * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the * necessary data needed for each message. */ class TransformBroadcaster{ public: - /** \brief Constructor (needs a ros::Node reference) */ - TF2_ROS_PUBLIC - TransformBroadcaster(rclcpp::Node::SharedPtr node); + /** \brief Node interface constructor */ + template> + TransformBroadcaster( + NodeT && node, + const rclcpp::QoS & qos = rclcpp::QoS(100), + const rclcpp::PublisherOptionsWithAllocator & options = + rclcpp::PublisherOptionsWithAllocator()) + { + publisher_ = rclcpp::create_publisher( + node, "/tf", qos, options); + }; - /** \brief Send a StampedTransform + /** \brief Send a StampedTransform * The stamped data structure includes frame_id, and time, and parent_id already. */ // void sendTransform(const StampedTransform & transform); - /** \brief Send a vector of StampedTransforms + /** \brief Send a vector of StampedTransforms * The stamped data structure includes frame_id, and time, and parent_id already. */ //void sendTransform(const std::vector & transforms); @@ -70,8 +78,6 @@ class TransformBroadcaster{ void sendTransform(const std::vector & transforms); private: - /// Internal reference to ros::Node - rclcpp::Node::SharedPtr node_; rclcpp::Publisher::SharedPtr publisher_; }; diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index e6c6ca2c1..3a5d3928e 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -29,9 +29,11 @@ /** \author Tully Foote */ -#ifndef TF2_ROS_TRANSFORMLISTENER_H -#define TF2_ROS_TRANSFORMLISTENER_H +#ifndef TF2_ROS__TRANSFORM_LISTENER_H_ +#define TF2_ROS__TRANSFORM_LISTENER_H_ +#include +#include #include #include "tf2_msgs/msg/tf_message.hpp" #include "rclcpp/rclcpp.hpp" @@ -39,55 +41,92 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/visibility_control.h" -namespace tf2_ros{ + +namespace tf2_ros +{ /** \brief This class provides an easy way to request and receive coordinate frame transform information. */ class TransformListener { - public: /**@brief Constructor for transform listener */ TF2_ROS_PUBLIC - TransformListener(tf2::BufferCore& buffer, bool spin_thread = true); - - TF2_ROS_PUBLIC - TransformListener(tf2::BufferCore& buffer, rclcpp::Node::SharedPtr nh, bool spin_thread = true); + explicit TransformListener(tf2::BufferCore & buffer, bool spin_thread = true); + + template> + TransformListener( + tf2::BufferCore & buffer, + NodeT && node, + bool spin_thread = true, + const rclcpp::QoS & qos = rclcpp::QoS(100), + const rclcpp::SubscriptionOptionsWithAllocator & options = + rclcpp::SubscriptionOptionsWithAllocator()) + : buffer_(buffer) + { + init(node, spin_thread, qos, options); + } TF2_ROS_PUBLIC - ~TransformListener(); + virtual ~TransformListener(); private: + template> + void init( + NodeT && node, + bool spin_thread, + const rclcpp::QoS & qos = rclcpp::QoS(100), + const rclcpp::SubscriptionOptionsWithAllocator & options = + rclcpp::SubscriptionOptionsWithAllocator()) + { + using callback_t = std::function; + callback_t cb = std::bind(&TransformListener::subscription_callback, this, std::placeholders::_1, false); + callback_t static_cb = std::bind(&TransformListener::subscription_callback, this, std::placeholders::_1, true); - /// Initialize this transform listener, subscribing, advertising services, etc. - void init(); - void initThread(); + message_subscription_tf_ = rclcpp::create_subscription( + node, + "/tf", + qos, + std::move(cb), + options); + message_subscription_tf_static_ = rclcpp::create_subscription( + node, + "/tf_static", + qos, + std::move(static_cb), + options); + + if (spin_thread) { + initThread(node->get_node_base_interface()); + } + } + + TF2_ROS_PUBLIC + void initThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface); /// Callback function for ros message subscriptoin - void subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg); - void static_subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg); - void subscription_callback_impl(const tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static); + TF2_ROS_PUBLIC + void subscription_callback(tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static); // ros::CallbackQueue tf_message_callback_queue_; - std::thread* dedicated_listener_thread_; - rclcpp::Node::SharedPtr node_; + std::thread * dedicated_listener_thread_ = nullptr; + + rclcpp::Node::SharedPtr optional_default_node_ = nullptr; rclcpp::Subscription::SharedPtr message_subscription_tf_; rclcpp::Subscription::SharedPtr message_subscription_tf_static_; - tf2::BufferCore& buffer_; - bool using_dedicated_thread_; + tf2::BufferCore & buffer_; + bool using_dedicated_thread_ = false; tf2::TimePoint last_update_; - + void dedicatedListenerThread() { - while (using_dedicated_thread_) - { + while (using_dedicated_thread_) { break; - //TODO(tfoote) reenable callback queue processing - //tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01)); + // TODO(tfoote) reenable callback queue processing + // tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01)); } - }; - + } }; -} +} // namespace tf2_ros -#endif //TF_TRANSFORMLISTENER_H +#endif // TF2_ROS__TRANSFORM_LISTENER_H_ diff --git a/tf2_ros/src/static_transform_broadcaster.cpp b/tf2_ros/src/static_transform_broadcaster.cpp index fcaf2ffc9..3c21aebc3 100644 --- a/tf2_ros/src/static_transform_broadcaster.cpp +++ b/tf2_ros/src/static_transform_broadcaster.cpp @@ -37,10 +37,6 @@ namespace tf2_ros { -StaticTransformBroadcaster::StaticTransformBroadcaster(rclcpp::Node::SharedPtr node) -: StaticTransformBroadcaster(node->get_node_topics_interface()) -{} - void StaticTransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped & msgtf) { std::vector v1; diff --git a/tf2_ros/src/transform_broadcaster.cpp b/tf2_ros/src/transform_broadcaster.cpp index 6ed09c0ab..2720367da 100644 --- a/tf2_ros/src/transform_broadcaster.cpp +++ b/tf2_ros/src/transform_broadcaster.cpp @@ -37,12 +37,6 @@ namespace tf2_ros { -TransformBroadcaster::TransformBroadcaster(rclcpp::Node::SharedPtr node) : - node_(node) -{ - publisher_ = node_->create_publisher("/tf", 100); -}; - void TransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped & msgtf) { std::vector v1; @@ -50,7 +44,6 @@ void TransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStam sendTransform(v1); } - void TransformBroadcaster::sendTransform(const std::vector & msgtf) { tf2_msgs::msg::TFMessage message; @@ -61,5 +54,4 @@ void TransformBroadcaster::sendTransform(const std::vectorpublish(message); } - } diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 5af60060c..b52104e99 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -29,106 +29,68 @@ /** \author Tully Foote */ +#include +#include +#include + #include "tf2_ros/transform_listener.h" +#include "rclcpp/create_subscription.hpp" using namespace tf2_ros; -//TODO(tfoote replace these terrible macros) +// TODO(tfoote replace these terrible macros) #define ROS_ERROR printf #define ROS_FATAL printf #define ROS_INFO printf #define ROS_WARN printf -TransformListener::TransformListener(tf2::BufferCore& buffer, bool spin_thread): - dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false) +TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) +: buffer_(buffer), + optional_default_node_(rclcpp::Node::make_shared("transform_listener_impl")) { - //TODO(tfoote)make this anonymous - node_ = rclcpp::Node::make_shared("transform_listener_impl"); - init(); - if (spin_thread) - initThread(); + init(optional_default_node_, spin_thread); } -TransformListener::TransformListener(tf2::BufferCore& buffer, rclcpp::Node::SharedPtr nh, bool spin_thread) -: dedicated_listener_thread_(NULL) -, node_(nh) -, buffer_(buffer) -, using_dedicated_thread_(false) -{ - init(); - if (spin_thread) - initThread(); -} - - TransformListener::~TransformListener() { using_dedicated_thread_ = false; - if (dedicated_listener_thread_) - { + if (dedicated_listener_thread_) { dedicated_listener_thread_->join(); delete dedicated_listener_thread_; } } -void test_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg){ - return; -} - -void TransformListener::init() -{ - message_subscription_tf_ = node_->create_subscription( - "/tf", 100, std::bind(&TransformListener::subscription_callback, this, std::placeholders::_1)); - message_subscription_tf_static_ = node_->create_subscription( - "/tf_static", - 100, - std::bind(&TransformListener::static_subscription_callback, this, std::placeholders::_1)); -} - -void TransformListener::initThread() +void TransformListener::initThread( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { using_dedicated_thread_ = true; // This lambda is required because `std::thread` cannot infer the correct // rclcpp::spin, since there are more than one versions of it (overloaded). // see: http://stackoverflow.com/a/27389714/671658 // I (wjwwood) chose to use the lamda rather than the static cast solution. - auto run_func = [](rclcpp::Node::SharedPtr node) { - return rclcpp::spin(node); - }; - dedicated_listener_thread_ = new std::thread(run_func, node_); - //Tell the buffer we have a dedicated thread to enable timeouts + auto run_func = [](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { + return rclcpp::spin(node_base_interface); + }; + dedicated_listener_thread_ = new std::thread(run_func, node_base_interface); + // Tell the buffer we have a dedicated thread to enable timeouts buffer_.setUsingDedicatedThread(true); } - - -void TransformListener::subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) +void TransformListener::subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static) { - subscription_callback_impl(msg, false); -} -void TransformListener::static_subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) -{ - subscription_callback_impl(msg, true); -} - -void TransformListener::subscription_callback_impl(const tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static) -{ - const tf2_msgs::msg::TFMessage& msg_in = *msg; - //TODO(tfoote) find a way to get the authority - std::string authority = "Authority undetectable"; //msg_evt.getPublisherName(); // lookup the authority - for (unsigned int i = 0; i < msg_in.transforms.size(); i++) - { - try - { + const tf2_msgs::msg::TFMessage & msg_in = *msg; + // TODO(tfoote) find a way to get the authority + std::string authority = "Authority undetectable"; // msg_evt.getPublisherName(); // lookup the authority + for (auto i = 0u; i < msg_in.transforms.size(); i++) { + try { buffer_.setTransform(msg_in.transforms[i], authority, is_static); - } - - catch (tf2::TransformException& ex) - { - ///\todo Use error reporting + } catch (tf2::TransformException & ex) { + // /\todo Use error reporting std::string temp = ex.what(); - ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str()); + ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", + msg_in.transforms[i].child_frame_id.c_str(), + msg_in.transforms[i].header.frame_id.c_str(), temp.c_str()); } } -}; +} diff --git a/tf2_ros/test/node_wrapper.hpp b/tf2_ros/test/node_wrapper.hpp new file mode 100644 index 000000000..45613d718 --- /dev/null +++ b/tf2_ros/test/node_wrapper.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef NODE_WRAPPER_HPP_ +#define NODE_WRAPPER_HPP_ + +#include +#include + +class NodeWrapper +{ +public: + explicit NodeWrapper(const std::string & name) + : node(std::make_shared(name)) + {} + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() {return this->node->get_node_base_interface();} + + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr + get_node_topics_interface() {return this->node->get_node_topics_interface();} + +private: + rclcpp::Node::SharedPtr node; +}; + +#endif // NODE_WRAPPER_HPP_ diff --git a/tf2_ros/test/test_static_transform_broadcaster.cpp b/tf2_ros/test/test_static_transform_broadcaster.cpp new file mode 100644 index 000000000..76890d8be --- /dev/null +++ b/tf2_ros/test/test_static_transform_broadcaster.cpp @@ -0,0 +1,76 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "node_wrapper.hpp" + +class CustomNode : public rclcpp::Node +{ +public: + CustomNode() + : rclcpp::Node("tf2_ros_test_static_transform_broadcaster_node") + {} + + void init_tf_broadcaster() + { + tf_broadcaster_ = std::make_shared(shared_from_this()); + } + +private: + std::shared_ptr tf_broadcaster_; +}; + +TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_rclcpp_node) +{ + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + tf2_ros::StaticTransformBroadcaster tfb(node); +} + +TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_custom_rclcpp_node) +{ + auto node = std::make_shared("tf2_ros_message_filter"); + + tf2_ros::StaticTransformBroadcaster tfb(node); +} + +TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_as_member) +{ + auto custom_node = std::make_shared(); + custom_node->init_tf_broadcaster(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tf2_ros/test/test_transform_broadcaster.cpp b/tf2_ros/test/test_transform_broadcaster.cpp new file mode 100644 index 000000000..9a4a68979 --- /dev/null +++ b/tf2_ros/test/test_transform_broadcaster.cpp @@ -0,0 +1,76 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "node_wrapper.hpp" + +class CustomNode : public rclcpp::Node +{ +public: + CustomNode() + : rclcpp::Node("tf2_ros_test_transform_broadcaster_node") + {} + + void init_tf_broadcaster() + { + tf_broadcaster_ = std::make_shared(shared_from_this()); + } + +private: + std::shared_ptr tf_broadcaster_; +}; + +TEST(tf2_test_transform_broadcaster, transform_broadcaster_rclcpp_node) +{ + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + tf2_ros::TransformBroadcaster tfb(node); +} + +TEST(tf2_test_transform_broadcaster, transform_broadcaster_custom_rclcpp_node) +{ + auto node = std::make_shared("tf2_ros_message_filter"); + + tf2_ros::TransformBroadcaster tfb(node); +} + +TEST(tf2_test_transform_broadcaster, transform_broadcaster_as_member) +{ + auto custom_node = std::make_shared(); + custom_node->init_tf_broadcaster(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tf2_ros/test/test_transform_listener.cpp b/tf2_ros/test/test_transform_listener.cpp new file mode 100644 index 000000000..e368921ef --- /dev/null +++ b/tf2_ros/test/test_transform_listener.cpp @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "node_wrapper.hpp" + +class CustomNode : public rclcpp::Node +{ +public: + CustomNode() + : rclcpp::Node("tf2_ros_test_transform_listener_node") + {} + + void init_tf_listener() + { + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf_listener_ = std::make_shared(buffer, shared_from_this(), false); + } + +private: + std::shared_ptr tf_listener_; +}; + +TEST(tf2_test_transform_listener, transform_listener_rclcpp_node) +{ + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::TransformListener tfl(buffer, node, false); +} + +TEST(tf2_test_transform_listener, transform_listener_custom_rclcpp_node) +{ + auto node = std::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::TransformListener tfl(buffer, node, false); +} + +TEST(tf2_test_transform_listener, transform_listener_as_member) +{ + auto custom_node = std::make_shared(); + custom_node->init_tf_listener(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +}