Change dynamics_linearizing_controller to chainable controller.

diff --git a/include/linearizing_controllers/dynamics_linearizing_controller.hpp b/include/linearizing_controllers/dynamics_linearizing_controller.hpp
index e26c579..3b2320f 100644 (file)
--- a/include/linearizing_controllers/dynamics_linearizing_controller.hpp
+++ b/include/linearizing_controllers/dynamics_linearizing_controller.hpp
@@ -27,7 +27,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
 
-#include <controller_interface/controller_interface.hpp>
+#include <controller_interface/chainable_controller_interface.hpp>
 #include <realtime_tools/realtime_buffer.hpp>
 #include <realtime_tools/realtime_publisher.hpp>
 
@@ -40,7 +40,7 @@ namespace effort_controllers
 {
        using CallbackReturn=rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
-       class DynamicsLinearizingController: public controller_interface::ControllerInterface
+       class DynamicsLinearizingController: public controller_interface::ChainableControllerInterface
        {
                public:
                LINEARIZING_CONTROLLERS_PUBLIC
@@ -65,7 +65,16 @@ namespace effort_controllers
                CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
 
                LINEARIZING_CONTROLLERS_PUBLIC
-               controller_interface::return_type update(const rclcpp::Time &time,const rclcpp::Duration &period) override;
+               std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces(void) override;
+
+               LINEARIZING_CONTROLLERS_PUBLIC
+               bool on_set_chained_mode(bool chained_mode) override;
+
+               LINEARIZING_CONTROLLERS_PUBLIC
+               controller_interface::return_type update_reference_from_subscribers(const rclcpp::Time &time,const rclcpp::Duration &period) override;
+
+               LINEARIZING_CONTROLLERS_PUBLIC
+               controller_interface::return_type update_and_write_commands(const rclcpp::Time &time,const rclcpp::Duration &period) override;
 
                private:
                std::vector<std::string> joints_;

diff --git a/linearizing_controllers_plugins.xml b/linearizing_controllers_plugins.xml
index 44efe8b..3b56724 100644 (file)
--- a/linearizing_controllers_plugins.xml
+++ b/linearizing_controllers_plugins.xml
@@ -1,7 +1,7 @@
 <library path="linearizing_controllers">
   <class name="effort_controllers/DynamicsLinearizingController"
        type="effort_controllers::DynamicsLinearizingController"
-       base_class_type="controller_interface::ControllerInterface">
+       base_class_type="controller_interface::ChainableControllerInterface">
     <description>
       The DynamicsLinearizingController linearizes the dynamics of
       a differential-drive mobile robot.  The linearized inputs are linear

diff --git a/src/dynamics_linearizing_controller.cpp b/src/dynamics_linearizing_controller.cpp
index b98c117..32cf878 100644 (file)
--- a/src/dynamics_linearizing_controller.cpp
+++ b/src/dynamics_linearizing_controller.cpp
@@ -60,6 +60,9 @@ namespace effort_controllers
                        return CallbackReturn::ERROR;
                }
 
+               // reference_interfaces_ is declared in the base class
+               reference_interfaces_.resize(2);
+
                return CallbackReturn::SUCCESS;
        }
 
@@ -190,6 +193,30 @@ namespace effort_controllers
                return config;
        }
 
+       std::vector<hardware_interface::CommandInterface> DynamicsLinearizingController::on_export_reference_interfaces(void)
+       {
+               std::vector<hardware_interface::CommandInterface> interfaces;
+
+               std::vector<std::string> names;
+               names.push_back("linear/acceleration");
+               names.push_back("angular/acceleration");
+
+               reference_interfaces_.resize(names.size());
+
+               for(size_t i=0; i < names.size();i++)
+               {
+                       // reference_interfaces_ is declared in the base class
+                       interfaces.push_back(hardware_interface::CommandInterface(std::string(get_node()->get_name()),names[i],&reference_interfaces_[i]));
+               }
+
+               return interfaces;
+       }
+
+       bool DynamicsLinearizingController::on_set_chained_mode(bool /*chained_mode*/)
+       {
+               return true;
+       }
+
        CallbackReturn DynamicsLinearizingController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
        {
                Eigen::Vector3d x;
@@ -219,7 +246,15 @@ namespace effort_controllers
                return CallbackReturn::SUCCESS;
        }
 
-       controller_interface::return_type DynamicsLinearizingController::update(const rclcpp::Time & /*time*/,const rclcpp::Duration & /*period*/)
+       controller_interface::return_type DynamicsLinearizingController::update_reference_from_subscribers(const rclcpp::Time & /*time*/,const rclcpp::Duration &/*period*/)
+       {
+               // Move functionality to the `update_and_write_commands` because of the missing arguments in
+               // humble - otherwise issues with multiple time-sources might happen when working with simulators
+
+               return controller_interface::return_type::OK;
+       }
+
+       controller_interface::return_type DynamicsLinearizingController::update_and_write_commands(const rclcpp::Time & /*time*/,const rclcpp::Duration &/*period*/)
        {
                auto time=get_node()->get_clock()->now();
                auto dt=time-lastSamplingTime_;
@@ -241,11 +276,22 @@ namespace effort_controllers
                Eigen::Vector2d u;
                odom_.getVelocity(u);
 
-               auto command=rt_command_.readFromRT();
-               if(command && *command)
+               if(is_in_chained_mode())
                {
-                       v_[0]=(*command)->linear.x;
-                       v_[1]=(*command)->angular.z;
+                       if(!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]))
+                       {
+                               v_[0]=reference_interfaces_[0];
+                               v_[1]=reference_interfaces_[1];
+                       }
+               }
+               else
+               {
+                       auto command=rt_command_.readFromRT();
+                       if(command && *command)
+                       {
+                               v_[0]=(*command)->linear.x;
+                               v_[1]=(*command)->angular.z;
+                       }
                }
 
                auto param=rt_param_.readFromRT();
@@ -344,4 +390,4 @@ namespace effort_controllers
 }
 
 PLUGINLIB_EXPORT_CLASS(effort_controllers::DynamicsLinearizingController,
-        controller_interface::ControllerInterface)
+        controller_interface::ChainableControllerInterface)