From 1987311ad599565733f2e5050649da90c34babca Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 27 Jul 2023 10:07:47 +0200 Subject: [PATCH] Set command buffers in test_controller when ALL interfaces are requested When only the controller_interface::InterfaceConfiguration.type == ALL is given, but the names vector is empty, the command buffer vector is created with size 0 However, when writing the commands in update(), values for all command_interfaces are being happily read from a vector with size 0, which is not very optimal. With this commit, we resize the command buffer on on_activate when ALL interfaces are requested, since at that point we know all available interfaces, as they are set by the controller_manager's switch() method. --- .../test/test_controller/test_controller.cpp | 14 ++++++++++++++ .../test/test_controller/test_controller.hpp | 3 +++ 2 files changed, 17 insertions(+) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 8c859d7826..1a9fbf1a79 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -94,6 +94,20 @@ CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*pr return CallbackReturn::SUCCESS; } +CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (cmd_iface_cfg_.type == controller_interface::interface_configuration_type::ALL) + { + external_commands_for_testing_.resize(command_interfaces_.size(), 0.0); + cmd_iface_cfg_.names.resize(command_interfaces_.size()); + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + cmd_iface_cfg_.names[i] = command_interfaces_[i].get_name(); + } + } + return CallbackReturn::SUCCESS; +} + CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) { if (simulate_cleanup_failure) diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index f73f592037..177f999705 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -55,6 +55,9 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CONTROLLER_MANAGER_PUBLIC CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;