improved implementation

Signed-off-by: ahcorde <[email protected]>

CMakeLists.txt

@@ -13,6 +13,7 @@ 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
@@ -25,6 +26,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC include)
 ament_target_dependencies(${PROJECT_NAME}
     control_msgs
     rclcpp
+    rclcpp_lifecycle
     realtime_tools)
 
 if(BUILD_TESTING)
@@ -43,6 +45,7 @@ if(BUILD_TESTING)
 
   ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp)
   target_link_libraries(pid_publisher_tests ${PROJECT_NAME})
+  ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle)
 endif()
 
 # Install
@@ -55,7 +58,9 @@ install(TARGETS ${PROJECT_NAME}
   RUNTIME DESTINATION bin)
 
 ament_export_dependencies(
+  control_msgs
   rclcpp
+  rclcpp_lifecycle
   realtime_tools)
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})

include/control_toolbox/pid_ros.hpp

@@ -31,8 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-#ifndef CONTROL_TOOLBOX__PIDROS_HPP_
-#define CONTROL_TOOLBOX__PIDROS_HPP_
+#ifndef CONTROL_TOOLBOX__PID_ROS_HPP_
+#define CONTROL_TOOLBOX__PID_ROS_HPP_
 
 #include <memory>
 #include <string>
@@ -177,9 +177,9 @@ class PidROS
 
 }  // namespace control_toolbox
 
-#ifndef CONTROL_TOOLBOX__PIDROS_IMPL_HPP_
+#ifndef CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_
 // Template implementations
 #include "pid_ros_impl.hpp"
 #endif
 
-#endif  // CONTROL_TOOLBOX__PIDROS_HPP_
+#endif  // CONTROL_TOOLBOX__PID_ROS_HPP_

include/control_toolbox/pid_ros_impl.hpp

@@ -31,32 +31,43 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-#ifndef CONTROL_TOOLBOX__PIDROS_IMPL_HPP_
-#define CONTROL_TOOLBOX__PIDROS_IMPL_HPP_
+#ifndef CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_
+#define CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_
 
 #include <algorithm>
 #include <cmath>
+#include <memory>
 #include <string>
 #include <utility>
 #include <vector>
 
 #include "control_toolbox/pid_ros.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
 
 namespace control_toolbox
 {
 
 template<typename NodeT>
 PidROS<NodeT>::PidROS(std::shared_ptr<NodeT> node_ptr, std::string topic_prefix)
-:node_(node_ptr)
+: node_(node_ptr)
 {
   if (topic_prefix.back() != '.' && !topic_prefix.empty()) {
     topic_prefix_ = topic_prefix + ".";
   } else {
     topic_prefix_ = topic_prefix;
   }
 
-  state_pub_ =  node_->create_publisher<control_msgs::msg::PidState>(
+  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_));
 }
@@ -69,7 +80,9 @@ PidROS<NodeT>::getBooleanParam(const std::string & param_name, bool & value)
   if (node_->has_parameter(param_name)) {
     node_->get_parameter(param_name, param);
     if (rclcpp::PARAMETER_BOOL != param.get_type()) {
-      RCLCPP_ERROR(node_->get_logger(), "Wrong parameter type '%s', not boolean", param_name.c_str());
+      RCLCPP_ERROR(
+        node_->get_logger(), "Wrong parameter type '%s', not boolean",
+        param_name.c_str());
       return false;
     }
     value = param.as_bool();
@@ -87,7 +100,9 @@ PidROS<NodeT>::getDoubleParam(const std::string & param_name, double & value)
   if (node_->has_parameter(param_name)) {
     node_->get_parameter(param_name, param);
     if (rclcpp::PARAMETER_DOUBLE != param.get_type()) {
-      RCLCPP_ERROR(node_->get_logger(), "Wrong parameter type '%s', not double", param_name.c_str());
+      RCLCPP_ERROR(
+        node_->get_logger(), "Wrong parameter type '%s', not double",
+        param_name.c_str());
       return false;
     }
     value = param.as_double();
@@ -327,9 +342,9 @@ PidROS<NodeT>::setParameterEventCallback()
 
   parameter_callback_ =
     node_->get_node_parameters_interface()->add_on_set_parameters_callback(
-      on_parameter_event_callback);
+    on_parameter_event_callback);
 }
 
 }  // namespace control_toolbox
 
-#endif  // CONTROL_TOOLBOX__PIDROS_IMPL_HPP_
+#endif  // CONTROL_TOOLBOX__PID_ROS_IMPL_HPP_

package.xml

@@ -20,6 +20,7 @@
 
   <depend>control_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>realtime_tools</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>

src/pid.cpp

@@ -140,7 +140,7 @@ double Pid::computeCommand(double error, uint64_t dt)
   error_dot_ = d_error_;
 
   // Calculate the derivative error
-  error_dot_ = (error - p_error_last_) / (dt/1e9);
+  error_dot_ = (error - p_error_last_) / (dt / 1e9);
   p_error_last_ = error;
 
   return computeCommand(error, error_dot_, dt);
@@ -166,7 +166,7 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt)
   p_term = gains.p_gain_ * p_error_;
 
   // Calculate the integral of the position error
-  i_error_ += (dt/1e9) * p_error_;
+  i_error_ += (dt / 1e9) * p_error_;
 
   if (gains.antiwindup_ && gains.i_gain_ != 0) {
     // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_

test/pid_publisher_tests.cpp

@@ -65,6 +65,40 @@ TEST(PidPublihserTest, PublishTest)
   ASSERT_TRUE(callback_called);
 }
 
+TEST(PidPublihserTest, PublishTestLifecycle)
+{
+  const size_t ATTEMPTS = 100;
+  const std::chrono::milliseconds DELAY(250);
+
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("pid_publisher_test");
+
+  control_toolbox::PidROS<rclcpp_lifecycle::LifecycleNode> pid_ros(node);
+
+  pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false);
+
+  bool callback_called = false;
+  control_msgs::msg::PidState::SharedPtr last_state_msg;
+  auto state_callback = [&](const control_msgs::msg::PidState::SharedPtr)
+    {
+      callback_called = true;
+    };
+
+  auto state_sub = node->create_subscription<control_msgs::msg::PidState>(
+    "/pid_state", rclcpp::SensorDataQoS(), state_callback);
+
+  double command = pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0));
+  EXPECT_EQ(-1.5, command);
+
+  // wait for callback
+  for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) {
+    pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0));
+    rclcpp::spin_some(node->get_node_base_interface());
+    std::this_thread::sleep_for(DELAY);
+  }
+
+  ASSERT_TRUE(callback_called);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);