Twist Controller sync for open sourcing

This Commit builds on a rebased PR #300

The Original author @livanov93 was in 2022 open sourcing part of a
PickNik package form 2021.

This commit brings the internal picknik and previously open PR #300 back
in sync and rebases the open PR onto humble.

Author: moriarty

cartesian_controllers/include/cartesian_controllers/twist_controller.hpp

@@ -1,4 +1,4 @@
-// Copyright 2022 VoodooIT, sole proprietorship
+// Copyright 2021, PickNik Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -21,14 +21,14 @@
 
 #include "controller_interface/controller_interface.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
-#include "realtime_tools/realtime_buffer.h"
-
 #include "cartesian_controllers/visibility_control.h"
+#include "realtime_tools/realtime_buffer.h"
 
 namespace cartesian_controllers
 {
 using CmdType = geometry_msgs::msg::TwistStamped;
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
 class TwistController : public controller_interface::ControllerInterface
 {
 public:
@@ -44,10 +44,6 @@ class TwistController : public controller_interface::ControllerInterface
   CARTESIAN_CONTROLLERS_PUBLIC
   CallbackReturn on_init() override;
 
-  CARTESIAN_CONTROLLERS_PUBLIC
-  controller_interface::return_type update(
-    const rclcpp::Time & time, const rclcpp::Duration & period) override;
-
   CARTESIAN_CONTROLLERS_PUBLIC
   CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
 
@@ -57,6 +53,10 @@ class TwistController : public controller_interface::ControllerInterface
   CARTESIAN_CONTROLLERS_PUBLIC
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
+  CARTESIAN_CONTROLLERS_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
 protected:
   std::string joint_name_;
   std::vector<std::string> interface_names_;

cartesian_controllers/include/cartesian_controllers/visibility_control.h

@@ -1,4 +1,4 @@
-// Copyright 2022 VoodooIT, sole proprietorship
+// Copyright 2021, PickNik Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

cartesian_controllers/src/twist_controller.cpp

@@ -1,4 +1,4 @@
-// Copyright 2022 VoodooIT, sole proprietorship
+// Copyright 2021, PickNik Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -14,8 +14,17 @@
 
 #include "cartesian_controllers/twist_controller.hpp"
 
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/helpers.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+
 namespace cartesian_controllers
 {
+using hardware_interface::LoanedCommandInterface;
+
 TwistController::TwistController()
 : controller_interface::ControllerInterface(),
   rt_command_ptr_(nullptr),
@@ -60,38 +69,9 @@ CallbackReturn TwistController::on_init()
   return CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type TwistController::update(
-  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
-{
-  auto twist_commands = rt_command_ptr_.readFromRT();
-
-  // no command received yet
-  if (!twist_commands || !(*twist_commands))
-  {
-    return controller_interface::return_type::OK;
-  }
-
-  if (command_interfaces_.size() != 6)
-  {
-    RCLCPP_ERROR_THROTTLE(
-      get_node()->get_logger(), *node_->get_clock(), 1000,
-      "Twist controller needs does not match number of interfaces needed 6, given (%zu) interfaces",
-      command_interfaces_.size());
-    return controller_interface::return_type::ERROR;
-  }
-  command_interfaces_[0].set_value((*twist_commands)->twist.linear.x);
-  command_interfaces_[1].set_value((*twist_commands)->twist.linear.y);
-  command_interfaces_[2].set_value((*twist_commands)->twist.linear.z);
-  command_interfaces_[3].set_value((*twist_commands)->twist.angular.x);
-  command_interfaces_[4].set_value((*twist_commands)->twist.angular.y);
-  command_interfaces_[5].set_value((*twist_commands)->twist.angular.z);
-
-  return controller_interface::return_type::OK;
-}
-
 CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  joint_name_ = node_->get_parameter("joint").as_string();
+  joint_name_ = get_node()->get_parameter("joint").as_string();
 
   if (joint_name_.empty())
   {
@@ -102,7 +82,7 @@ CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & /*p
   // Specialized, child controllers set interfaces before calling configure function.
   if (interface_names_.empty())
   {
-    interface_names_ = node_->get_parameter("interface_names").as_string_array();
+    interface_names_ = get_node()->get_parameter("interface_names").as_string_array();
   }
 
   if (interface_names_.empty())
@@ -133,7 +113,36 @@ CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State & /*
   return CallbackReturn::SUCCESS;
 }
 
+controller_interface::return_type TwistController::update(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  auto twist_commands = rt_command_ptr_.readFromRT();
+
+  // no command received yet
+  if (!twist_commands || !(*twist_commands))
+  {
+    return controller_interface::return_type::OK;
+  }
+
+  if (command_interfaces_.size() != 6)
+  {
+    RCLCPP_ERROR_THROTTLE(
+      get_node()->get_logger(), *get_node()->get_clock(), 1000,
+      "Twist controller needs does not match number of interfaces needed 6, given (%zu) interfaces",
+      command_interfaces_.size());
+    return controller_interface::return_type::ERROR;
+  }
+  command_interfaces_[0].set_value((*twist_commands)->twist.linear.x);
+  command_interfaces_[1].set_value((*twist_commands)->twist.linear.y);
+  command_interfaces_[2].set_value((*twist_commands)->twist.linear.z);
+  command_interfaces_[3].set_value((*twist_commands)->twist.angular.x);
+  command_interfaces_[4].set_value((*twist_commands)->twist.angular.y);
+  command_interfaces_[5].set_value((*twist_commands)->twist.angular.z);
+
+  return controller_interface::return_type::OK;
+}
 }  // namespace cartesian_controllers
+
 #include "pluginlib/class_list_macros.hpp"
 
 PLUGINLIB_EXPORT_CLASS(

cartesian_controllers/test/test_twist_controller.cpp

@@ -14,6 +14,7 @@
 
 #include "test_twist_controller.hpp"
 
+#include <lifecycle_msgs/msg/state.hpp>
 #include <memory>
 #include <string>
 #include <utility>
@@ -151,7 +152,7 @@ TEST_F(TwistControllerTest, command_callback_test)
     auto node_state = controller_->configure();
     ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
 
-    node_state = controller_->activate();
+    node_state = controller_->get_node()->activate();
     ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
 
     // send a new command