Add queue_size to the forward command controller

forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp

@@ -86,6 +86,8 @@ class ForwardControllersBase : public controller_interface::ControllerInterface
 
   std::vector<std::string> command_interface_types_;
 
+  size_t history_depth_ = 1u;
+
   // the realtime container to exchange the reference from subscriber
   realtime_tools::RealtimeThreadSafeBox<CmdType> rt_command_;
   // save the last reference in case of unable to get value from box

forward_command_controller/src/forward_command_controller.cpp

@@ -51,6 +51,8 @@ controller_interface::CallbackReturn ForwardCommandController::read_parameters()
     command_interface_types_.push_back(joint + "/" + params_.interface_name);
   }
 
+  history_depth_ = static_cast<size_t>(params_.queue_size);
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 

forward_command_controller/src/forward_command_controller_parameters.yaml

@@ -15,3 +15,12 @@ forward_command_controller:
     },
     read_only: true
   }
+  queue_size: {
+    type: int,
+    description: "Size of the command queue",
+    default_value: 1,
+    read_only: true,
+    validation: {
+      gt_eq<>: 1
+    },
+  }

forward_command_controller/src/forward_controllers_base.cpp

@@ -70,8 +70,9 @@ controller_interface::CallbackReturn ForwardControllersBase::on_configure(
     return ret;
   }
 
+  auto qos = rclcpp::SystemDefaultsQoS().keep_last(history_depth_);
   joints_command_subscriber_ = get_node()->create_subscription<CmdType>(
-    "~/commands", rclcpp::SystemDefaultsQoS(),
+    "~/commands", qos,
     [this](const CmdType::SharedPtr msg)
     {
       const auto cmd = *msg;

forward_command_controller/src/multi_interface_forward_command_controller.cpp

@@ -51,6 +51,8 @@ controller_interface::CallbackReturn MultiInterfaceForwardCommandController::rea
     command_interface_types_.push_back(params_.joint + "/" + interface);
   }
 
+  history_depth_ = static_cast<size_t>(params_.queue_size);
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 

forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml

@@ -15,3 +15,12 @@ multi_interface_forward_command_controller:
     },
     read_only: true
   }
+  queue_size: {
+    type: int,
+    description: "Size of the command queue",
+    default_value: 1,
+    read_only: true,
+    validation: {
+      gt_eq<>: 1
+    },
+  }