From eb4316b1dc65d9a93a2463b4f2b22f3510f0ab1b Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Thu, 15 Aug 2024 14:34:02 +0200 Subject: [PATCH 01/18] Preparation of Handles for Variant Support (#1678) --- .../include/hardware_interface/handle.hpp | 80 ++++++++++--------- .../include/transmission_interface/handle.hpp | 8 +- 2 files changed, 45 insertions(+), 43 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index dc536e51be..bfe0ff1eb8 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -16,41 +16,45 @@ #define HARDWARE_INTERFACE__HANDLE_HPP_ #include +#include #include "hardware_interface/macros.hpp" namespace hardware_interface { + +using HANDLE_DATATYPE = std::variant; + /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( + Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } - explicit ReadOnlyHandle(const std::string & interface_name) + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - explicit ReadOnlyHandle(const char * interface_name) + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + Handle(const Handle & other) = default; - ReadOnlyHandle(ReadOnlyHandle && other) = default; + Handle(Handle && other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + Handle & operator=(const Handle & other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + Handle & operator=(Handle && other) = default; - virtual ~ReadOnlyHandle() = default; + virtual ~Handle() = default; /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } @@ -70,60 +74,58 @@ class ReadOnlyHandle double get_value() const { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; + // END + } + + void set_value(double value) + { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value_ directly if old functionality is removed + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; + // END } protected: std::string prefix_name_; std::string interface_name_; + HANDLE_DATATYPE value_; + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; + // END }; -class ReadWriteHandle : public ReadOnlyHandle +class StateInterface : public Handle { public: - ReadWriteHandle( + explicit StateInterface( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) - { - } - - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} - - ReadWriteHandle(const ReadWriteHandle & other) = default; - - ReadWriteHandle(ReadWriteHandle && other) = default; - - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; - - ReadWriteHandle & operator=(ReadWriteHandle && other) = default; - - virtual ~ReadWriteHandle() = default; - - void set_value(double value) + : Handle(prefix_name, interface_name, value_ptr) { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; } -}; -class StateInterface : public ReadOnlyHandle -{ -public: StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using Handle::Handle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public Handle { public: + explicit CommandInterface( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : Handle(prefix_name, interface_name, value_ptr) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -134,7 +136,7 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using Handle::Handle; }; } // namespace hardware_interface diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index 024a019bfd..4c40189648 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -20,17 +20,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; } // namespace transmission_interface From 80c264f024c06079707378e0bbaf0f361a1dff6c Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 16 Aug 2024 11:17:14 +0200 Subject: [PATCH 02/18] Robustify controller spawner and add integration test with many controllers (#1501) --------- Co-authored-by: Dr. Denis --- .../controller_manager_services.py | 54 ++++++++++++++++--- .../test/test_spawner_unspawner.cpp | 26 +++++++++ 2 files changed, 73 insertions(+), 7 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index be11a4b782..3f5a820fae 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -32,7 +32,39 @@ class ServiceNotFoundError(Exception): pass -def service_caller(node, service_name, service_type, request, service_timeout=0.0): +def service_caller( + node, + service_name, + service_type, + request, + service_timeout=0.0, + call_timeout=10.0, + max_attempts=3, +): + """ + Abstraction of a service call. + + Has an optional timeout to find the service, receive the answer to a call + and a mechanism to retry a call of no response is received. + + @param node Node object to be associated with + @type rclpy.node.Node + @param service_name Service URL + @type str + @param request The request to be sent + @type service request type + @param service_timeout Timeout (in seconds) to wait until the service is available. 0 means + waiting forever, retrying every 10 seconds. + @type float + @param call_timeout Timeout (in seconds) for getting a response + @type float + @param max_attempts Number of attempts until a valid response is received. With some + middlewares it can happen, that the service response doesn't reach the client leaving it in + a waiting state forever. + @type int + @return The service response + + """ cli = node.create_client(service_type, service_name) while not cli.service_is_ready(): @@ -44,12 +76,20 @@ def service_caller(node, service_name, service_type, request, service_timeout=0. node.get_logger().warn(f"Could not contact service {service_name}") node.get_logger().debug(f"requester: making request: {request}\n") - future = cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - if future.result() is not None: - return future.result() - else: - raise RuntimeError(f"Exception while calling service: {future.exception()}") + future = None + for attempt in range(max_attempts): + future = cli.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout) + if future.result() is None: + node.get_logger().warning( + f"Failed getting a result from calling {service_name} in " + f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" + ) + else: + return future.result() + raise RuntimeError( + f"Could not successfully call service {service_name} after {max_attempts} attempts." + ) def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 3edf770436..db3940dbcc 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -374,6 +374,32 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) } } +TEST_F(TestLoadController, spawner_with_many_controllers) +{ + std::stringstream ss; + const size_t num_controllers = 50; + const std::string controller_base_name = "ctrl_"; + for (size_t i = 0; i < num_controllers; i++) + { + const std::string controller_name = controller_base_name + std::to_string(static_cast(i)); + cm_->set_parameter( + rclcpp::Parameter(controller_name + ".type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + ss << controller_name << " "; + } + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner(ss.str() + " -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), num_controllers); + + for (size_t i = 0; i < num_controllers; i++) + { + auto ctrl = cm_->get_loaded_controllers()[i]; + ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture { From 0631e3edecadd8b4a50ad2a3dc1374030167f71a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 12:21:41 +0200 Subject: [PATCH 03/18] Refactor spawner to be able to reuse code for ros2controlcli (#1661) --- .../controller_manager/__init__.py | 8 + .../controller_manager_services.py | 117 +++++++++++++ .../controller_manager/hardware_spawner.py | 14 +- .../controller_manager/spawner.py | 165 +----------------- .../test/test_spawner_unspawner.cpp | 13 +- .../ros2controlcli/verb/list_controllers.py | 3 +- .../verb/list_hardware_components.py | 3 +- .../verb/list_hardware_interfaces.py | 3 +- 8 files changed, 141 insertions(+), 185 deletions(-) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index f49bed4d34..4a8d7daee5 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -23,6 +23,10 @@ set_hardware_component_state, switch_controllers, unload_controller, + get_parameter_from_param_file, + set_controller_parameters, + set_controller_parameters_from_param_file, + bcolors, ) __all__ = [ @@ -36,4 +40,8 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", + "get_parameter_from_param_file", + "set_controller_parameters", + "set_controller_parameters_from_param_file", + "bcolors", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 3f5a820fae..7b0f726d68 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -26,6 +26,29 @@ ) import rclpy +import yaml +from rcl_interfaces.msg import Parameter + +# @note: The versions conditioning is added here to support the source-compatibility with Humble +# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0 +try: + from rclpy.parameter import get_parameter_value +except ImportError: + from ros2param.api import get_parameter_value +from ros2param.api import call_set_parameters + + +# from https://stackoverflow.com/a/287944 +class bcolors: + MAGENTA = "\033[95m" + OKBLUE = "\033[94m" + OKCYAN = "\033[96m" + OKGREEN = "\033[92m" + WARNING = "\033[93m" + FAIL = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + UNDERLINE = "\033[4m" class ServiceNotFoundError(Exception): @@ -220,3 +243,97 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti request, service_timeout, ) + + +def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): + with open(parameter_file) as f: + namespaced_controller = ( + controller_name if namespace == "/" else f"{namespace}/{controller_name}" + ) + parameters = yaml.safe_load(f) + if namespaced_controller in parameters: + value = parameters[namespaced_controller] + if not isinstance(value, dict) or "ros__parameters" not in value: + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" + ) + if parameter_name in parameters[namespaced_controller]["ros__parameters"]: + return parameters[namespaced_controller]["ros__parameters"][parameter_name] + else: + return None + else: + return None + + +def set_controller_parameters( + node, controller_manager_name, controller_name, parameter_name, parameter_value +): + parameter = Parameter() + parameter.name = controller_name + "." + parameter_name + parameter_string = str(parameter_value) + parameter.value = get_parameter_value(string_value=parameter_string) + + response = call_set_parameters( + node=node, node_name=controller_manager_name, parameters=[parameter] + ) + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + node.get_logger().info( + bcolors.OKCYAN + + 'Setting controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) + return False + return True + + +def set_controller_parameters_from_param_file( + node, controller_manager_name, controller_name, parameter_file, namespace=None +): + if parameter_file: + spawner_namespace = namespace if namespace else node.get_namespace() + set_controller_parameters( + node, controller_manager_name, controller_name, "param_file", parameter_file + ) + + controller_type = get_parameter_from_param_file( + controller_name, spawner_namespace, parameter_file, "type" + ) + if controller_type: + if not set_controller_parameters( + node, controller_manager_name, controller_name, "type", controller_type + ): + return False + + fallback_controllers = get_parameter_from_param_file( + controller_name, spawner_namespace, parameter_file, "fallback_controllers" + ) + if fallback_controllers: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "fallback_controllers", + fallback_controllers, + ): + return False + return True diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 3e3a487c6a..29c0b5e97c 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -19,6 +19,7 @@ from controller_manager import ( list_hardware_components, set_hardware_component_state, + bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError @@ -28,19 +29,6 @@ from rclpy.signals import SignalHandlerOptions -# from https://stackoverflow.com/a/287944 -class bcolors: - HEADER = "\033[95m" - OKBLUE = "\033[94m" - OKCYAN = "\033[96m" - OKGREEN = "\033[92m" - WARNING = "\033[93m" - FAIL = "\033[91m" - ENDC = "\033[0m" - BOLD = "\033[1m" - UNDERLINE = "\033[4m" - - def first_match(iterable, predicate): return next((n for n in iterable if predicate(n)), None) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 0e83eb9d07..bd277d5464 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,7 +19,6 @@ import sys import time import warnings -import yaml from controller_manager import ( configure_controller, @@ -27,35 +26,14 @@ load_controller, switch_controllers, unload_controller, + set_controller_parameters_from_param_file, + bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy -from rcl_interfaces.msg import Parameter from rclpy.node import Node - -# @note: The versions conditioning is added here to support the source-compatibility with Humble -# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0 -try: - from rclpy.parameter import get_parameter_value -except ImportError: - from ros2param.api import get_parameter_value from rclpy.signals import SignalHandlerOptions -from ros2param.api import call_set_parameters - -# from https://stackoverflow.com/a/287944 - - -class bcolors: - MAGENTA = "\033[95m" - OKBLUE = "\033[94m" - OKCYAN = "\033[96m" - OKGREEN = "\033[92m" - WARNING = "\033[93m" - FAIL = "\033[91m" - ENDC = "\033[0m" - BOLD = "\033[1m" - UNDERLINE = "\033[4m" def first_match(iterable, predicate): @@ -87,24 +65,6 @@ def is_controller_loaded(node, controller_manager, controller_name, service_time return any(c.name == controller_name for c in controllers) -def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): - with open(parameter_file) as f: - namespaced_controller = ( - controller_name if namespace == "/" else f"{namespace}/{controller_name}" - ) - parameters = yaml.safe_load(f) - if namespaced_controller in parameters: - value = parameters[namespaced_controller] - if not isinstance(value, dict) or "ros__parameters" not in value: - raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" - ) - if parameter_name in parameters[namespaced_controller]["ros__parameters"]: - return parameters[namespaced_controller]["ros__parameters"][parameter_name] - else: - return None - - def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() @@ -162,14 +122,6 @@ def main(args=None): action="store_true", required=False, ) - parser.add_argument( - "--fallback_controllers", - help="Fallback controllers list are activated as a fallback strategy when the" - " spawned controllers fail. When the argument is provided, it takes precedence over" - " the fallback_controllers list in the param file", - default=None, - nargs="+", - ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -210,7 +162,6 @@ def main(args=None): try: for controller_name in controller_names: - fallback_controllers = args.fallback_controllers if is_controller_loaded( node, controller_manager_name, controller_name, controller_manager_timeout @@ -221,112 +172,14 @@ def main(args=None): + bcolors.ENDC ) else: - controller_type = ( - None - if param_file is None - else get_parameter_from_param_file( - controller_name, spawner_namespace, param_file, "type" - ) - ) - if controller_type: - parameter = Parameter() - parameter.name = controller_name + ".type" - parameter.value = get_parameter_value(string_value=controller_type) - - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter] - ) - assert len(response.results) == 1 - result = response.results[0] - if result.successful: - node.get_logger().info( - bcolors.OKCYAN - + 'Set controller type to "' - + controller_type - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller type to "' - + controller_type - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - return 1 - if param_file: - parameter = Parameter() - parameter.name = controller_name + ".params_file" - parameter.value = get_parameter_value(string_value=param_file) - - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter] - ) - assert len(response.results) == 1 - result = response.results[0] - if result.successful: - node.get_logger().info( - bcolors.OKCYAN - + 'Set controller params file to "' - + param_file - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller params file to "' - + param_file - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - return 1 - - if not fallback_controllers and param_file: - fallback_controllers = get_parameter_from_param_file( - controller_name, spawner_namespace, param_file, "fallback_controllers" - ) - - if fallback_controllers: - parameter = Parameter() - parameter.name = controller_name + ".fallback_controllers" - parameter.value = get_parameter_value(string_value=str(fallback_controllers)) - - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter] - ) - assert len(response.results) == 1 - result = response.results[0] - if result.successful: - node.get_logger().info( - bcolors.OKCYAN - + 'Setting fallback_controllers to ["' - + ",".join(fallback_controllers) - + '"] for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set fallback_controllers to ["' - + ",".join(fallback_controllers) - + '"] for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) + if not set_controller_parameters_from_param_file( + node, + controller_manager_name, + controller_name, + param_file, + spawner_namespace, + ): return 1 ret = load_controller(node, controller_manager_name, controller_name) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index db3940dbcc..852ea90bc7 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -329,20 +329,14 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ( - call_spawner( - "ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 " - "-p " + - test_file_path), - 0); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --load-only -p " + test_file_path), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); { auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } @@ -355,8 +349,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index a26a1168a1..3daf752b8c 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_controllers -from controller_manager.spawner import bcolors +from controller_manager import list_controllers, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index e2b427b233..e3e2269920 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_hardware_components -from controller_manager.spawner import bcolors +from controller_manager import list_hardware_components, bcolors from lifecycle_msgs.msg import State diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 7aa850f3bc..4510998ad9 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_hardware_interfaces -from controller_manager.spawner import bcolors +from controller_manager import list_hardware_interfaces, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy From 079392b94867c3372c0050492327001057118090 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 12:23:33 +0200 Subject: [PATCH 04/18] Fix spawner tests timeout (#1692) --- controller_manager/test/test_spawner_unspawner.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 852ea90bc7..49326f9e79 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -255,6 +255,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; + ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file EXPECT_EQ( call_spawner( @@ -306,6 +307,7 @@ TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill // timeout command will kill it after the specified time with signal SIGINT + ControllerManagerRunner cm_runner(this); cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); std::stringstream ss; ss << "timeout --signal=INT 5 " @@ -622,6 +624,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; + ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file EXPECT_EQ( call_spawner( @@ -682,6 +685,7 @@ TEST_F( const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; + ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file EXPECT_EQ( call_spawner( From 4498d25ff7cfdc40780a34b36cf4d1fea40f7521 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 13:40:45 +0200 Subject: [PATCH 05/18] Avoid using the global arguments for internal controller nodes (#1694) --- .../controller_interface_base.hpp | 5 +++-- .../test/test_controller_interface.cpp | 13 +++++++++---- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 9eaee9f15b..e881f08aae 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -174,11 +174,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy { // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCLCPP_VERSION_MAJOR >= 21 - return rclcpp::NodeOptions().enable_logger_service(true); + return rclcpp::NodeOptions().enable_logger_service(true).use_global_arguments(false); #else return rclcpp::NodeOptions() .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true); + .automatically_declare_parameters_from_overrides(true) + .use_global_arguments(false); #endif } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 03838b1a2e..f5c0a6b3de 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -16,6 +16,8 @@ #include #include +#include +#include #include "rclcpp/executor_options.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -57,13 +59,16 @@ TEST(TestableControllerInterface, init) TEST(TestableControllerInterface, setting_update_rate_in_configure) { // mocks the declaration of overrides parameters in a yaml file - char const * const argv[] = {"", "--ros-args", "-p", "update_rate:=2812"}; - int argc = arrlen(argv); - rclcpp::init(argc, argv); + rclcpp::init(0, nullptr); TestableControllerInterface controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); + auto node_options = controller.define_custom_node_options(); + std::vector node_options_arguments = node_options.arguments(); + node_options_arguments.push_back("--ros-args"); + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("update_rate:=2812"); + node_options = node_options.arguments(node_options_arguments); ASSERT_EQ( controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options), controller_interface::return_type::OK); From fb1d254cc73c4dd407d69d6caa813d290d8a2fa9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 21 Aug 2024 17:40:57 +0200 Subject: [PATCH 06/18] Fix params_file typo in spawner and update release notes for use_global_arguments (#1701) --- .../controller_interface/controller_interface_base.hpp | 5 ++--- .../controller_manager/controller_manager_services.py | 2 +- controller_manager/src/controller_manager.cpp | 1 + doc/migration.rst | 4 ++++ doc/release_notes.rst | 2 ++ 5 files changed, 10 insertions(+), 4 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index e881f08aae..9eaee9f15b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -174,12 +174,11 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy { // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCLCPP_VERSION_MAJOR >= 21 - return rclcpp::NodeOptions().enable_logger_service(true).use_global_arguments(false); + return rclcpp::NodeOptions().enable_logger_service(true); #else return rclcpp::NodeOptions() .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true) - .use_global_arguments(false); + .automatically_declare_parameters_from_overrides(true); #endif } diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 7b0f726d68..a09be5d14b 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -312,7 +312,7 @@ def set_controller_parameters_from_param_file( if parameter_file: spawner_namespace = namespace if namespace else node.get_namespace() set_controller_parameters( - node, controller_manager_name, controller_name, "param_file", parameter_file + node, controller_manager_name, controller_name, "params_file", parameter_file ) controller_type = get_parameter_from_param_file( diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 541fc57d02..734596f8bd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2843,6 +2843,7 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( } controller_node_options = controller_node_options.arguments(node_options_arguments); + controller_node_options.use_global_arguments(false); return controller_node_options; } diff --git a/doc/migration.rst b/doc/migration.rst index 68359f072f..f24f338e52 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -2,6 +2,10 @@ Iron to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +controller_interface +******************** +* The changes from `(PR #1694) `__ will affect how the controllers will be loading the parameters. Defining parameters in a single yaml file and loading it to the controller_manager node alone will no longer work. + In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from NodeOptions to false, to avoid getting influenced by global arguments. controller_manager ****************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index caa28ca9d0..fa393d72ff 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,6 +22,8 @@ For details see the controller_manager section. * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. +* The controllers will now set ``use_global_arguments`` from NodeOptions to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). + From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. controller_manager ****************** From 40ea19176d47c63e9e873b7744cfad8eef366b41 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 21 Aug 2024 17:41:57 +0200 Subject: [PATCH 07/18] Introduce Creation of Handles with InterfaceDescription (variant support) (#1679) --- .../hardware_interface/component_parser.hpp | 18 +++ .../include/hardware_interface/handle.hpp | 30 +++-- .../hardware_interface/hardware_info.hpp | 39 ++++++- hardware_interface/src/component_parser.cpp | 34 ++++++ .../test/test_component_parser.cpp | 110 ++++++++++++++++++ hardware_interface/test/test_handle.cpp | 31 +++++ 6 files changed, 252 insertions(+), 10 deletions(-) diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 38ca0cf89d..2d0c067606 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -32,5 +32,23 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's StateInterfaces for the component + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_state_interface_descriptions( + const std::vector & component_info); + +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's CommandInterfaces for the component + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_command_interface_descriptions( + const std::vector & component_info); + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index bfe0ff1eb8..6f919033de 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,9 +15,11 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" namespace hardware_interface @@ -29,6 +31,8 @@ using HANDLE_DATATYPE = std::variant; class Handle { public: + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) @@ -36,11 +40,25 @@ class Handle { } + explicit Handle(const InterfaceDescription & interface_description) + : prefix_name_(interface_description.prefix_name), + interface_name_(interface_description.interface_info.name) + { + // As soon as multiple datatypes are used in HANDLE_DATATYPE + // we need to initialize according the type passed in interface description + value_ = std::numeric_limits::quiet_NaN(); + value_ptr_ = std::get_if(&value_); + } + + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { @@ -103,10 +121,8 @@ class Handle class StateInterface : public Handle { public: - explicit StateInterface( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : Handle(prefix_name, interface_name, value_ptr) + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } @@ -120,10 +136,8 @@ class StateInterface : public Handle class CommandInterface : public Handle { public: - explicit CommandInterface( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : Handle(prefix_name, interface_name, value_ptr) + explicit CommandInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } /// CommandInterface copy constructor is actively deleted. diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 2bd2099e69..b4e47c610e 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -40,9 +40,9 @@ struct InterfaceInfo std::string max; /// (Optional) Initial value of the interface. std::string initial_value; - /// (Optional) The datatype of the interface, e.g. "bool", "int". Used by GPIOs. + /// (Optional) The datatype of the interface, e.g. "bool", "int". std::string data_type; - /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. + /// (Optional) If the handle is an array, the size of the array. int size; /// (Optional) enable or disable the limits for the command interfaces bool enable_limits; @@ -126,6 +126,41 @@ struct TransmissionInfo std::unordered_map parameters; }; +/** + * This structure stores information about an interface for a specific hardware which should be + * instantiated internally. + */ +struct InterfaceDescription +{ + InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) + : prefix_name(prefix_name_in), + interface_info(interface_info_in), + interface_name(prefix_name + "/" + interface_info.name) + { + } + + /** + * Name of the interface defined by the user. + */ + std::string prefix_name; + + /** + * Information about the Interface type (position, velocity,...) as well as limits and so on. + */ + InterfaceInfo interface_info; + + /** + * Name of the interface + */ + std::string interface_name; + + std::string get_prefix_name() const { return prefix_name; } + + std::string get_interface_name() const { return interface_info.name; } + + std::string get_name() const { return interface_name; } +}; + /// This structure stores information about hardware defined in a robot's URDF. struct HardwareInfo { diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 42432dda8d..0ef6c084f9 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -905,4 +905,38 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } +std::vector parse_state_interface_descriptions( + const std::vector & component_info) +{ + std::vector component_state_interface_descriptions; + component_state_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & state_interface : component.state_interfaces) + { + component_state_interface_descriptions.emplace_back( + InterfaceDescription(component.name, state_interface)); + } + } + return component_state_interface_descriptions; +} + +std::vector parse_command_interface_descriptions( + const std::vector & component_info) +{ + std::vector component_command_interface_descriptions; + component_command_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & command_interface : component.command_interfaces) + { + component_command_interface_descriptions.emplace_back( + InterfaceDescription(component.name, command_interface)); + } + } + return component_command_interface_descriptions; +} + } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index be891787f3..6a78b0807d 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1404,3 +1404,113 @@ TEST_F(TestComponentParser, urdf_incomplete_throws_error) std::string(ros2_control_test_assets::urdf_tail); ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } + +TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_state_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_state_descriptions[0].get_interface_name(), "position"); + EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); + + EXPECT_EQ(joint_state_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_state_descriptions[1].get_interface_name(), "position"); + EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); +} + +TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_command_descriptions = + parse_command_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_command_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_command_descriptions[0].get_interface_name(), "position"); + EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); + + EXPECT_EQ(joint_command_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_command_descriptions[1].get_interface_name(), "position"); + EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1"); +} + +TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_sensor_only + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto sensor_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].sensors); + EXPECT_EQ(sensor_state_descriptions[0].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].get_interface_name(), "roll"); + EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); + EXPECT_EQ(sensor_state_descriptions[1].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].get_interface_name(), "pitch"); + EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); + EXPECT_EQ(sensor_state_descriptions[2].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].get_interface_name(), "yaw"); + EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); + + EXPECT_EQ(sensor_state_descriptions[3].get_prefix_name(), "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].get_interface_name(), "image"); + EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); +} + +TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "analog_input1"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); + EXPECT_EQ(gpio_state_descriptions[2].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].get_interface_name(), "analog_input2"); + EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); + + EXPECT_EQ(gpio_state_descriptions[3].get_prefix_name(), "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_interface_name(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); +} + +TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_command_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); +} diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index da8258c643..7d79c032f0 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -14,8 +14,11 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" using hardware_interface::CommandInterface; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using hardware_interface::StateInterface; namespace @@ -64,3 +67,31 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } + +TEST(TestHandle, interface_description_state_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + StateInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} + +TEST(TestHandle, interface_description_command_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + CommandInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} From ea215615c57afea054dc5b97e5287993f3dbaf6c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 22 Aug 2024 10:28:36 +0200 Subject: [PATCH 08/18] Add `use_global_arguments` NodeOptions documentation reference (#1707) --- doc/migration.rst | 2 +- doc/release_notes.rst | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/migration.rst b/doc/migration.rst index f24f338e52..8e1ca096f1 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -5,7 +5,7 @@ Iron to Jazzy controller_interface ******************** * The changes from `(PR #1694) `__ will affect how the controllers will be loading the parameters. Defining parameters in a single yaml file and loading it to the controller_manager node alone will no longer work. - In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from NodeOptions to false, to avoid getting influenced by global arguments. + In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments. controller_manager ****************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index fa393d72ff..4aaa828091 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,7 +22,7 @@ For details see the controller_manager section. * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. -* The controllers will now set ``use_global_arguments`` from NodeOptions to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). +* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. controller_manager From 0a607fe9a86d558f060a5ac1678878f90af78c16 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 22 Aug 2024 11:02:25 +0200 Subject: [PATCH 09/18] Use handle_name_ variable instead of allocating for every `get_name` call (#1706) --- .../include/hardware_interface/handle.hpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 6f919033de..07ba1cd98e 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -36,13 +36,17 @@ class Handle Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) - : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) + : prefix_name_(prefix_name), + interface_name_(interface_name), + handle_name_(prefix_name_ + "/" + interface_name_), + value_ptr_(value_ptr) { } explicit Handle(const InterfaceDescription & interface_description) : prefix_name_(interface_description.prefix_name), - interface_name_(interface_description.interface_info.name) + interface_name_(interface_description.interface_info.name), + handle_name_(prefix_name_ + "/" + interface_name_) { // As soon as multiple datatypes are used in HANDLE_DATATYPE // we need to initialize according the type passed in interface description @@ -53,14 +57,14 @@ class Handle [[deprecated("Use InterfaceDescription for initializing the Interface")]] explicit Handle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr) { } [[deprecated("Use InterfaceDescription for initializing the Interface")]] explicit Handle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr) { } @@ -77,12 +81,12 @@ class Handle /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } - const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } + const std::string & get_name() const { return handle_name_; } const std::string & get_interface_name() const { return interface_name_; } [[deprecated( - "Replaced by get_name method, which is semantically more correct")]] const std::string + "Replaced by get_name method, which is semantically more correct")]] const std::string & get_full_name() const { return get_name(); @@ -111,6 +115,7 @@ class Handle protected: std::string prefix_name_; std::string interface_name_; + std::string handle_name_; HANDLE_DATATYPE value_; // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed From dd352eb45a9a983116f61dc1a5fc650003844cf3 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Thu, 22 Aug 2024 11:05:06 +0200 Subject: [PATCH 10/18] inform user what reason is for not setting rt policy, inform is policy (#1705) --- controller_manager/src/ros2_control_node.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 44c9c37aed..8afc81877f 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -51,10 +52,16 @@ int main(int argc, char ** argv) { RCLCPP_WARN( cm->get_logger(), - "Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT " - "scheduling. See " + "Could not enable FIFO RT scheduling policy: with error number <%i>(%s). See " "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] " - "for details."); + "for details on how to enable realtime scheduling.", + errno, strerror(errno)); + } + else + { + RCLCPP_INFO( + cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.", + kSchedPriority); } // for calculating sleep time From c619b8a1755514db8a1a2074d9ff7b06026c3648 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 22 Aug 2024 19:41:52 +0100 Subject: [PATCH 11/18] Update changelogs --- controller_interface/CHANGELOG.rst | 6 ++++++ controller_manager/CHANGELOG.rst | 15 +++++++++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 8 ++++++++ hardware_interface_testing/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 6 ++++++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 6 ++++++ 11 files changed, 59 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 7b2169ed00..f7f5c5271b 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) +* Avoid using the global arguments for internal controller nodes (`#1694 `_) +* Contributors: Sai Kishor Kothakota + 4.15.0 (2024-08-05) ------------------- diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 46d3663362..a8cccd3968 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* inform user what reason is for not setting rt policy, inform is policy (`#1705 `_) +* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) +* Fix spawner tests timeout (`#1692 `_) +* Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) +* Robustify controller spawner and add integration test with many controllers (`#1501 `_) +* Handle waiting in Spawner and align Hardware Spawner functionality (`#1562 `_) +* Make list controller and list hardware components immediately visualize the state. (`#1606 `_) +* [CI] Add coveragepy and remove ignore: test (`#1668 `_) +* Fix spawner unload on kill test (`#1675 `_) +* [CM] Add more logging for easier debugging (`#1645 `_) +* refactor SwitchParams to fix the incosistencies in the spawner tests (`#1638 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Dr. Denis, Felix Exner (fexner), Manuel Muth, Sai Kishor Kothakota + 4.15.0 (2024-08-05) ------------------- * Add missing include for executors (`#1653 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index c8c11b580d..129f12fdef 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index bb0b478e4b..63c5a8ccb2 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use handle_name\_ variable instead of allocating for every `get_name` call (`#1706 `_) +* Introduce Creation of Handles with InterfaceDescription (variant support) (`#1679 `_) +* Preparation of Handles for Variant Support (`#1678 `_) +* [RM] Decouple read/write cycles of each component with mutex to not block other components (`#1646 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + 4.15.0 (2024-08-05) ------------------- * [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 3a60a2f534..9a8c46c82b 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- * [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 8581d26f72..4a0c434018 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 5106f98c38..2a67c358d7 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index f7e7d771b2..b84fd787ee 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 265084e74e..39b8db6c85 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) +* Make list controller and list hardware components immediately visualize the state. (`#1606 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + 4.15.0 (2024-08-05) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 30a674acc5..94626a2918 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 157a8ac09a..d6de7ed521 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Preparation of Handles for Variant Support (`#1678 `_) +* Fix flaky transmission_interface tests by making them deterministic. (`#1665 `_) +* Contributors: Manuel Muth, sgmurray + 4.15.0 (2024-08-05) ------------------- From ecfec159921bc646cf6350eab4e8bdc7bb48b6ed Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 22 Aug 2024 19:42:18 +0100 Subject: [PATCH 12/18] 4.16.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index f7f5c5271b..8a279ace35 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) * Avoid using the global arguments for internal controller nodes (`#1694 `_) * Contributors: Sai Kishor Kothakota diff --git a/controller_interface/package.xml b/controller_interface/package.xml index a60e5e90b6..951811ba96 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.15.0 + 4.16.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index a8cccd3968..138e7b5738 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * inform user what reason is for not setting rt policy, inform is policy (`#1705 `_) * Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) * Fix spawner tests timeout (`#1692 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 9f9a9bf796..981fbd919c 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.15.0 + 4.16.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 129f12fdef..054243f0f8 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index b6a57c2636..5dc5d69437 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.15.0 + 4.16.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 63c5a8ccb2..71920c6f89 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * Use handle_name\_ variable instead of allocating for every `get_name` call (`#1706 `_) * Introduce Creation of Handles with InterfaceDescription (variant support) (`#1679 `_) * Preparation of Handles for Variant Support (`#1678 `_) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 77d084ae7a..2d52a3327a 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.15.0 + 4.16.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 9a8c46c82b..9b1cb05d00 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 646923b4e6..13afe926a5 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.15.0 + 4.16.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 4a0c434018..21f0d95a24 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 1ff460cf14..d9e18baa1d 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.15.0 + 4.16.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 2a67c358d7..b7c06c1964 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index d5ef3ba862..d9ade902ca 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.15.0 + 4.16.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index b84fd787ee..918f5f3502 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 7be61a2f91..4bcf6cd1e4 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.15.0 + 4.16.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 39b8db6c85..81d2fe97b5 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) * Make list controller and list hardware components immediately visualize the state. (`#1606 `_) * Contributors: Dr. Denis, Sai Kishor Kothakota diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index f7ed71dae9..c11b4cf94f 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.15.0 + 4.16.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index d1c8e2d150..6b8a169fd6 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.15.0", + version="4.16.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 94626a2918..0593af2778 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index fc3de21897..7b39a442c4 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.15.0 + 4.16.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index fad594c205..da798c0e6b 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.15.0", + version="4.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index d6de7ed521..46f665cdb5 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * Preparation of Handles for Variant Support (`#1678 `_) * Fix flaky transmission_interface tests by making them deterministic. (`#1665 `_) * Contributors: Manuel Muth, sgmurray diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 9ee959c5bc..44d479a62c 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.15.0 + 4.16.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 08492a161b581b720a7b123a24ff74aef09dd370 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 23 Aug 2024 20:44:50 +0200 Subject: [PATCH 13/18] propage a portion of global args to the controller nodes (#1712) --- .../controller_manager/controller_manager.hpp | 1 + controller_manager/src/controller_manager.cpp | 51 +++++++++++++++---- controller_manager/src/ros2_control_node.cpp | 17 ++++++- 3 files changed, 58 insertions(+), 11 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 810936554e..7c09377bb9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -577,6 +577,7 @@ class ControllerManager : public rclcpp::Node std::map> controller_chained_reference_interfaces_cache_; std::map> controller_chained_state_interfaces_cache_; + rclcpp::NodeOptions cm_node_options_; std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 734596f8bd..70b9c8d628 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -24,6 +24,7 @@ #include "controller_manager_msgs/msg/hardware_component_state.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rcl/arguments.h" #include "rclcpp/version.h" #include "rclcpp_lifecycle/state.hpp" @@ -197,7 +198,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) { init_controller_manager(); } @@ -217,7 +219,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) { init_controller_manager(); } @@ -234,7 +237,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) { init_controller_manager(); } @@ -2819,14 +2823,30 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( rclcpp::NodeOptions controller_node_options = controller.c->define_custom_node_options(); std::vector node_options_arguments = controller_node_options.arguments(); - const std::string ros_args_arg = "--ros-args"; + + for (const std::string & arg : cm_node_options_.arguments()) + { + if (arg.find("__ns") != std::string::npos || arg.find("__node") != std::string::npos) + { + if ( + node_options_arguments.back() == RCL_REMAP_FLAG || + node_options_arguments.back() == RCL_SHORT_REMAP_FLAG) + { + node_options_arguments.pop_back(); + } + continue; + } + + node_options_arguments.push_back(arg); + } + if (controller.info.parameters_file.has_value()) { - if (!check_for_element(node_options_arguments, ros_args_arg)) + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { - node_options_arguments.push_back(ros_args_arg); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } - node_options_arguments.push_back("--params-file"); + node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); node_options_arguments.push_back(controller.info.parameters_file.value()); } @@ -2834,14 +2854,25 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); if (use_sim_time.as_bool()) { - if (!check_for_element(node_options_arguments, ros_args_arg)) + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { - node_options_arguments.push_back(ros_args_arg); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } - node_options_arguments.push_back("-p"); + node_options_arguments.push_back(RCL_PARAM_FLAG); node_options_arguments.push_back("use_sim_time:=true"); } + std::string arguments; + arguments.reserve(1000); + for (const auto & arg : node_options_arguments) + { + arguments.append(arg); + arguments.append(" "); + } + RCLCPP_INFO( + get_logger(), "Controller '%s' node arguments: %s", controller.info.name.c_str(), + arguments.c_str()); + controller_node_options = controller_node_options.arguments(node_options_arguments); controller_node_options.use_global_arguments(false); return controller_node_options; diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 8afc81877f..9c996d5aca 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -41,7 +41,22 @@ int main(int argc, char ** argv) std::make_shared(); std::string manager_node_name = "controller_manager"; - auto cm = std::make_shared(executor, manager_node_name); + rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); + std::vector node_arguments = cm_node_options.arguments(); + for (int i = 1; i < argc; ++i) + { + if (node_arguments.empty() && std::string(argv[i]) != "--ros-args") + { + // A simple way to reject non ros args + continue; + } + RCLCPP_INFO(rclcpp::get_logger("CM args"), "Adding argument: %s", argv[i]); + node_arguments.push_back(argv[i]); + } + cm_node_options.arguments(node_arguments); + + auto cm = std::make_shared( + executor, manager_node_name, "", cm_node_options); RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); From 0aa7eef925476209bfeb69e84aa3355c007fcefc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 24 Aug 2024 10:56:34 +0100 Subject: [PATCH 14/18] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 5 +++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 3 +++ hardware_interface_testing/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 35 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 8a279ace35..de6c1e4566 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- * Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 138e7b5738..5157f6b73b 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* propage a portion of global args to the controller nodes (`#1712 `_) +* Contributors: Sai Kishor Kothakota + 4.16.0 (2024-08-22) ------------------- * inform user what reason is for not setting rt policy, inform is policy (`#1705 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 054243f0f8..0c7c61ee3d 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 71920c6f89..fb38e49c86 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- * Use handle_name\_ variable instead of allocating for every `get_name` call (`#1706 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 9b1cb05d00..cf0769ece2 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 21f0d95a24..3202626c66 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index b7c06c1964..5ccc558858 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 918f5f3502..869e1da56f 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 81d2fe97b5..027e57aff6 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- * Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 0593af2778..27727d8b9f 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 46f665cdb5..83371dadd0 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- * Preparation of Handles for Variant Support (`#1678 `_) From 61648838293df9d3868f75f839908f6d224a2587 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 24 Aug 2024 10:57:09 +0100 Subject: [PATCH 15/18] 4.16.1 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index de6c1e4566..5a0a5ff573 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 951811ba96..39d15a6f20 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.16.0 + 4.16.1 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 5157f6b73b..2729f2f7fe 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- * propage a portion of global args to the controller nodes (`#1712 `_) * Contributors: Sai Kishor Kothakota diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 981fbd919c..cb6b380092 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.16.0 + 4.16.1 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 0c7c61ee3d..56e80d736b 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 5dc5d69437..195fda32f1 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.16.0 + 4.16.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index fb38e49c86..405756a48c 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 2d52a3327a..8c19535675 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.16.0 + 4.16.1 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index cf0769ece2..f1cd9531d0 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 13afe926a5..a88ae6ba93 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.16.0 + 4.16.1 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 3202626c66..02f6673e88 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index d9e18baa1d..8305cff9e5 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.16.0 + 4.16.1 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 5ccc558858..f522eda772 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index d9ade902ca..a8c423658d 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.16.0 + 4.16.1 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 869e1da56f..d5e898924b 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 4bcf6cd1e4..ea22ab6b11 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.16.0 + 4.16.1 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 027e57aff6..639a295a06 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index c11b4cf94f..176334ad54 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.16.0 + 4.16.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 6b8a169fd6..c0395ec63d 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.16.0", + version="4.16.1", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 27727d8b9f..abfab9c3c3 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 7b39a442c4..90e9fd1462 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.16.0 + 4.16.1 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index da798c0e6b..cd0b3141a7 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.16.0", + version="4.16.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 83371dadd0..523f72c1ee 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 44d479a62c..6a982ddef9 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.16.0 + 4.16.1 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 7a5779c43d73b4be6ca5344d62a75b89b3ca76c7 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Mon, 26 Aug 2024 14:09:18 +0200 Subject: [PATCH 16/18] Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (#1683) --- .../controller_interface/async_controller.hpp | 3 +- .../controller_interface_base.hpp | 2 +- .../src/chainable_controller_interface.cpp | 5 +- .../src/controller_interface_base.cpp | 4 +- controller_manager/src/controller_manager.cpp | 13 +- .../test_chainable_controller.cpp | 8 +- .../test/test_controller/test_controller.cpp | 8 +- .../test/test_controller_manager.cpp | 47 ++- ...roller_manager_hardware_error_handling.cpp | 137 ++++--- .../test/test_controller_manager_srvs.cpp | 30 +- .../test_controller_manager_urdf_passing.cpp | 13 +- ...llers_chaining_with_controller_manager.cpp | 355 +++++++++++------- .../test/test_load_controller.cpp | 98 +++-- .../test/test_release_interfaces.cpp | 32 +- .../test/test_spawner_unspawner.cpp | 65 ++-- doc/release_notes.rst | 5 +- .../include/hardware_interface/actuator.hpp | 2 +- .../hardware_interface/actuator_interface.hpp | 7 +- .../hardware_interface/async_components.hpp | 4 +- .../include/hardware_interface/sensor.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 7 +- .../include/hardware_interface/system.hpp | 2 +- .../hardware_interface/system_interface.hpp | 7 +- hardware_interface/src/actuator.cpp | 97 ++--- hardware_interface/src/resource_manager.cpp | 21 +- hardware_interface/src/sensor.cpp | 89 ++--- hardware_interface/src/system.cpp | 97 ++--- .../test/test_component_interfaces.cpp | 20 +- 28 files changed, 700 insertions(+), 480 deletions(-) diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp index 5601ff4703..357b3a2ce3 100644 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ b/controller_interface/include/controller_interface/async_controller.hpp @@ -82,7 +82,8 @@ class AsyncControllerThread TimePoint next_iteration_time = TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds())); - if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if ( + controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto const current_time = controller_->get_node()->now(); auto const measured_period = current_time - previous_time; diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 9eaee9f15b..fbb2111cc9 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -147,7 +147,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::shared_ptr get_node() const; CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; CONTROLLER_INTERFACE_PUBLIC unsigned int get_update_rate() const; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 9f4a171ec3..ae5cd326b6 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -97,7 +97,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) { bool result = false; - if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = on_set_chained_mode(chained_mode); @@ -112,7 +112,8 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) get_node()->get_logger(), "Can not change controller's chained mode because it is no in '%s' state. " "Current state is '%s'.", - hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str()); + hardware_interface::lifecycle_state_names::UNCONFIGURED, + get_lifecycle_state().label().c_str()); } return result; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index db9fb99d81..61d2c780b8 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -83,7 +83,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Then we don't need to do state-machine related checks. // // Other solution is to add check into the LifecycleNode if a transition is valid to trigger - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); is_async_ = get_node()->get_parameter("is_async").as_bool(); @@ -106,7 +106,7 @@ void ControllerInterfaceBase::release_interfaces() state_interfaces_.clear(); } -const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state() const +const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const { return node_->get_current_state(); } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 70b9c8d628..275f12f542 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -58,7 +58,8 @@ static const rmw_qos_profile_t qos_services = { inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; + return controller.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; } inline bool is_controller_inactive( @@ -69,7 +70,7 @@ inline bool is_controller_inactive( inline bool is_controller_active(const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + return controller.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; } inline bool is_controller_active( @@ -659,7 +660,7 @@ controller_interface::return_type ControllerManager::configure_controller( } auto controller = found_it->c; - auto state = controller->get_state(); + auto state = controller->get_lifecycle_state(); if ( state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -670,7 +671,7 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - auto new_state = controller->get_state(); + auto new_state = controller->get_lifecycle_state(); if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_DEBUG( @@ -1744,7 +1745,7 @@ void ControllerManager::list_controllers_srv_cb( controller_state.name = controllers[i].info.name; controller_state.type = controllers[i].info.type; controller_state.claimed_interfaces = controllers[i].info.claimed_interfaces; - controller_state.state = controllers[i].c->get_state().label(); + controller_state.state = controllers[i].c->get_lifecycle_state().label(); controller_state.is_chainable = controllers[i].c->is_chainable(); controller_state.is_chained = controllers[i].c->is_in_chained_mode(); @@ -2728,7 +2729,7 @@ void ControllerManager::controller_activity_diagnostic_callback( { all_active = false; } - stat.add(controllers[i].info.name, controllers[i].c->get_state().label()); + stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); } if (all_active) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index e43f2a13a1..e68af6345f 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -33,8 +33,8 @@ controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return cmd_iface_cfg_; } @@ -49,8 +49,8 @@ controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto state_iface_cfg = state_iface_cfg_; if (imu_sensor_) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7e3419fbc9..7b9af6ecfc 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -31,8 +31,8 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return cmd_iface_cfg_; } @@ -46,8 +46,8 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return state_iface_cfg_; } diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 5093250dfe..28e9f0477c 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -127,7 +127,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); // configure controller { @@ -141,7 +142,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; @@ -182,7 +185,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) ControllerManagerRunner cm_runner(this); EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); EXPECT_EQ( controller_interface::return_type::OK, @@ -210,7 +214,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); auto unload_future = std::async( std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, test_controller::TEST_CONTROLLER_NAME); @@ -221,7 +227,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(1, test_controller.use_count()); } @@ -242,7 +249,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); test_controller->get_node()->set_parameter({"update_rate", 4}); // configure controller @@ -255,7 +263,9 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; @@ -276,7 +286,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); // As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle for (size_t i = 0; i < 25; i++) @@ -323,7 +334,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); test_controller->get_node()->set_parameter(update_rate_parameter); @@ -337,7 +349,9 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; @@ -360,7 +374,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); const auto pre_internal_counter = test_controller->internal_counter; rclcpp::Rate loop_rate(cm_->get_update_rate()); @@ -430,7 +445,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); // configure controller @@ -443,7 +459,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function time_ = test_controller->get_node()->now(); // set to something nonzero @@ -467,7 +485,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); const auto cm_update_rate = cm_->get_update_rate(); diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 6e2fba23db..936a2dd4c4 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -122,16 +122,16 @@ class TestControllerManagerWithTestableCM // check if all controllers are added correctly EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_controller_system->get_state().id()); + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_broadcaster_all->get_state().id()); + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_broadcaster_sensor->get_state().id()); + test_broadcaster_sensor->get_lifecycle_state().id()); // configure controllers cm_->configure_controller(TEST_CONTROLLER_ACTUATOR_NAME); @@ -147,14 +147,16 @@ class TestControllerManagerWithTestableCM EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_broadcaster_sensor->get_state().id()); + test_broadcaster_sensor->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function switch_test_controllers( @@ -180,13 +182,17 @@ TEST_P(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardwar SetupAndConfigureControllers(strictness); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); { auto controllers = @@ -235,13 +241,17 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er } EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); // Execute first time without any errors { @@ -278,13 +288,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) @@ -350,13 +363,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) @@ -427,13 +443,17 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error } EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); // Execute first time without any errors { @@ -456,12 +476,13 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Executes the current cycle and returns ERROR"; EXPECT_EQ( - test_controller_actuator->get_state().id(), + test_controller_actuator->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Executes the current cycle and returns ERROR"; EXPECT_EQ( - test_controller_system->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + test_controller_system->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) @@ -474,13 +495,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) @@ -495,13 +519,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error // The states shouldn't change as there are no more controller errors EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); } } @@ -527,13 +554,17 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e } EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); // Execute first time without any errors { @@ -570,13 +601,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) @@ -644,13 +678,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index e53ad10578..26a5299a07 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -365,7 +365,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) std::weak_ptr test_controller_weak(test_controller); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -388,7 +389,9 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) test_controller_weak = test_controller; cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -412,13 +415,15 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) cm_->switch_controller( {test_controller::TEST_CONTROLLER_NAME}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); // Failed reload due to active controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor); ASSERT_FALSE(result->ok) << "Cannot reload if controllers are running"; - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should still have have a copy of " "this shared ptr, no unloading was performed"; @@ -460,7 +465,7 @@ TEST_F(TestControllerManagerSrvs, load_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - cm_->get_loaded_controllers()[0].c->get_state().id()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); } TEST_F(TestControllerManagerSrvs, unload_controller_srv) @@ -486,7 +491,8 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } @@ -520,7 +526,8 @@ TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controlle unload_request->name = test_controller::TEST_CONTROLLER_NAME; auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); // now load it and check if it got the new robot description @@ -561,15 +568,18 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - cm_->get_loaded_controllers()[0].c->get_state().id()); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // now unload the controller and check the state auto unload_request = std::make_shared(); unload_request->name = test_controller::TEST_CONTROLLER_NAME; ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 1556211a7f..e44db4838b 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -68,7 +68,8 @@ class TestControllerManagerWithTestableCM cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); ASSERT_NE(test_controller_, nullptr); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_->get_lifecycle_state().id()); } void configure_and_try_switch_that_returns_error() @@ -76,7 +77,8 @@ class TestControllerManagerWithTestableCM // configure controller cm_->configure_controller(CONTROLLER_NAME); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_->get_lifecycle_state().id()); // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); @@ -86,7 +88,8 @@ class TestControllerManagerWithTestableCM controller_interface::return_type::ERROR); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_->get_lifecycle_state().id()); } void try_successful_switch() @@ -95,7 +98,9 @@ class TestControllerManagerWithTestableCM strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, controller_interface::return_type::OK); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_->get_lifecycle_state().id()); } std::shared_ptr test_controller_; diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 4a81f3bf12..c143ab4862 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -220,31 +220,31 @@ class TestControllerChainingWithControllerManager EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - pid_left_wheel_controller->get_state().id()); + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - pid_right_wheel_controller->get_state().id()); + pid_right_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - diff_drive_controller->get_state().id()); + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - diff_drive_controller_two->get_state().id()); + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - sensor_fusion_controller->get_state().id()); + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); } // order or controller configuration is not important therefore we can reuse the same method @@ -257,7 +257,7 @@ class TestControllerChainingWithControllerManager // configure chainable controller and check exported interfaces cm_->configure_controller(PID_LEFT_WHEEL); EXPECT_EQ( - pid_left_wheel_controller->get_state().id(), + pid_left_wheel_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 1); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); @@ -270,7 +270,7 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(PID_RIGHT_WHEEL); EXPECT_EQ( - pid_right_wheel_controller->get_state().id(), + pid_right_wheel_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 2); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); @@ -287,7 +287,8 @@ class TestControllerChainingWithControllerManager RCLCPP_ERROR_STREAM(cm_->get_logger(), x); } EXPECT_EQ( - diff_drive_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + diff_drive_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 2); for (const auto & interface : @@ -306,7 +307,7 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(DIFF_DRIVE_CONTROLLER_TWO); EXPECT_EQ( - diff_drive_controller_two->get_state().id(), + diff_drive_controller_two->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); @@ -326,35 +327,35 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(POSITION_TRACKING_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); cm_->configure_controller(SENSOR_FUSION_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 7); cm_->configure_controller(ROBOT_LOCALIZATION_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); cm_->configure_controller(ODOM_PUBLISHER_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); cm_->configure_controller(POSITION_TRACKING_CONTROLLER_TWO); EXPECT_EQ( - position_tracking_controller_two->get_state().id(), + position_tracking_controller_two->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); @@ -963,17 +964,17 @@ TEST_P( EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the robot localization controller and see that the sensor fusion controller is still // active but not in the chained mode @@ -984,14 +985,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the odometry publisher controller DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 8u, true); @@ -1000,15 +1004,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the sensor_fusion controller and see that the diff_drive_controller is still active // but not in the chained mode @@ -1018,15 +1024,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the diff_drive_controller as all it's following controllers that uses it's // interfaces are deactivated @@ -1037,12 +1045,14 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); // all controllers are deactivated --> chained mode is not changed DeactivateAndCheckController( @@ -1054,15 +1064,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); } TEST_P( @@ -1131,18 +1143,22 @@ TEST_P( // Check if the controller activated (Should not be activated) ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); ActivateController( SENSOR_FUSION_CONTROLLER, expected.at(test_param.strictness).return_type, std::future_status::ready); // Check if the controller activated (Should not be activated) ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); // Test Case 2: Try to activate a preceding controller the same time when trying to // deactivate a following controller (using switch_controller function) @@ -1167,16 +1183,18 @@ TEST_P( // should be deactivated (if BEST_EFFORT) // If STRICT, preceding controller should stay deactivated and following controller // should stay activated - EXPECT_EQ(expected.at(test_param.strictness).state, pid_right_wheel_controller->get_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); } TEST_P( @@ -1243,20 +1261,22 @@ TEST_P( // depending controllers are active EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - diff_drive_controller_two->get_state().id()); + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Deactivate position_tracking_controller and activate position_tracking_controller_two switch_test_controllers( @@ -1264,10 +1284,10 @@ TEST_P( std::future_status::timeout, controller_interface::return_type::OK); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Now deactivate the position_tracking_controller_two and it should be in inactive state switch_test_controllers( @@ -1275,7 +1295,7 @@ TEST_P( controller_interface::return_type::OK); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Activate it again and deactivate it others to see if we can deactivate it in a group switch_test_controllers( @@ -1283,10 +1303,10 @@ TEST_P( controller_interface::return_type::OK); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Deactivate the first preceding controller (diff_drive_controller) and // activate the other preceding controller (diff_drive_controller_two) @@ -1298,29 +1318,34 @@ TEST_P( // Following controllers should stay active EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); // The original preceding controller (diff_drive_controller) should be inactive while // the other preceding controller should be active (diff_drive_controller_two) ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller_two->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Activate all the controllers again in group and deactivate the diff_drive_controller_two switch_test_controllers( @@ -1330,32 +1355,37 @@ TEST_P( controller_interface::return_type::OK); // Following controllers should stay active EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - diff_drive_controller_two->get_state().id()); + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); // This is false, because it only uses the state interfaces and exposes state interfaces EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); } TEST_P( @@ -1426,15 +1456,17 @@ TEST_P( // Verify preceding controller (diff_drive_controller) is inactive EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Attempt to deactivate inactive controller (diff_drive_controller) DeactivateController( @@ -1453,19 +1485,23 @@ TEST_P( // Check to see preceding controller (diff_drive_controller) is still inactive and // following controllers (pid_left_wheel_controller) (pid_left_wheel_controller) are still active EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Test Case 6: following controller is deactivated but preceding controller will be activated // --> return error; controllers stay in the same state @@ -1479,10 +1515,15 @@ TEST_P( // should be deactivated (if BEST_EFFORT) // If STRICT, preceding controller should stay deactivated and following controller // should stay activated - EXPECT_EQ(expected.at(test_param.strictness).state, pid_right_wheel_controller->get_state().id()); - EXPECT_EQ(expected.at(test_param.strictness).state, pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + expected.at(test_param.strictness).state, + pid_right_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + expected.at(test_param.strictness).state, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // Test Case 7: following controller deactivation but preceding controller is active // --> return error; controllers stay in the same state as they were @@ -1499,11 +1540,14 @@ TEST_P( // Expect all controllers to be active ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // Attempt to deactivate following controllers switch_test_controllers( @@ -1512,11 +1556,14 @@ TEST_P( // All controllers should still be active ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // Attempt to deactivate a following controller switch_test_controllers( @@ -1525,11 +1572,11 @@ TEST_P( // All controllers should still be active ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); } TEST_P( @@ -1602,21 +1649,26 @@ TEST_P( // Verify that initially all of them are in active state EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); // There is different error and timeout behavior depending on strictness static std::unordered_map expected = { @@ -1643,16 +1695,19 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // DiffDrive (preceding) controller is activated --> PID controller in chained mod // Let's try to deactivate the diff_drive_control, it should fail as there are still other @@ -1668,14 +1723,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Trying to deactivate the sensor fusion controller, however, it won't be deactivated as the // robot localization controller is still active @@ -1688,14 +1746,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the robot localization controller and see that the sensor fusion controller is still // active but not in the chained mode @@ -1706,14 +1767,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the sensor_fusion controller and this should be successful as there are no other // controllers using it's interfaces @@ -1723,14 +1787,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the odometry publisher controller and now the diff_drive should continue active but // not in chained mode @@ -1740,15 +1807,17 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the diff_drive_controller as all it's following controllers that uses it's // interfaces are deactivated @@ -1759,12 +1828,14 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); // all controllers are deactivated --> chained mode is not changed DeactivateAndCheckController( @@ -1776,15 +1847,17 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); } TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 19d34c0c11..3645c06c24 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -63,12 +63,14 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) ASSERT_NE(controller_if, nullptr); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); controller_if->get_node()->set_parameter({"update_rate", 1337}); cm_->configure_controller("test_controller_01"); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); EXPECT_EQ(1337u, controller_if->get_update_rate()); } @@ -119,10 +121,12 @@ TEST_P(TestLoadedControllerParametrized, load_and_configure_one_known_controller EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); cm_->configure_controller(CONTROLLER_NAME_1); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) @@ -131,7 +135,8 @@ TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) @@ -144,7 +149,8 @@ TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) // Stop controller stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) @@ -152,14 +158,16 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) const auto test_param = GetParam(); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); { // Test starting unconfigured controller, and starting configured afterwards start_test_controller( test_param.strictness, std::future_status::ready, test_param.expected_return); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); // Activate configured controller { @@ -167,12 +175,15 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) cm_->configure_controller(CONTROLLER_NAME_1); } start_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); } { // Stop controller stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + controller_if->get_lifecycle_state().id()); } } @@ -185,7 +196,8 @@ TEST_P(TestLoadedControllerParametrized, can_not_configure_active_controller) // Can not configure active controller EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) @@ -193,7 +205,8 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) const auto test_param = GetParam(); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); // Shutdown controller on purpose for testing ASSERT_EQ( @@ -206,7 +219,8 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) // Can not configure finalize controller EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_up) @@ -219,7 +233,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); std::shared_ptr test_controller = std::dynamic_pointer_cast(controller_if); @@ -228,7 +243,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u // Configure from inactive state: controller can no be cleaned-up test_controller->simulate_cleanup_failure = true; EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); EXPECT_EQ(0u, cleanup_calls); } @@ -241,7 +257,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure start_test_controller(test_param.strictness); stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); std::shared_ptr test_controller = std::dynamic_pointer_cast(controller_if); @@ -253,7 +270,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure ControllerManagerRunner cm_runner(this); EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); } - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); EXPECT_EQ(1u, cleanup_calls); } @@ -278,7 +296,8 @@ TEST_P(SwitchTest, EmptyListOrNonExistentTest) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); auto params = GetParam(); auto result = std::get<0>(params); @@ -382,19 +401,23 @@ class TestTwoLoadedControllers : public TestLoadController, ASSERT_NE(controller_if2, nullptr); EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if1->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if1->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); } }; TEST_F(TestTwoLoadedControllers, load_and_configure_two_known_controllers) { cm_->configure_controller(CONTROLLER_NAME_1); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); cm_->configure_controller(CONTROLLER_NAME_2); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); } TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) @@ -406,9 +429,11 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // Start controller #1 RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); // Stop controller 1, start controller 2 // Both fail because controller 2 because it is not configured and STRICT is used @@ -423,9 +448,9 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) switch_test_controllers( strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, STRICT, std::future_status::ready, controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); { ControllerManagerRunner cm_runner(this); @@ -435,26 +460,33 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_1}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); // Start controller 1 again RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); // Stop controller 1, start controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1, starting controller #2"); switch_test_controllers( strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_lifecycle_state().id()); // Stop controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #2"); switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_2}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); } INSTANTIATE_TEST_SUITE_P( diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index c0305324b6..9caef761ab 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -49,10 +49,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); { // Test starting the first controller RCLCPP_INFO(cm_->get_logger(), "Starting controller #1"); @@ -67,10 +67,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test starting the second controller when the first is running @@ -87,10 +87,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping controller #1 and starting controller #2 @@ -106,10 +106,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping controller #2 and starting controller #1 @@ -125,10 +125,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -149,10 +149,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -170,10 +170,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test starting both controllers at the same time @@ -191,9 +191,9 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } } diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 49326f9e79..d073abaf95 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -111,7 +111,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } // Try to spawn again, it should fail because already active @@ -131,7 +132,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } cm_->switch_controller( @@ -147,7 +149,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } // Unload and reload @@ -159,7 +162,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -186,7 +190,8 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) [&](const auto & controller) { return controller.info.name == controller_name; }); ASSERT_TRUE(it != loaded_controllers.end()); ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } }; @@ -270,7 +275,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -280,7 +285,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -295,12 +300,14 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } TEST_F(TestLoadController, unload_on_kill) @@ -339,7 +346,8 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } // Try to spawn now the controller with fallback controllers inside the yaml @@ -352,20 +360,23 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_THAT( ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_3 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); - ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } } @@ -391,7 +402,7 @@ TEST_F(TestLoadController, spawner_with_many_controllers) { auto ctrl = cm_->get_loaded_controllers()[i]; ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -472,7 +483,8 @@ TEST_F( auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -548,7 +560,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) [&](const auto & controller) { return controller.info.name == controller_name; }); ASSERT_TRUE(it != loaded_controllers.end()); ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } }; @@ -646,7 +659,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -656,7 +669,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -671,12 +684,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } TEST_F( @@ -722,7 +737,7 @@ TEST_F( ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -732,7 +747,7 @@ TEST_F( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -747,10 +762,12 @@ TEST_F( auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 4aaa828091..d0fe7b5962 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,8 +22,8 @@ For details see the controller_manager section. * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. -* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). - From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. +* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_).From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. controller_manager ****************** @@ -103,6 +103,7 @@ hardware_interface * Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. joint_limits ************ diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index bb91b62057..4fcdd93307 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -93,7 +93,7 @@ class Actuator final std::string get_group_name() const; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index a74d2a4964..d88fcd1f2d 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -220,13 +220,16 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * \return state. */ - const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** * \return state. */ - void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } /// Get the logger of the ActuatorInterface. /** diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp index 0504419b38..052c4ba921 100644 --- a/hardware_interface/include/hardware_interface/async_components.hpp +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -84,7 +84,9 @@ class AsyncComponentThread TimePoint next_iteration_time = TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds())); - if (component->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if ( + component->get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto current_time = clock_interface_->get_clock()->now(); auto measured_period = current_time - previous_time; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 98197bd0e9..e1490c91fb 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -80,7 +80,7 @@ class Sensor final std::string get_group_name() const; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 0c7f43e7fc..4e9ac48d5d 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -159,13 +159,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return state. */ - const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** * \return state. */ - void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } /// Get the logger of the SensorInterface. /** diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 1cc29a085d..34056c982e 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -93,7 +93,7 @@ class System final std::string get_group_name() const; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 54f1af33a7..37745b36fb 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -222,13 +222,16 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return state. */ - const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** * \return state. */ - void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } /// Get the logger of the SystemInterface. /** diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 52cfc681a1..d586b463af 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -47,158 +47,158 @@ const rclcpp_lifecycle::State & Actuator::initialize( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(actuator_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::configure() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - switch (impl_->on_configure(impl_->get_state())) + switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::cleanup() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_cleanup(impl_->get_state())) + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::shutdown() { std::unique_lock lock(actuators_mutex_); if ( - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - switch (impl_->on_shutdown(impl_->get_state())) + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::activate() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_activate(impl_->get_state())) + switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::deactivate() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - switch (impl_->on_deactivate(impl_->get_state())) + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::error() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_error(impl_->get_state())) + switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } std::vector Actuator::export_state_interfaces() @@ -231,7 +231,10 @@ std::string Actuator::get_name() const { return impl_->get_name(); } std::string Actuator::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_state(); } +const rclcpp_lifecycle::State & Actuator::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -244,15 +247,15 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->read(time, period); if (result == return_type::ERROR) @@ -274,15 +277,15 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->write(time, period); if (result == return_type::ERROR) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 53fcad94d2..72e966fcab 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -514,7 +514,7 @@ class ResourceStorage switch (target_state.id()) { case State::PRIMARY_STATE_UNCONFIGURED: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = true; @@ -538,7 +538,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_INACTIVE: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -558,7 +558,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_ACTIVE: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -582,7 +582,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_FINALIZED: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = shutdown_hardware(hardware); @@ -1462,7 +1462,8 @@ std::unordered_map ResourceManager::get_comp { for (auto & component : container) { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); + resource_storage_->hardware_info_map_[component.get_name()].state = + component.get_lifecycle_state(); } }; @@ -1546,8 +1547,9 @@ bool ResourceManager::prepare_command_mode_switch( for (auto & component : components) { if ( - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try { @@ -1611,8 +1613,9 @@ bool ResourceManager::perform_command_mode_switch( for (auto & component : components) { if ( - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try { diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9f1d4b73e1..2cffa649fd 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -46,158 +46,158 @@ const rclcpp_lifecycle::State & Sensor::initialize( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(sensor_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::configure() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - switch (impl_->on_configure(impl_->get_state())) + switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::cleanup() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_cleanup(impl_->get_state())) + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::shutdown() { std::unique_lock lock(sensors_mutex_); if ( - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - switch (impl_->on_shutdown(impl_->get_state())) + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::activate() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_activate(impl_->get_state())) + switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::deactivate() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - switch (impl_->on_deactivate(impl_->get_state())) + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::error() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_error(impl_->get_state())) + switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } std::vector Sensor::export_state_interfaces() @@ -209,7 +209,10 @@ std::string Sensor::get_name() const { return impl_->get_name(); } std::string Sensor::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); } +const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -222,15 +225,15 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->read(time, period); if (result == return_type::ERROR) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 82473563b6..8f455a7bd5 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -45,158 +45,158 @@ const rclcpp_lifecycle::State & System::initialize( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(system_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::configure() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - switch (impl_->on_configure(impl_->get_state())) + switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::cleanup() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_cleanup(impl_->get_state())) + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::shutdown() { std::unique_lock lock(system_mutex_); if ( - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - switch (impl_->on_shutdown(impl_->get_state())) + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::activate() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_activate(impl_->get_state())) + switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::deactivate() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - switch (impl_->on_deactivate(impl_->get_state())) + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::error() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_error(impl_->get_state())) + switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } std::vector System::export_state_interfaces() @@ -227,7 +227,10 @@ std::string System::get_name() const { return impl_->get_name(); } std::string System::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_state(); } +const rclcpp_lifecycle::State & System::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -240,15 +243,15 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->read(time, period); if (result == return_type::ERROR) @@ -270,15 +273,15 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->write(time, period); if (result == return_type::ERROR) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 986c32d37b..b6a8cc81c8 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -726,7 +726,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -749,7 +749,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -786,7 +786,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -809,7 +809,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -843,7 +843,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - state = sensor_hw.get_state(); + state = sensor_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -868,7 +868,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - state = sensor_hw.get_state(); + state = sensor_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -911,7 +911,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -939,7 +939,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -976,7 +976,7 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1004,7 +1004,7 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); From 9e57adfbc414f92492278fdba10e60716a5995b7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 26 Aug 2024 14:10:35 +0200 Subject: [PATCH 17/18] Fix unload of controllers when spawned with `--unload-on-kill` (#1717) --- .../controller_manager/spawner.py | 27 ++++++++++++------- .../test/test_spawner_unspawner.cpp | 19 +++++++++++++ 2 files changed, 37 insertions(+), 9 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index bd277d5464..d6df3be01f 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -234,7 +234,7 @@ def main(args=None): node.get_logger().info( bcolors.OKGREEN - + "Configured and activated all the parsed controllers list!" + + f"Configured and activated all the parsed controllers list : {controller_names}!" + bcolors.ENDC ) @@ -258,16 +258,25 @@ def main(args=None): ) return 1 - node.get_logger().info("Deactivated controller") - - ret = unload_controller(node, controller_manager_name, controller_name) - if not ret.ok: - node.get_logger().error( - bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC + node.get_logger().info( + f"Successfully deactivated controllers : {controller_names}" ) - return 1 - node.get_logger().info("Unloaded controller") + unload_status = True + for controller_name in controller_names: + ret = unload_controller(node, controller_manager_name, controller_name) + if not ret.ok: + unload_status = False + node.get_logger().error( + bcolors.FAIL + + f"Failed to unload controller : {controller_name}" + + bcolors.ENDC + ) + + if unload_status: + node.get_logger().info(f"Successfully unloaded controllers : {controller_names}") + else: + return 1 return 0 except KeyboardInterrupt: pass diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index d073abaf95..0812d7ffa4 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -328,6 +328,25 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +TEST_F(TestLoadController, unload_on_kill_activate_as_group) +{ + // Launch spawner with unload on kill + // timeout command will kill it after the specified time with signal SIGINT + ControllerManagerRunner cm_runner(this); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + std::stringstream ss; + ss << "timeout --signal=INT 5 " + << std::string(coveragepy_script) + + " $(ros2 pkg prefix controller_manager)/lib/controller_manager/spawner " + << "ctrl_3 ctrl_2 --activate-as-group -c test_controller_manager --unload-on-kill"; + + EXPECT_NE(std::system(ss.str().c_str()), 0) + << "timeout should have killed spawner and returned non 0 code"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); +} + TEST_F(TestLoadController, spawner_test_fallback_controllers) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + From 8f7374faf482e83c94c0a477bfbe8b2e672f241c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 26 Aug 2024 15:57:40 +0200 Subject: [PATCH 18/18] Add breaking changes to migration notes (#1718) --- doc/migration.rst | 2 ++ doc/release_notes.rst | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/doc/migration.rst b/doc/migration.rst index 8e1ca096f1..255db9b8ae 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -6,6 +6,7 @@ controller_interface ******************** * The changes from `(PR #1694) `__ will affect how the controllers will be loading the parameters. Defining parameters in a single yaml file and loading it to the controller_manager node alone will no longer work. In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments. +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. controller_manager ****************** @@ -73,3 +74,4 @@ controller_manager hardware_interface ****************** * ``test_components`` was moved to its own package. Update the dependencies if you are using them. (`#1325 `_) +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index d0fe7b5962..50583b3bf4 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,7 +22,7 @@ For details see the controller_manager section. * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. -* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_).From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. +* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. controller_manager