From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Tue, 3 Jun 2025 08:36:46 +0000 (-0300)
Subject: Change dynamics_linearizing_controller to chainable controller.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=cb6ac8814ce07672faac702f7b03dff172ced465;p=linearizing_controllers.git

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
--- 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
--- 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
--- 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)