From fb5dac0b00dd3dbbb510362e3ba1f66be0a100f0 Mon Sep 17 00:00:00 2001
From: ahcorde <ahcorde@gmail.com>
Date: Sat, 20 Jun 2020 12:06:33 +0200
Subject: [PATCH] Removed lifecycle as a depend

Signed-off-by: ahcorde <ahcorde@gmail.com>
---
 CMakeLists.txt                           |  4 +---
 include/control_toolbox/pid_ros.hpp      |  7 +++++++
 include/control_toolbox/pid_ros_impl.hpp | 17 +++++++----------
 package.xml                              |  2 +-
 test/pid_publisher_tests.cpp             |  5 +++++
 5 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ef06902c..93bcd183 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,7 +13,6 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
 find_package(realtime_tools REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
@@ -26,13 +25,13 @@ target_include_directories(${PROJECT_NAME} PUBLIC include)
 ament_target_dependencies(${PROJECT_NAME}
     control_msgs
     rclcpp
-    rclcpp_lifecycle
     realtime_tools)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
+  find_package(rclcpp_lifecycle REQUIRED)
   list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright)
 
   ament_lint_auto_find_test_dependencies()
@@ -60,7 +59,6 @@ install(TARGETS ${PROJECT_NAME}
 ament_export_dependencies(
   control_msgs
   rclcpp
-  rclcpp_lifecycle
   realtime_tools)
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp
index ae4f716b..aeec5e47 100644
--- a/include/control_toolbox/pid_ros.hpp
+++ b/include/control_toolbox/pid_ros.hpp
@@ -140,6 +140,13 @@ class PidROS
    */
   double getCurrentCmd();
 
+  /*!
+   * \brief Return PID state publisher
+   * \return shared_ptr to the PID state publisher
+   */
+  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>>
+  getPidStatePublisher();
+
   /*!
    * \brief Return PID error terms for the controller.
    * \param pe[out] The proportional error.
diff --git a/include/control_toolbox/pid_ros_impl.hpp b/include/control_toolbox/pid_ros_impl.hpp
index 2657fa24..d308b877 100644
--- a/include/control_toolbox/pid_ros_impl.hpp
+++ b/include/control_toolbox/pid_ros_impl.hpp
@@ -42,8 +42,6 @@
 #include <vector>
 
 #include "control_toolbox/pid_ros.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
 
 namespace control_toolbox
 {
@@ -60,14 +58,6 @@ PidROS<NodeT>::PidROS(std::shared_ptr<NodeT> node_ptr, std::string topic_prefix)
 
   state_pub_ = node_->template create_publisher<control_msgs::msg::PidState>(
     topic_prefix + "/pid_state", rclcpp::SensorDataQoS());
-  rclcpp_lifecycle::LifecycleNode::SharedPtr is_lifecycle_node =
-    std::dynamic_pointer_cast<rclcpp_lifecycle::LifecycleNode>(node_);
-  if (is_lifecycle_node) {
-    auto state_pub_lifecycle_ =
-      std::dynamic_pointer_cast<rclcpp_lifecycle::LifecyclePublisher<control_msgs::msg::PidState>>(
-      state_pub_);
-    state_pub_lifecycle_->on_activate();
-  }
   rt_state_pub_.reset(
     new realtime_tools::RealtimePublisher<control_msgs::msg::PidState>(state_pub_));
 }
@@ -174,6 +164,13 @@ PidROS<NodeT>::reset()
   pid_.reset();
 }
 
+template<typename NodeT>
+std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>>
+PidROS<NodeT>::getPidStatePublisher()
+{
+  return state_pub_;
+}
+
 template<typename NodeT>
 double
 PidROS<NodeT>::computeCommand(double error, rclcpp::Duration dt)
diff --git a/package.xml b/package.xml
index 32c63635..17d1a027 100644
--- a/package.xml
+++ b/package.xml
@@ -20,12 +20,12 @@
 
   <depend>control_msgs</depend>
   <depend>rclcpp</depend>
-  <depend>rclcpp_lifecycle</depend>
   <depend>realtime_tools</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>rclcpp_lifecycle</test_depend>
 
 </package>
diff --git a/test/pid_publisher_tests.cpp b/test/pid_publisher_tests.cpp
index 7067bb80..3ec0891d 100644
--- a/test/pid_publisher_tests.cpp
+++ b/test/pid_publisher_tests.cpp
@@ -74,6 +74,11 @@ TEST(PidPublihserTest, PublishTestLifecycle)
 
   control_toolbox::PidROS<rclcpp_lifecycle::LifecycleNode> pid_ros(node);
 
+  auto state_pub_lifecycle_ =
+      std::dynamic_pointer_cast<rclcpp_lifecycle::LifecyclePublisher<control_msgs::msg::PidState>>(
+      pid_ros.getPidStatePublisher());
+    state_pub_lifecycle_->on_activate();
+
   pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false);
 
   bool callback_called = false;