ros2 · Myzhar · Nov 5, 2018
@@ -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}

@@ -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<geometry_msgs::msg::TransformStamped> & 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<tf2_msgs::msg::TFMessage>::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<geometry_msgs::msg::TransformStamped>& transforms);
 
-};
+      private:
+        /// Internal reference to rclcpp::Node
+        rclcpp::Node::SharedPtr node_;
+        rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr publisher_;
+        tf2_msgs::msg::TFMessage net_message_;
+
+        /// Internal reference to rclcpp_lifecycle::LifecycleNode
+        rclcpp_lifecycle::LifecycleNode::SharedPtr lf_node_;
+        rclcpp_lifecycle::LifecyclePublisher<tf2_msgs::msg::TFMessage>::SharedPtr lf_publisher_;
+
+        /// Discriminates node mode
+        bool lifecycle_ = false;
+
+    };
 
 }
 

@@ -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<StampedTransform> & 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<StampedTransform> & 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<geometry_msgs::msg::TransformStamped> & 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<tf2_msgs::msg::TFMessage>::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<geometry_msgs::msg::TransformStamped>& transforms);
 
-};
+      private:
+        /// Internal reference to rclcpp::Node
+        rclcpp::Node::SharedPtr node_;
+        rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr publisher_;
+
+        /// Internal reference to rclcpp_lifecycle::LifecycleNode
+        rclcpp_lifecycle::LifecycleNode::SharedPtr lf_node_;
+        rclcpp_lifecycle::LifecyclePublisher<tf2_msgs::msg::TFMessage>::SharedPtr lf_publisher_;
+
+        /// Discriminates node mode
+        bool lifecycle_ = false;
+
+    };
 
 }
 

@@ -15,12 +15,14 @@
 
   <build_depend>geometry_msgs</build_depend>
   <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_lifecycle</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>tf2</build_depend>
   <build_depend>tf2_msgs</build_depend>
 
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_lifecycle</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>tf2</exec_depend>
   <exec_depend>tf2_msgs</exec_depend>

@@ -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<tf2_msgs::msg::TFMessage>("/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<tf2_msgs::msg::TFMessage>("/tf_static", custom_qos_profile);
+    }
 
-void StaticTransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped & msgtf)
-{
-  std::vector<geometry_msgs::msg::TransformStamped> 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<tf2_msgs::msg::TFMessage>("/tf_static", custom_qos_profile);
+    }
 
-void StaticTransformBroadcaster::sendTransform(const std::vector<geometry_msgs::msg::TransformStamped> & 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<geometry_msgs::msg::TransformStamped> 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<geometry_msgs::msg::TransformStamped>& 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_);
+        }
+    }
 
 }