diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index 789fb7e4f..38970ec24 100644 --- a/tf2_ros/CMakeLists.txt +++ b/tf2_ros/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(ament_cmake REQUIRED) # find_package(actionlib_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_msgs REQUIRED) @@ -24,6 +25,7 @@ find_package(rmw) include_directories(include ${geometry_msgs_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} + ${rclcpp_lifecycle_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ${rmw_implementation_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS} @@ -43,6 +45,7 @@ add_library(${PROJECT_NAME} SHARED target_link_libraries(${PROJECT_NAME} ${geometry_msgs_LIBRARIES} ${rclcpp_LIBRARIES} + ${rclcpp_lifecycle_LIBRARIES} ${rmw_LIBRARIES} ${rmw_implementation_LIBRARIES} ${tf2_LIBRARIES} @@ -70,6 +73,7 @@ target_link_libraries(static_transform_publisher ${PROJECT_NAME} ${geometry_msgs_LIBRARIES} ${rclcpp_LIBRARIES} + ${rclcpp_lifecycle_LIBRARIES} ${rmw_LIBRARIES} ${rmw_implementation_LIBRARIES} ${tf2_LIBRARIES} @@ -83,6 +87,7 @@ target_link_libraries(tf2_echo ${PROJECT_NAME} ${geometry_msgs_LIBRARIES} ${rclcpp_LIBRARIES} + ${rclcpp_lifecycle_LIBRARIES} ${rmw_LIBRARIES} ${rmw_implementation_LIBRARIES} ${tf2_LIBRARIES} @@ -96,6 +101,7 @@ target_link_libraries(tf2_echo ${PROJECT_NAME} ${geometry_msgs_LIBRARIES} ${rclcpp_LIBRARIES} + ${rclcpp_lifecycle_LIBRARIES} ${rmw_LIBRARIES} ${rmw_implementation_LIBRARIES} ${tf2_LIBRARIES} diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h index 1a5eb227f..397f037f3 100644 --- a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h @@ -1,10 +1,10 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * 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 @@ -13,7 +13,7 @@ * * 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 @@ -36,41 +36,52 @@ #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/visibility_control.h" -namespace tf2_ros -{ +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 - * necessary data needed for each message. */ + /** \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: + class StaticTransformBroadcaster { + public: - TF2_ROS_PUBLIC - StaticTransformBroadcaster(rclcpp::Node::SharedPtr node); + TF2_ROS_PUBLIC + StaticTransformBroadcaster(rclcpp::Node::SharedPtr node); - /** \brief Send a TransformStamped message - * The stamped data structure includes frame_id, and time, and parent_id already. */ - TF2_ROS_PUBLIC - void sendTransform(const geometry_msgs::msg::TransformStamped & transform); + TF2_ROS_PUBLIC + StaticTransformBroadcaster(rclcpp_lifecycle::LifecycleNode::SharedPtr node); - /** \brief Send a vector of TransformStamped messages - * The stamped data structure includes frame_id, and time, and parent_id already. */ - TF2_ROS_PUBLIC - void sendTransform(const std::vector & transforms); + /** \brief Send a TransformStamped message + * The stamped data structure includes frame_id, and time, and parent_id already. */ + TF2_ROS_PUBLIC + void sendTransform(const geometry_msgs::msg::TransformStamped& transform); -private: - /// Internal reference to ros::Node - rclcpp::Node::SharedPtr node_; - rclcpp::Publisher::SharedPtr publisher_; - tf2_msgs::msg::TFMessage net_message_; + /** \brief Send a vector of TransformStamped messages + * The stamped data structure includes frame_id, and time, and parent_id already. */ + TF2_ROS_PUBLIC + void sendTransform(const std::vector& transforms); -}; + private: + /// Internal reference to rclcpp::Node + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr publisher_; + tf2_msgs::msg::TFMessage net_message_; + + /// Internal reference to rclcpp_lifecycle::LifecycleNode + rclcpp_lifecycle::LifecycleNode::SharedPtr lf_node_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr lf_publisher_; + + /// Discriminates node mode + bool lifecycle_ = false; + + }; } diff --git a/tf2_ros/include/tf2_ros/transform_broadcaster.h b/tf2_ros/include/tf2_ros/transform_broadcaster.h index 61d6be09c..f5e55518a 100644 --- a/tf2_ros/include/tf2_ros/transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/transform_broadcaster.h @@ -1,10 +1,10 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * 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 @@ -13,7 +13,7 @@ * * 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 @@ -34,47 +34,59 @@ #define TF2_ROS_TRANSFORMBROADCASTER_H #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/visibility_control.h" -namespace tf2_ros -{ +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 - * necessary data needed for each message. */ + /** \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); + class TransformBroadcaster { + public: + /** \brief Constructor (needs a ros::Node reference) */ + TF2_ROS_PUBLIC + TransformBroadcaster(rclcpp::Node::SharedPtr node); - /** \brief Send a StampedTransform - * The stamped data structure includes frame_id, and time, and parent_id already. */ - // void sendTransform(const StampedTransform & transform); + /** \brief Constructor (needs a ros::Node reference) */ + TF2_ROS_PUBLIC + TransformBroadcaster(rclcpp_lifecycle::LifecycleNode::SharedPtr node); - /** \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); + /** \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 TransformStamped message - * The stamped data structure includes frame_id, and time, and parent_id already. */ - TF2_ROS_PUBLIC - void sendTransform(const geometry_msgs::msg::TransformStamped & transform); + /** \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); - /** \brief Send a vector of TransformStamped messages - * The stamped data structure includes frame_id, and time, and parent_id already. */ - TF2_ROS_PUBLIC - void sendTransform(const std::vector & transforms); + /** \brief Send a TransformStamped message + * The stamped data structure includes frame_id, and time, and parent_id already. */ + TF2_ROS_PUBLIC + void sendTransform(const geometry_msgs::msg::TransformStamped& transform); -private: - /// Internal reference to ros::Node - rclcpp::Node::SharedPtr node_; - rclcpp::Publisher::SharedPtr publisher_; + /** \brief Send a vector of TransformStamped messages + * The stamped data structure includes frame_id, and time, and parent_id already. */ + TF2_ROS_PUBLIC + void sendTransform(const std::vector& transforms); -}; + private: + /// Internal reference to rclcpp::Node + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr publisher_; + + /// Internal reference to rclcpp_lifecycle::LifecycleNode + rclcpp_lifecycle::LifecycleNode::SharedPtr lf_node_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr lf_publisher_; + + /// Discriminates node mode + bool lifecycle_ = false; + + }; } diff --git a/tf2_ros/package.xml b/tf2_ros/package.xml index fb691871c..b7199d982 100644 --- a/tf2_ros/package.xml +++ b/tf2_ros/package.xml @@ -15,12 +15,14 @@ geometry_msgs rclcpp + rclcpp_lifecycle std_msgs tf2 tf2_msgs geometry_msgs rclcpp + rclcpp_lifecycle std_msgs tf2 tf2_msgs diff --git a/tf2_ros/src/static_transform_broadcaster.cpp b/tf2_ros/src/static_transform_broadcaster.cpp index 826c49153..f925c6c5d 100644 --- a/tf2_ros/src/static_transform_broadcaster.cpp +++ b/tf2_ros/src/static_transform_broadcaster.cpp @@ -1,10 +1,10 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * 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 @@ -13,7 +13,7 @@ * * 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 @@ -37,41 +37,54 @@ namespace tf2_ros { -StaticTransformBroadcaster::StaticTransformBroadcaster(rclcpp::Node::SharedPtr node): - node_(node) -{ - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; - custom_qos_profile.depth = 100; - // TODO(tfoote) latched equivalent - publisher_ = node_->create_publisher("/tf_static", custom_qos_profile); -} + StaticTransformBroadcaster::StaticTransformBroadcaster(rclcpp::Node::SharedPtr node): + node_(node) { + lifecycle_ = false; + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; + custom_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + custom_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + custom_qos_profile.depth = 1; + // TODO(tfoote) check if this is latched equivalent + publisher_ = node_->create_publisher("/tf_static", custom_qos_profile); + } -void StaticTransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped & msgtf) -{ - std::vector v1; - v1.push_back(msgtf); - sendTransform(v1); -} + StaticTransformBroadcaster::StaticTransformBroadcaster(rclcpp_lifecycle::LifecycleNode::SharedPtr node): + lf_node_(node) { + lifecycle_ = true; + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; + custom_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + custom_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + custom_qos_profile.depth = 1; + // TODO(tfoote) check if this is latched equivalent + lf_publisher_ = lf_node_->create_publisher("/tf_static", custom_qos_profile); + } -void StaticTransformBroadcaster::sendTransform(const std::vector & msgtf) -{ - for (auto it_in = msgtf.begin(); it_in != msgtf.end(); ++it_in) - { - bool match_found = false; - for (auto it_msg = net_message_.transforms.begin(); it_msg != net_message_.transforms.end(); ++it_msg) - { - if (it_in->child_frame_id == it_msg->child_frame_id) - { - *it_msg = *it_in; - match_found = true; - break; - } + void StaticTransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped& msgtf) { + std::vector v1; + v1.push_back(msgtf); + sendTransform(v1); } - if (! match_found) - net_message_.transforms.push_back(*it_in); - } - publisher_->publish(net_message_); -} + void StaticTransformBroadcaster::sendTransform(const std::vector& msgtf) { + for (auto it_in = msgtf.begin(); it_in != msgtf.end(); ++it_in) { + bool match_found = false; + for (auto it_msg = net_message_.transforms.begin(); it_msg != net_message_.transforms.end(); ++it_msg) { + if (it_in->child_frame_id == it_msg->child_frame_id) { + *it_msg = *it_in; + match_found = true; + break; + } + } + if (! match_found) { + net_message_.transforms.push_back(*it_in); + } + } + + if (!lifecycle_) { + publisher_->publish(net_message_); + } else { + lf_publisher_->publish(net_message_); + } + } } diff --git a/tf2_ros/src/transform_broadcaster.cpp b/tf2_ros/src/transform_broadcaster.cpp index a6231e925..53f5da646 100644 --- a/tf2_ros/src/transform_broadcaster.cpp +++ b/tf2_ros/src/transform_broadcaster.cpp @@ -1,10 +1,10 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * 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 @@ -13,7 +13,7 @@ * * 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 @@ -32,36 +32,48 @@ #include "rclcpp/rclcpp.hpp" + #include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/transform_broadcaster.h" namespace tf2_ros { -TransformBroadcaster::TransformBroadcaster(rclcpp::Node::SharedPtr node) : - node_(node) -{ - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; - custom_qos_profile.depth = 100; - publisher_ = node_->create_publisher("/tf", custom_qos_profile); -}; + TransformBroadcaster::TransformBroadcaster(rclcpp::Node::SharedPtr node) : + node_(node) { + lifecycle_ = false; -void TransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped & msgtf) -{ - std::vector v1; - v1.push_back(msgtf); - sendTransform(v1); -} + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; + custom_qos_profile.depth = 100; + publisher_ = node_->create_publisher("/tf", custom_qos_profile); + }; + TF2_ROS_PUBLIC + TransformBroadcaster::TransformBroadcaster(rclcpp_lifecycle::LifecycleNode::SharedPtr node) : + lf_node_(node) { + lifecycle_ = true; -void TransformBroadcaster::sendTransform(const std::vector & msgtf) -{ - tf2_msgs::msg::TFMessage message; - for (std::vector::const_iterator it = msgtf.begin(); it != msgtf.end(); ++it) - { - message.transforms.push_back(*it); - } - publisher_->publish(message); -} + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; + custom_qos_profile.depth = 100; + lf_publisher_ = lf_node_->create_publisher("/tf", custom_qos_profile); + } + + void TransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped& msgtf) { + std::vector v1; + v1.push_back(msgtf); + sendTransform(v1); + } + + void TransformBroadcaster::sendTransform(const std::vector& msgtf) { + tf2_msgs::msg::TFMessage message; + for (std::vector::const_iterator it = msgtf.begin(); it != msgtf.end(); ++it) { + message.transforms.push_back(*it); + } + if (!lifecycle_) { + publisher_->publish(message); + } else { + lf_publisher_->publish(message); + } + } }