diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 29862f70e4..6fadbbdace 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.16.0 + rev: v3.17.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.28.6 + rev: 0.29.1 hooks: - id: check-github-workflows args: ["--verbose"] diff --git a/codecov.yml b/codecov.yml index 97106f32ff..764afc34e6 100644 --- a/codecov.yml +++ b/codecov.yml @@ -14,8 +14,6 @@ fixes: comment: layout: "diff, flags, files" behavior: default -ignore: - - "**/test" flags: unittests: paths: diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index c4f9c56800..5a0a5ff573 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +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 + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* move critical variables to the private context (`#1623 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.13.0 (2024-07-08) ------------------- * [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) 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/package.xml b/controller_interface/package.xml index 1ee83bd019..39d15a6f20 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.13.0 + 4.16.1 Description of controller_interface Bence Magyar Denis Štogl 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_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); diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 1ef5eef03b..2729f2f7fe 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,44 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- +* 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 `_) +* 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 `_) +* Fix the namespaced controller_manager spawner + added tests (`#1640 `_) +* CM: Add missing includes (`#1641 `_) +* Fix rst markup (`#1642 `_) +* Add a pytest launch file to test ros2_control_node (`#1636 `_) +* [CM] Remove deprecated spawner args (`#1639 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Remove noqa (`#1626 `_) +* Fix controller starting with later load of robot description test (`#1624 `_) +* [CM] Remove support for the description parameter and use only `robot_description` topic (`#1358 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Henry Moore, Sai Kishor Kothakota + 4.13.0 (2024-07-08) ------------------- * Change the spamming checking interface ERROR to DEBUG (`#1605 `_) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 7c437d35e0..5dea15c0d1 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -185,6 +185,7 @@ if(BUILD_TESTING) ament_add_gmock(test_spawner_unspawner test/test_spawner_unspawner.cpp + TIMEOUT 120 ) target_link_libraries(test_spawner_unspawner controller_manager @@ -209,6 +210,11 @@ if(BUILD_TESTING) ament_target_dependencies(test_hardware_management_srvs controller_manager_msgs ) + + find_package(ament_cmake_pytest REQUIRED) + install(FILES test/test_ros2_control_node.yaml + DESTINATION test) + ament_add_pytest_test(test_ros2_control_node test/test_ros2_control_node_launch.py) endif() install( 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 62b350f7d3..a09be5d14b 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -26,28 +26,96 @@ ) 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 -def service_caller(node, service_name, service_type, request, service_timeout=10.0): + +# 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): + pass + + +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) - if not cli.service_is_ready(): - node.get_logger().debug( - f"waiting {service_timeout} seconds for service {service_name} to become available..." - ) - if not cli.wait_for_service(service_timeout): - raise RuntimeError(f"Could not contact service {service_name}") + while not cli.service_is_ready(): + node.get_logger().info(f"waiting for service {service_name} to become available...") + if service_timeout: + if not cli.wait_for_service(service_timeout): + raise ServiceNotFoundError(f"Could not contact service {service_name}") + elif not cli.wait_for_service(10.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=10.0): +def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = ConfigureController.Request() request.name = controller_name return service_caller( @@ -59,7 +127,7 @@ def configure_controller(node, controller_manager_name, controller_name, service ) -def list_controllers(node, controller_manager_name, service_timeout=10.0): +def list_controllers(node, controller_manager_name, service_timeout=0.0): request = ListControllers.Request() return service_caller( node, @@ -70,7 +138,7 @@ def list_controllers(node, controller_manager_name, service_timeout=10.0): ) -def list_controller_types(node, controller_manager_name, service_timeout=10.0): +def list_controller_types(node, controller_manager_name, service_timeout=0.0): request = ListControllerTypes.Request() return service_caller( node, @@ -81,7 +149,7 @@ def list_controller_types(node, controller_manager_name, service_timeout=10.0): ) -def list_hardware_components(node, controller_manager_name, service_timeout=10.0): +def list_hardware_components(node, controller_manager_name, service_timeout=0.0): request = ListHardwareComponents.Request() return service_caller( node, @@ -92,7 +160,7 @@ def list_hardware_components(node, controller_manager_name, service_timeout=10.0 ) -def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0): +def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0): request = ListHardwareInterfaces.Request() return service_caller( node, @@ -103,7 +171,7 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0 ) -def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = LoadController.Request() request.name = controller_name return service_caller( @@ -115,7 +183,7 @@ def load_controller(node, controller_manager_name, controller_name, service_time ) -def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0): +def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -128,7 +196,7 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi def set_hardware_component_state( - node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0 + node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 ): request = SetHardwareComponentState.Request() request.name = component_name @@ -165,7 +233,7 @@ def switch_controllers( ) -def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = UnloadController.Request() request.name = controller_name return service_caller( @@ -175,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, "params_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 c95fb6181e..29c0b5e97c 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -15,45 +15,24 @@ import argparse import sys -import time -from controller_manager import set_hardware_component_state +from controller_manager import ( + list_hardware_components, + set_hardware_component_state, + bcolors, +) +from controller_manager.controller_manager_services import ServiceNotFoundError from lifecycle_msgs.msg import State import rclpy -from rclpy.duration import Duration from rclpy.node import Node 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) -def wait_for_value_or(function, node, timeout, default, description): - while node.get_clock().now() < timeout: - if result := function(): - return result - node.get_logger().info( - f"Waiting for {description}", throttle_duration_sec=2, skip_first=True - ) - time.sleep(0.2) - return default - - def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -75,35 +54,11 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def wait_for_controller_manager(node, controller_manager, timeout_duration): - # List of service names from controller_manager we wait for - service_names = ( - f"{controller_manager}/list_hardware_components", - f"{controller_manager}/set_hardware_component_state", - ) - - # Wait for controller_manager - timeout = node.get_clock().now() + Duration(seconds=timeout_duration) - node_and_namespace = wait_for_value_or( - lambda: find_node_and_namespace(node, controller_manager), - node, - timeout, - None, - f"'{controller_manager}' node to exist", - ) - - # Wait for the services if the node was found - if node_and_namespace: - node_name, namespace = node_and_namespace - return wait_for_value_or( - lambda: has_service_names(node, node_name, namespace, service_names), - node, - timeout, - False, - f"'{controller_manager}' services to be available", - ) - - return False +def is_hardware_component_loaded( + node, controller_manager, hardware_component, service_timeout=0.0 +): + components = list_hardware_components(node, hardware_component, service_timeout).component + return any(c.name == hardware_component for c in components) def handle_set_component_state_service_call( @@ -167,10 +122,9 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) - # add arguments which are mutually exclusive activate_or_confiigure_grp.add_argument( "--activate", @@ -202,13 +156,15 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: - if not wait_for_controller_manager( - node, controller_manager_name, controller_manager_timeout + if not is_hardware_component_loaded( + node, controller_manager_name, hardware_component, controller_manager_timeout ): - node.get_logger().error("Controller manager not available") - return 1 - - if activate: + node.get_logger().warn( + bcolors.WARNING + + "Hardware Component is not loaded - state can not be changed." + + bcolors.ENDC + ) + elif activate: activate_components(node, controller_manager_name, hardware_component) elif configure: configure_components(node, controller_manager_name, hardware_component) @@ -218,6 +174,11 @@ def main(args=None): ) parser.print_help() return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 14d0ad68d1..3bbb050433 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import warnings from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PythonExpression @@ -21,7 +20,7 @@ def generate_load_controller_launch_description( - controller_name, controller_type=None, controller_params_file=None, extra_spawner_args=[] + controller_name, controller_params_file=None, extra_spawner_args=[] ): """ Generate launch description for loading a controller using spawner. @@ -32,13 +31,12 @@ def generate_load_controller_launch_description( Examples -------- - # Assuming the controller type and controller parameters are known to the controller_manager + # Assuming the controller parameters are known to the controller_manager generate_load_controller_launch_description('joint_state_broadcaster') - # Passing controller type and controller parameter file to load + # Passing controller parameter file to load the controller (Controller type is retrieved from config file) generate_load_controller_launch_description( 'joint_state_broadcaster', - controller_type='joint_state_broadcaster/JointStateBroadcaster', controller_params_file=os.path.join(get_package_share_directory('my_pkg'), 'config', 'controller_params.yaml'), extra_spawner_args=[--load-only] @@ -62,15 +60,6 @@ def generate_load_controller_launch_description( LaunchConfiguration("controller_manager_name"), ] - if controller_type: - warnings.warn( - "The 'controller_type' argument is deprecated and will be removed in future releases." - " Declare the controller type parameter in the param file instead.", - DeprecationWarning, - stacklevel=2, - ) - spawner_arguments += ["--controller-type", controller_type] - if controller_params_file: spawner_arguments += ["--param-file", controller_params_file] diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index dbefd360b9..d6df3be01f 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,52 +26,20 @@ 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.duration import Duration 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: - 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) -def wait_for_value_or(function, node, timeout, default, description): - while node.get_clock().now() < timeout: - if result := function(): - return result - node.get_logger().info( - f"Waiting for {description}", throttle_duration_sec=2, skip_first=True - ) - time.sleep(0.2) - return default - - def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace return namespace + ("" if namespace.endswith("/") else "/") + node_name @@ -93,64 +60,11 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def wait_for_controller_manager(node, controller_manager, timeout_duration): - # List of service names from controller_manager we wait for - service_names = ( - f"{controller_manager}/configure_controller", - f"{controller_manager}/list_controllers", - f"{controller_manager}/list_controller_types", - f"{controller_manager}/list_hardware_components", - f"{controller_manager}/list_hardware_interfaces", - f"{controller_manager}/load_controller", - f"{controller_manager}/reload_controller_libraries", - f"{controller_manager}/switch_controller", - f"{controller_manager}/unload_controller", - ) - - # Wait for controller_manager - timeout = node.get_clock().now() + Duration(seconds=timeout_duration) - node_and_namespace = wait_for_value_or( - lambda: find_node_and_namespace(node, controller_manager), - node, - timeout, - None, - f"'{controller_manager}' node to exist", - ) - - # Wait for the services if the node was found - if node_and_namespace: - node_name, namespace = node_and_namespace - return wait_for_value_or( - lambda: has_service_names(node, node_name, namespace, service_names), - node, - timeout, - False, - f"'{controller_manager}' services to be available", - ) - - return False - - -def is_controller_loaded(node, controller_manager, controller_name): - controllers = list_controllers(node, controller_manager).controller +def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0): + controllers = list_controllers(node, controller_manager, service_timeout).controller return any(c.name == controller_name for c in controllers) -def get_parameter_from_param_file(controller_name, parameter_file, parameter_name): - with open(parameter_file) as f: - parameters = yaml.safe_load(f) - if controller_name in parameters: - value = parameters[controller_name] - 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 : {controller_name}" - ) - if parameter_name in parameters[controller_name]["ros__parameters"]: - return parameters[controller_name]["ros__parameters"][parameter_name] - else: - return None - - def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() @@ -166,10 +80,15 @@ def main(args=None): "-p", "--param-file", help="Controller param file to be loaded into controller node before configure", + default=None, required=False, ) parser.add_argument( - "-n", "--namespace", help="Namespace for the controller", default="", required=False + "-n", + "--namespace", + help="DEPRECATED Namespace for the controller_manager and the controller(s)", + default=None, + required=False, ) parser.add_argument( "--load-only", @@ -183,13 +102,6 @@ def main(args=None): action="store_true", required=False, ) - parser.add_argument( - "-t", - "--controller-type", - help="If not provided it should exist in the controller manager namespace (deprecated)", - default=None, - required=False, - ) parser.add_argument( "-u", "--unload-on-kill", @@ -200,8 +112,8 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) parser.add_argument( "--activate-as-group", @@ -210,169 +122,64 @@ 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) controller_names = args.controller_names controller_manager_name = args.controller_manager - controller_namespace = args.namespace param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout - if args.controller_type: + if param_file and not os.path.isfile(param_file): + raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) + + node = Node("spawner_" + controller_names[0]) + + if node.get_namespace() != "/" and args.namespace: + raise RuntimeError( + f"Setting namespace through both '--namespace {args.namespace}' arg and the ROS 2 standard way " + f"'--ros-args -r __ns:={node.get_namespace()}' is not allowed!" + ) + + if args.namespace: warnings.filterwarnings("always") warnings.warn( - "The '--controller-type' argument is deprecated and will be removed in future releases." - " Declare the controller type parameter in the param file instead.", + "The '--namespace' argument is deprecated and will be removed in future releases." + " Use the ROS 2 standard way of setting the node namespacing using --ros-args -r __ns:=", DeprecationWarning, ) - if param_file and not os.path.isfile(param_file): - raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) + spawner_namespace = args.namespace if args.namespace else node.get_namespace() - node = Node("spawner_" + controller_names[0]) + if not spawner_namespace.startswith("/"): + spawner_namespace = f"/{spawner_namespace}" if not controller_manager_name.startswith("/"): - spawner_namespace = node.get_namespace() - if spawner_namespace != "/": + if spawner_namespace and spawner_namespace != "/": controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" else: controller_manager_name = f"/{controller_manager_name}" try: - if not wait_for_controller_manager( - node, controller_manager_name, controller_manager_timeout - ): - node.get_logger().error( - bcolors.FAIL + "Controller manager not available" + bcolors.ENDC - ) - return 1 - for controller_name in controller_names: - fallback_controllers = args.fallback_controllers - controller_type = args.controller_type - prefixed_controller_name = controller_name - if controller_namespace: - prefixed_controller_name = controller_namespace + "/" + controller_name - if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): + if is_controller_loaded( + node, controller_manager_name, controller_name, controller_manager_timeout + ): node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" + bcolors.ENDC ) else: - if not controller_type and param_file: - controller_type = get_parameter_from_param_file( - controller_name, param_file, "type" - ) - if controller_type: - parameter = Parameter() - parameter.name = prefixed_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 - + prefixed_controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller type to "' - + controller_type - + '" for ' - + bcolors.BOLD - + prefixed_controller_name - + bcolors.ENDC - ) - return 1 - if param_file: - parameter = Parameter() - parameter.name = prefixed_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 - + prefixed_controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller params file to "' - + param_file - + '" for ' - + bcolors.BOLD - + prefixed_controller_name - + bcolors.ENDC - ) - return 1 - - if not fallback_controllers and param_file: - fallback_controllers = get_parameter_from_param_file( - controller_name, param_file, "fallback_controllers" - ) - - if fallback_controllers: - parameter = Parameter() - parameter.name = prefixed_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 - + prefixed_controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set fallback_controllers to ["' - + ",".join(fallback_controllers) - + '"] for ' - + bcolors.BOLD - + prefixed_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) @@ -381,16 +188,12 @@ def main(args=None): bcolors.FAIL + "Failed loading controller " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 node.get_logger().info( - bcolors.OKBLUE - + "Loaded " - + bcolors.BOLD - + prefixed_controller_name - + bcolors.ENDC + bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC ) if not args.load_only: @@ -415,7 +218,7 @@ def main(args=None): bcolors.OKGREEN + "Configured and activated " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) @@ -431,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 ) @@ -455,17 +258,31 @@ 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 + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 5acf05e65d..e42d85aee9 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -19,6 +19,7 @@ import warnings from controller_manager import switch_controllers, unload_controller +from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy from rclpy.node import Node @@ -57,6 +58,11 @@ def main(args=None): node.get_logger().info("Unloaded controller") return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 02e532866d..bc9f75384e 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -136,6 +136,7 @@ There are two scripts to interact with controller manager from launch files: 1. ``spawner`` - loads, configures and start a controller on startup. 2. ``unspawner`` - stops and unloads a controller. + 3. ``hardware_spawner`` - activates and configures a hardware component. ``spawner`` @@ -149,7 +150,7 @@ There are two scripts to interact with controller manager from launch files: controller_name positional arguments: - controller_name Name of the controller + controller_names List of controllers options: -h, --help show this help message and exit @@ -158,14 +159,16 @@ There are two scripts to interact with controller manager from launch files: -p PARAM_FILE, --param-file PARAM_FILE Controller param file to be loaded into controller node before configure -n NAMESPACE, --namespace NAMESPACE - Namespace for the controller + DEPRECATED Namespace for the controller_manager and the controller(s) --load-only Only load the controller and leave unconfigured. --inactive Load and configure the controller, however do not activate them - -t CONTROLLER_TYPE, --controller-type CONTROLLER_TYPE - If not provided it should exist in the controller manager namespace -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT Time to wait for the controller manager + --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether + --fallback_controllers FALLBACK_CONTROLLERS [FALLBACK_CONTROLLERS ...] + 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 ``unspawner`` @@ -184,20 +187,39 @@ There are two scripts to interact with controller manager from launch files: -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node +``hardware_spawner`` +^^^^^^^^^^^^^^^^^^^^^^ + +.. code-block:: console + + $ ros2 run controller_manager hardware_spawner -h + usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name + + positional arguments: + hardware_component_name + The name of the hardware component which should be activated. + + options: + -h, --help show this help message and exit + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node + --activate Activates the given components. Note: Components are by default configured before activated. + --configure Configures the given components. + rqt_controller_manager ---------------------- A GUI tool to interact with the controller manager services to be able to switch the lifecycle states of the controllers as well as the hardware components. .. image:: images/rqt_controller_manager.png -It can be launched independently using the following command or as rqt plugin. +It can be launched independently using the following command or as rqt plugin: .. code-block:: console ros2 run rqt_controller_manager rqt_controller_manager - * Double-click on a controller or hardware component to show the additional info. - * Right-click on a controller or hardware component to show a context menu with options for lifecycle management. +* Double-click on a controller or hardware component to show the additional info. +* Right-click on a controller or hardware component to show a context menu with options for lifecycle management. Using the Controller Manager in a Process ----------------------------------------- diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 2f326ff302..7c09377bb9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -577,20 +577,32 @@ 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_; struct SwitchParams { - bool do_switch = {false}; - bool started = {false}; - rclcpp::Time init_time = {rclcpp::Time::max()}; + void reset() + { + do_switch = false; + started = false; + strictness = 0; + activate_asap = false; + } + + bool do_switch; + bool started; // Switch options - int strictness = {0}; - bool activate_asap = {false}; - rclcpp::Duration timeout = rclcpp::Duration{0, 0}; + int strictness; + bool activate_asap; + std::chrono::nanoseconds timeout; + + // conditional variable and mutex to wait for the switch to complete + std::condition_variable cv; + std::mutex mutex; }; SwitchParams switch_params_; diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c8da38364a..cb6b380092 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.13.0 + 4.16.1 Description of controller_manager Bence Magyar Denis Štogl @@ -30,6 +30,8 @@ std_msgs ament_cmake_gmock + ament_cmake_pytest + python3-coverage hardware_interface_testing ros2_control_test_assets diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 48dbbc720a..275f12f542 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -15,6 +15,7 @@ #include "controller_manager/controller_manager.hpp" #include +#include #include #include #include @@ -23,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" @@ -56,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( @@ -67,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( @@ -196,7 +199,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) { init_controller_manager(); } @@ -216,7 +220,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) { init_controller_manager(); } @@ -233,7 +238,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) { init_controller_manager(); } @@ -529,12 +535,16 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c get_logger(), "The 'type' param was not defined for '%s'.", controller_name.c_str()); return nullptr; } + RCLCPP_INFO( + get_logger(), "Loading controller : '%s' of type '%s'", controller_name.c_str(), + controller_type.c_str()); return load_controller(controller_name, controller_type); } controller_interface::return_type ControllerManager::unload_controller( const std::string & controller_name) { + RCLCPP_INFO(get_logger(), "Unloading controller: '%s'", controller_name.c_str()); std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); @@ -632,7 +642,7 @@ std::vector ControllerManager::get_loaded_controllers() const controller_interface::return_type ControllerManager::configure_controller( const std::string & controller_name) { - RCLCPP_INFO(get_logger(), "Configuring controller '%s'", controller_name.c_str()); + RCLCPP_INFO(get_logger(), "Configuring controller: '%s'", controller_name.c_str()); const auto & controllers = get_loaded_controllers(); @@ -650,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) @@ -661,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( @@ -848,6 +858,7 @@ controller_interface::return_type ControllerManager::configure_controller( void ControllerManager::clear_requests() { + switch_params_.do_switch = false; deactivate_request_.clear(); activate_request_.clear(); // Set these interfaces as unavailable when clearing requests to avoid leaving them in available @@ -877,7 +888,8 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } - switch_params_ = SwitchParams(); + // reset the switch param internal variables + switch_params_.reset(); if (!deactivate_request_.empty() || !activate_request_.empty()) { @@ -916,16 +928,24 @@ controller_interface::return_type ControllerManager::switch_controller( strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } - RCLCPP_DEBUG(get_logger(), "Activating controllers:"); + std::string activate_list, deactivate_list; + activate_list.reserve(500); + deactivate_list.reserve(500); for (const auto & controller : activate_controllers) { - RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); + activate_list.append(controller); + activate_list.append(" "); } - RCLCPP_DEBUG(get_logger(), "Deactivating controllers:"); for (const auto & controller : deactivate_controllers) { - RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); + deactivate_list.append(controller); + deactivate_list.append(" "); } + RCLCPP_INFO_EXPRESSION( + get_logger(), !activate_list.empty(), "Activating controllers: [ %s]", activate_list.c_str()); + RCLCPP_INFO_EXPRESSION( + get_logger(), !deactivate_list.empty(), "Deactivating controllers: [ %s]", + deactivate_list.c_str()); const auto list_controllers = [this, strictness]( const std::vector & controller_list, @@ -1298,19 +1318,27 @@ controller_interface::return_type ControllerManager::switch_controller( // start the atomic controller switching switch_params_.strictness = strictness; switch_params_.activate_asap = activate_asap; - switch_params_.init_time = rclcpp::Clock().now(); - switch_params_.timeout = timeout; + if (timeout == rclcpp::Duration{0, 0}) + { + RCLCPP_INFO_ONCE(get_logger(), "Switch controller timeout is set to 0, using default 1s!"); + switch_params_.timeout = std::chrono::nanoseconds(1'000'000'000); + } + else + { + switch_params_.timeout = timeout.to_chrono(); + } switch_params_.do_switch = true; - // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - while (rclcpp::ok() && switch_params_.do_switch) + std::unique_lock switch_params_guard(switch_params_.mutex, std::defer_lock); + if (!switch_params_.cv.wait_for( + switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; })) { - if (!rclcpp::ok()) - { - return controller_interface::return_type::ERROR; - } - std::this_thread::sleep_for(std::chrono::microseconds(100)); + RCLCPP_ERROR( + get_logger(), "Switch controller timed out after %f seconds!", + static_cast(switch_params_.timeout.count()) / 1e9); + clear_requests(); + return controller_interface::return_type::ERROR; } // copy the controllers spec from the used to the unused list @@ -1717,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(); @@ -2130,13 +2158,32 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & if (!ok) { std::vector stop_request = {}; + std::string failed_hardware_string; + failed_hardware_string.reserve(500); // Determine controllers to stop for (const auto & hardware_name : failed_hardware_names) { + failed_hardware_string.append(hardware_name); + failed_hardware_string.append(" "); auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); } - + std::string stop_request_string; + stop_request_string.reserve(500); + for (const auto & controller : stop_request) + { + stop_request_string.append(controller); + stop_request_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), + "Deactivating following hardware components as their read cycle resulted in an error: [ %s]", + failed_hardware_string.c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !stop_request_string.empty(), + "Deactivating following controllers as their hardware components read cycle resulted in an " + "error: [ %s]", + stop_request_string.c_str()); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); deactivate_controllers(rt_controller_list, stop_request); @@ -2146,6 +2193,12 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & void ControllerManager::manage_switch() { + std::unique_lock guard(switch_params_.mutex, std::try_to_lock); + if (!guard.owns_lock()) + { + RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle."); + return; + } // Ask hardware interfaces to change mode if (!resource_manager_->perform_command_mode_switch( activate_command_interface_request_, deactivate_command_interface_request_)) @@ -2174,6 +2227,7 @@ void ControllerManager::manage_switch() // All controllers switched --> switching done switch_params_.do_switch = false; + switch_params_.cv.notify_all(); } controller_interface::return_type ControllerManager::update( @@ -2285,13 +2339,33 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration if (!ok) { std::vector stop_request = {}; + std::string failed_hardware_string; + failed_hardware_string.reserve(500); // Determine controllers to stop for (const auto & hardware_name : failed_hardware_names) { + failed_hardware_string.append(hardware_name); + failed_hardware_string.append(" "); auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); } - + std::string stop_request_string; + stop_request_string.reserve(500); + for (const auto & controller : stop_request) + { + stop_request_string.append(controller); + stop_request_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), + "Deactivating following hardware components as their write cycle resulted in an error: [ " + "%s]", + failed_hardware_string.c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !stop_request_string.empty(), + "Deactivating following controllers as their hardware components write cycle resulted in an " + "error: [ %s]", + stop_request_string.c_str()); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); deactivate_controllers(rt_controller_list, stop_request); @@ -2655,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) @@ -2750,14 +2824,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()); } @@ -2765,15 +2855,27 @@ 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 7addaf6cee..e0094b7e01 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include #include "controller_manager/controller_manager.hpp" +#include "rclcpp/executors.hpp" #include "realtime_tools/thread_priority.hpp" using namespace std::chrono_literals; @@ -39,7 +41,21 @@ 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; + } + 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()); @@ -50,10 +66,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 diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 72326e8c6c..78cb9eaee9 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -29,6 +29,7 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" #include "std_msgs/msg/string.hpp" @@ -61,14 +62,15 @@ class ControllerManagerFixture : public ::testing::Test { public: explicit ControllerManagerFixture( - const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, + const std::string & cm_namespace = "") : robot_description_(robot_description) { executor_ = std::make_shared(); cm_ = std::make_shared( std::make_unique( rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), - executor_, TEST_CM_NAME); + executor_, TEST_CM_NAME, cm_namespace); // We want to be able to not pass robot description immediately if (!robot_description_.empty()) { 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 af04cc5406..26a5299a07 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -168,9 +168,38 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + // Switch with a very low timeout 1 ns and it should fail as there is no enough time to switch + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->switch_controller( + {}, {test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 1))); + + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(1u, result->controller.size()); + ASSERT_EQ("active", result->controller[0].state); + ASSERT_THAT( + result->controller[0].claimed_interfaces, + UnorderedElementsAre( + "joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk", + "joint1/position", "joint1/max_velocity")); + ASSERT_THAT( + result->controller[0].required_command_interfaces, + UnorderedElementsAre( + "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position", + "joint2/max_acceleration", "joint2/velocity", "joint3/velocity")); + ASSERT_THAT( + result->controller[0].required_state_interfaces, + UnorderedElementsAre( + "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface", + "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", + "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + + // Try again with higher timeout cm_->switch_controller( {}, {test_controller::TEST_CONTROLLER_NAME}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(3, 0)); result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(1u, result->controller.size()); @@ -336,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"; @@ -359,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"; @@ -383,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"; @@ -431,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) @@ -457,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()); } @@ -491,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 @@ -532,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_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 95a6efba30..892427bab7 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -11,3 +11,17 @@ chainable_ctrl_with_parameters_and_type: ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] + +/foo_namespace/ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] + +/foo_namespace/chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +/foo_namespace/ctrl_with_parameters_and_no_type: + ros__parameters: + joint_names: ["joint2"] 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_ros2_control_node.yaml b/controller_manager/test/test_ros2_control_node.yaml new file mode 100644 index 0000000000..ce0602d6b3 --- /dev/null +++ b/controller_manager/test/test_ros2_control_node.yaml @@ -0,0 +1,11 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + +ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint0"] + +joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py new file mode 100644 index 0000000000..c8c1136849 --- /dev/null +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -0,0 +1,95 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("controller_manager"), + "test", + "test_ros2_control_node.yaml", + ] + ) + + control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + ) + + return LaunchDescription([control_node, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "controller_manager" in self.node.get_node_names() + time.sleep(0.1) + assert found, "controller_manager not found!" + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shutdown. +class TestDescriptionCraneShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 7680d8270c..0812d7ffa4 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -27,6 +27,7 @@ using ::testing::_; using ::testing::Return; +const char coveragepy_script[] = "python3 -m coverage run --append --branch"; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture @@ -68,14 +69,18 @@ class TestLoadController : public ControllerManagerFixtureget_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 @@ -125,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( @@ -141,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 @@ -153,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); } } @@ -180,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); } }; @@ -244,27 +255,12 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) } } -TEST_F(TestLoadController, spawner_test_type_in_arg) -{ - // Provide controller type via -t argument - EXPECT_EQ( - call_spawner( - "ctrl_2 -c test_controller_manager -t " + - std::string(test_controller::TEST_CONTROLLER_CLASS_NAME)), - 0); - - ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); - auto ctrl_2 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); - ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); -} - 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( @@ -279,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]; @@ -289,12 +285,14 @@ 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( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path), + "ctrl_with_parameters_and_no_type -c test_controller_manager --controller-manager-timeout " + "1.0 -p " + + test_file_path), 256); // Will still be same as the current call will fail ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); @@ -302,23 +300,46 @@ 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) { // 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 " - << "ros2 run controller_manager spawner " - << "ctrl_3 -c test_controller_manager -t " - << std::string(test_controller::TEST_CONTROLLER_CLASS_NAME) << " --unload-on-kill"; + << std::string(coveragepy_script) + + " $(ros2 pkg prefix controller_manager)/lib/controller_manager/spawner " + << "ctrl_3 -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, 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"; @@ -336,21 +357,16 @@ 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_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); + 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 @@ -362,22 +378,50 @@ 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_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); + 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); + } +} + +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_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -432,7 +476,7 @@ TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spaw cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 256) + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --controller-manager-timeout 1.0"), 256) << "could not spawn controller because not robot description and not services for controller " "manager are active"; } @@ -458,6 +502,291 @@ 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); + } +} + +class TestLoadControllerWithNamespacedCM +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithNamespacedCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf, "foo_namespace") + { + } + + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + [&]() + { + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + }); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + rclcpp::TimerBase::SharedPtr update_timer_; + + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --controller-manager-timeout 1.0"), 256) + << "Should fail without defining the namespace"; + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); + + auto validate_loaded_controllers = [&]() + { + auto loaded_controllers = cm_->get_loaded_controllers(); + for (size_t i = 0; i < loaded_controllers.size(); i++) + { + auto ctrl = loaded_controllers[i]; + const std::string controller_name = "ctrl_" + std::to_string(i + 1); + + RCLCPP_ERROR(rclcpp::get_logger("test_controller_manager"), "%s", controller_name.c_str()); + auto it = std::find_if( + loaded_controllers.begin(), loaded_controllers.end(), + [&](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_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + }; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + { + validate_loaded_controllers(); + } + + // Try to spawn again multiple but one of them is already active, it should fail because already + // active + EXPECT_NE( + call_spawner("ctrl_1 ctrl_3 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0) + << "Cannot configure from active"; + + std::vector start_controllers = {}; + std::vector stop_controllers = {"ctrl_1"}; + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + + // We should be able to reconfigure and activate a configured controller + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_3 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + validate_loaded_controllers(); } + + stop_controllers = {"ctrl_1", "ctrl_2"}; + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + for (auto ctrl : stop_controllers) + { + cm_->unload_controller(ctrl); + cm_->load_controller(ctrl); + } + + // We should be able to reconfigure and activate am unconfigured loaded controller + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + validate_loaded_controllers(); + } + + // Unload and reload + EXPECT_EQ(call_unspawner("ctrl_1 ctrl_3 -c foo_namespace/test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been unloaded"; + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_3 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; + { + validate_loaded_controllers(); + } + + // Now unload everything and load them as a group in a single operation + EXPECT_EQ(call_unspawner("ctrl_1 ctrl_2 ctrl_3 -c /foo_namespace/test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded"; + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager --activate-as-group --ros-args " + "-r __ns:=/foo_namespace"), + 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; + { + validate_loaded_controllers(); + } +} + +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( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --controller-manager-timeout 1.0 -p " + + test_file_path), + 256) + << "Should fail without the namespacing it"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + test_file_path + " --ros-args -r __ns:=/foo_namespace"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + 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_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + 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_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + " --ros-args -r __ns:=/foo_namespace"), + 256) + << "Should fail as no type is defined!"; + // Will still be same as the current call will fail + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + 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_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_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F( + TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file_deprecated_namespace_arg) +{ + 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( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --controller-manager-timeout 1.0 -p " + + test_file_path), + 256) + << "Should fail without the namespacing it"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " + + test_file_path + " --ros-args -r __ns:=/random_namespace"), + 256) + << "Should fail when parsed namespace through both way with different namespaces"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" + + test_file_path + " --ros-args -r __ns:=/foo_namespace"), + 256) + << "Should fail when parsed namespace through both ways even with same namespacing name"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 0) + << "Should work when parsed through the deprecated arg"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + 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_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + 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_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 256) + << "Should fail as no type is defined!"; + // Will still be same as the current call will fail + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + 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_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_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index f6d6fad69e..56e80d736b 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- + 4.13.0 (2024-07-08) ------------------- * [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index b0bd54a0a7..195fda32f1 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.13.0 + 4.16.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/doc/debugging.rst b/doc/debugging.rst new file mode 100644 index 0000000000..76f288e49d --- /dev/null +++ b/doc/debugging.rst @@ -0,0 +1,81 @@ +Debugging +^^^^^^^^^ + +All controllers and hardware components are plugins loaded into the ``controller_manager``. Therefore, the debugger must be attached to the ``controller_manager``. If multiple ``controller_manager`` instances are running on your robot or machine, you need to attach the debugger to the ``controller_manager`` associated with the hardware component or controller you want to debug. + +How-To +****************** + +* Install ``xterm``, ``gdb`` and ``gdbserver`` on your system + + .. code-block:: bash + + sudo apt install xterm gdb gdbserver + +* Make sure you run a "debug" or "release with debug information" build: + This is done by passing ``--cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo`` to ``colcon build``. + Remember that in release builds some breakpoints might not behave as you expect as the the corresponding line might have been optimized by the compiler. For such cases, a full Debug build (``--cmake-args -DCMAKE_BUILD_TYPE=Debug``) is recommended. + +* Adapt the launch file to run the controller manager with the debugger attached: + + * Version A: Run it directly with the gdb CLI: + + Add ``prefix=['xterm -e gdb -ex run --args']`` to the ``controller_manager`` node entry in your launch file. + Due to how ``ros2launch`` works we need to run the specific node in a separate terminal instance. + + * Version B: Run it with gdbserver: + + Add ``prefix=['gdbserver localhost:3000']`` to the ``controller_manager`` node entry in your launch file. + Afterwards, you can either attach a gdb CLI instance or any IDE of your choice to that ``gdbserver`` instance. + Ensure you start your debugger from a terminal where you have sourced your workspace to properly resolve all paths. + + Example launch file entry: + + .. code-block:: python + + # Obtain the controller config file for the ros2 control node + controller_config_file = get_package_file("", "config/controllers.yaml") + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[controller_config_file], + output="both", + emulate_tty=True, + remappings=[ + ("~/robot_description", "/robot_description") + ], + prefix=['xterm -e gdb -ex run --args'] # or prefix=['gdbserver localhost:3000'] + ) + + ld.add_action(controller_manager) + + +Additional notes +***************** + +* Debugging plugins + + You can only set breakpoints in plugins after the plugin has been loaded. In the ros2_control context this means after the controller / hardware component has been loaded: + +* Debug builds + + It's often practical to include debug information only for the specific package you want to debug. + ``colcon build --packages-select [package_name] --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo`` or ``colcon build --packages-select [package_name] --cmake-args -DCMAKE_BUILD_TYPE=Debug`` + +* Realtime + + .. warning:: + + The ``update/on_activate/on_deactivate`` method of a controller and the ``read/write/on_activate/perform_command_mode_switch`` methods of a hardware component all run in the context of the realtime update loop. Setting breakpoints there can and will cause issues that might even break your hardware in the worst case. + + From experience, it might be better to use meaningful logs for the real-time context (with caution) or to add additional debug state interfaces (or publishers in the case of a controller). + +However, running the controller_manager and your plugin with gdb can still be very useful for debugging errors such as segfaults, as you can gather a full backtrace. + +References +*********** + +* `ROS 2 and GDB `_ +* `Using GDB to debug a plugin `_ +* `GDB CLI Tutorial `_ diff --git a/doc/index.rst b/doc/index.rst index b1d8e21c7d..09a2ddf745 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -28,3 +28,12 @@ Concepts Joint Kinematics <../hardware_interface/doc/joints_userdoc.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> + +===================================== +Guidelines and Best Practices +===================================== + +.. toctree:: + :titlesonly: + + Debugging the Controller Manager and Plugins diff --git a/doc/migration.rst b/doc/migration.rst index 68359f072f..255db9b8ae 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -2,6 +2,11 @@ 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. +* 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 ****************** @@ -69,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 a4a6f31734..50583b3bf4 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -21,8 +21,9 @@ For details see the controller_manager section. * Pass controller manager update rate on the init of the controller interface (`#1141 `_) * 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. +* 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. +* 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 ****************** @@ -69,6 +70,8 @@ controller_manager The parameters within the ``ros2_control`` tag are not supported any more. * The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). +* The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). +* The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). hardware_interface ****************** @@ -99,6 +102,8 @@ 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/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 76ddab43cc..405756a48c 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +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 `_) +* [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 `_) +* add missing rclcpp logging include for Humble compatibility build (`#1635 `_) +* Contributors: Sai Kishor Kothakota + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* [ResourceManager] Make destructor virtual for use in derived classes (`#1607 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.13.0 (2024-07-08) ------------------- * [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (`#1585 `_) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 3534d9f587..4fcdd93307 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -42,7 +42,8 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC explicit Actuator(std::unique_ptr impl); - Actuator(Actuator && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit Actuator(Actuator && other) noexcept; ~Actuator() = default; @@ -92,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); @@ -102,6 +103,7 @@ class Actuator final private: std::unique_ptr impl_; + mutable std::recursive_mutex actuators_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index b637068fe3..d88fcd1f2d 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -220,15 +220,17 @@ 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; + } -protected: /// Get the logger of the ActuatorInterface. /** * \return logger of the ActuatorInterface. @@ -241,6 +243,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + /// Get the hardware info of the ActuatorInterface. + /** + * \return hardware info of the ActuatorInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; 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/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 dc536e51be..07ba1cd98e 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,52 +15,78 @@ #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 { + +using HANDLE_DATATYPE = std::variant; + /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + 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 ReadOnlyHandle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + explicit Handle(const InterfaceDescription & interface_description) + : prefix_name_(interface_description.prefix_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 + value_ = std::numeric_limits::quiet_NaN(); + value_ptr_ = std::get_if(&value_); } - explicit ReadOnlyHandle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + explicit Handle(const std::string & interface_name) + : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + [[deprecated("Use InterfaceDescription for initializing the Interface")]] - ReadOnlyHandle(ReadOnlyHandle && other) = default; + explicit Handle(const char * interface_name) + : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr) + { + } + + Handle(const Handle & other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + Handle(Handle && other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + Handle & operator=(const Handle & other) = default; - virtual ~ReadOnlyHandle() = default; + Handle & operator=(Handle && other) = default; + + virtual ~Handle() = default; /// 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(); @@ -70,60 +96,55 @@ 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_; + 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 double * value_ptr_; + // END }; -class ReadWriteHandle : public ReadOnlyHandle +class StateInterface : public Handle { public: - ReadWriteHandle( - 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) + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { - 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 InterfaceDescription & interface_description) + : Handle(interface_description) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -134,7 +155,7 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using Handle::Handle; }; } // namespace hardware_interface 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/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 4126910d44..e1490c91fb 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -42,7 +42,8 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC explicit Sensor(std::unique_ptr impl); - Sensor(Sensor && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit Sensor(Sensor && other) noexcept; ~Sensor() = default; @@ -79,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); @@ -89,6 +90,7 @@ class Sensor final private: std::unique_ptr impl_; + mutable std::recursive_mutex sensors_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2cff8b2a3a..4e9ac48d5d 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -159,15 +159,17 @@ 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; + } -protected: /// Get the logger of the SensorInterface. /** * \return logger of the SensorInterface. @@ -180,6 +182,13 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + /// Get the hardware info of the SensorInterface. + /** + * \return hardware info of the SensorInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 9308872ef8..34056c982e 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -42,7 +42,8 @@ class System final HARDWARE_INTERFACE_PUBLIC explicit System(std::unique_ptr impl); - System(System && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit System(System && other) noexcept; ~System() = default; @@ -92,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); @@ -102,6 +103,7 @@ class System final private: std::unique_ptr impl_; + mutable std::recursive_mutex system_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index f9173fb754..37745b36fb 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -25,6 +25,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -221,15 +222,17 @@ 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; + } -protected: /// Get the logger of the SystemInterface. /** * \return logger of the SystemInterface. @@ -242,6 +245,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + /// Get the hardware info of the SystemInterface. + /** + * \return hardware info of the SystemInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index a3a1d6c77a..8c19535675 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.13.0 + 4.16.1 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 66419402f8..d586b463af 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -35,156 +36,169 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} +Actuator::Actuator(Actuator && other) noexcept +{ + std::lock_guard lock(other.actuators_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & Actuator::initialize( const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + std::unique_lock lock(actuators_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + std::unique_lock lock(actuators_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + std::unique_lock lock(actuators_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + std::unique_lock lock(actuators_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + std::unique_lock lock(actuators_mutex_); + 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() { - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + std::unique_lock lock(actuators_mutex_); + 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() @@ -217,20 +231,31 @@ 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) { + std::unique_lock lock(actuators_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping read() call for actuator '%s' since it is locked", + impl_->get_name().c_str()); + 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) @@ -243,16 +268,24 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(actuators_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping write() call for actuator '%s' since it is locked", + impl_->get_name().c_str()); + 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/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/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index abe36ba262..8b46d21f4d 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -59,8 +59,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i }; // check if to create mock command interface for sensor - auto it = info_.hardware_parameters.find("mock_sensor_commands"); - if (it != info_.hardware_parameters.end()) + auto it = get_hardware_info().hardware_parameters.find("mock_sensor_commands"); + if (it != get_hardware_info().hardware_parameters.end()) { use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); } @@ -70,8 +70,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // check if to create mock command interface for gpio - it = info_.hardware_parameters.find("mock_gpio_commands"); - if (it != info_.hardware_parameters.end()) + it = get_hardware_info().hardware_parameters.find("mock_gpio_commands"); + if (it != get_hardware_info().hardware_parameters.end()) { use_mock_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); } @@ -82,7 +82,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // check if there is parameter that disables commands // this way we simulate disconnected driver - it = info_.hardware_parameters.find("disable_commands"); + it = get_hardware_info().hardware_parameters.find("disable_commands"); if (it != info.hardware_parameters.end()) { command_propagation_disabled_ = hardware_interface::parse_bool(it->second); @@ -93,7 +93,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // check if there is parameter that enables dynamic calculation - it = info_.hardware_parameters.find("calculate_dynamics"); + it = get_hardware_info().hardware_parameters.find("calculate_dynamics"); if (it != info.hardware_parameters.end()) { calculate_dynamics_ = hardware_interface::parse_bool(it->second); @@ -107,12 +107,12 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; - it = info_.hardware_parameters.find("position_state_following_offset"); - if (it != info_.hardware_parameters.end()) + it = get_hardware_info().hardware_parameters.find("position_state_following_offset"); + if (it != get_hardware_info().hardware_parameters.end()) { position_state_following_offset_ = hardware_interface::stod(it->second); - it = info_.hardware_parameters.find("custom_interface_with_following_offset"); - if (it != info_.hardware_parameters.end()) + it = get_hardware_info().hardware_parameters.find("custom_interface_with_following_offset"); + if (it != get_hardware_info().hardware_parameters.end()) { custom_interface_with_following_offset_ = it->second; } @@ -121,9 +121,10 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i index_custom_interface_with_following_offset_ = std::numeric_limits::max(); // Initialize storage for standard interfaces - initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_, info_.joints); + initialize_storage_vectors( + joint_commands_, joint_states_, standard_interfaces_, get_hardware_info().joints); // set all values without initial values to 0 - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { for (auto j = 0u; j < standard_interfaces_.size(); j++) { @@ -135,7 +136,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // search for non-standard joint interfaces - for (const auto & joint : info_.joints) + for (const auto & joint : get_hardware_info().joints) { // populate non-standard command interfaces to other_interfaces_ populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_); @@ -145,7 +146,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // Initialize storage for non-standard interfaces - initialize_storage_vectors(other_commands_, other_states_, other_interfaces_, info_.joints); + initialize_storage_vectors( + other_commands_, other_states_, other_interfaces_, get_hardware_info().joints); // when following offset is used on custom interface then find its index if (!custom_interface_with_following_offset_.empty()) @@ -170,7 +172,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - for (const auto & sensor : info_.sensors) + for (const auto & sensor : get_hardware_info().sensors) { for (const auto & interface : sensor.state_interfaces) { @@ -183,10 +185,10 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } initialize_storage_vectors( - sensor_mock_commands_, sensor_states_, sensor_interfaces_, info_.sensors); + sensor_mock_commands_, sensor_states_, sensor_interfaces_, get_hardware_info().sensors); // search for gpio interfaces - for (const auto & gpio : info_.gpios) + for (const auto & gpio : get_hardware_info().gpios) { // populate non-standard command interfaces to gpio_interfaces_ populate_non_standard_interfaces(gpio.command_interfaces, gpio_interfaces_); @@ -198,12 +200,14 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // Mock gpio command interfaces if (use_mock_gpio_command_interfaces_) { - initialize_storage_vectors(gpio_mock_commands_, gpio_states_, gpio_interfaces_, info_.gpios); + initialize_storage_vectors( + gpio_mock_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); } // Real gpio command interfaces else { - initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_, info_.gpios); + initialize_storage_vectors( + gpio_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); } return CallbackReturn::SUCCESS; @@ -214,9 +218,9 @@ std::vector GenericSystem::export_state_inte std::vector state_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { - const auto & joint = info_.joints[i]; + const auto & joint = get_hardware_info().joints[i]; for (const auto & interface : joint.state_interfaces) { // Add interface: if not in the standard list then use "other" interface list @@ -236,14 +240,15 @@ std::vector GenericSystem::export_state_inte // Sensor state interfaces if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) + get_hardware_info().sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); }; // GPIO state interfaces - if (!populate_interfaces(info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) + if (!populate_interfaces( + get_hardware_info().gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) { throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); } @@ -256,9 +261,9 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (size_t i = 0; i < info_.joints.size(); ++i) + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) { - const auto & joint = info_.joints[i]; + const auto & joint = get_hardware_info().joints[i]; for (const auto & interface : joint.command_interfaces) { // Add interface: if not in the standard list than use "other" interface list @@ -278,13 +283,14 @@ std::vector GenericSystem::export_command_ } } // Set position control mode per default - joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); + joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX); // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) + get_hardware_info().sensors, sensor_interfaces_, sensor_mock_commands_, + command_interfaces, true)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); @@ -295,7 +301,8 @@ std::vector GenericSystem::export_command_ if (use_mock_gpio_command_interfaces_) { if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) + get_hardware_info().gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, + true)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -305,7 +312,7 @@ std::vector GenericSystem::export_command_ else { if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) + get_hardware_info().gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -328,29 +335,30 @@ return_type GenericSystem::prepare_command_mode_switch( const size_t FOUND_ONCE_FLAG = 1000000; + const auto & info = get_hardware_info(); std::vector joint_found_in_x_requests_; - joint_found_in_x_requests_.resize(info_.joints.size(), 0); + joint_found_in_x_requests_.resize(info.joints.size(), 0); for (const auto & key : start_interfaces) { // check if interface is joint auto joint_it_found = std::find_if( - info_.joints.begin(), info_.joints.end(), + info.joints.begin(), info.joints.end(), [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); - if (joint_it_found != info_.joints.end()) + if (joint_it_found != info.joints.end()) { - const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + const size_t joint_index = std::distance(info.joints.begin(), joint_it_found); if (joint_found_in_x_requests_[joint_index] == 0) { joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_found_in_x_requests_[joint_index] += 1; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { if (!calculate_dynamics_) { @@ -358,11 +366,11 @@ return_type GenericSystem::prepare_command_mode_switch( get_logger(), "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " "might lead to wrong feedback and unexpected behavior.", - info_.joints[joint_index].name.c_str()); + info.joints[joint_index].name.c_str()); } joint_found_in_x_requests_[joint_index] += 1; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { if (!calculate_dynamics_) { @@ -370,7 +378,7 @@ return_type GenericSystem::prepare_command_mode_switch( get_logger(), "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " "this might lead to wrong feedback and unexpected behavior.", - info_.joints[joint_index].name.c_str()); + info.joints[joint_index].name.c_str()); } joint_found_in_x_requests_[joint_index] += 1; } @@ -382,14 +390,14 @@ return_type GenericSystem::prepare_command_mode_switch( } } - for (size_t i = 0; i < info_.joints.size(); ++i) + for (size_t i = 0; i < info.joints.size(); ++i) { // There has to always be at least one control mode from the above three set if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) { RCLCPP_ERROR( get_logger(), "Joint '%s' has to have '%s', '%s', or '%s' interface!", - info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, + info.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); ret_val = hardware_interface::return_type::ERROR; } @@ -401,7 +409,7 @@ return_type GenericSystem::prepare_command_mode_switch( get_logger(), "Got multiple (%zu) starting interfaces for joint '%s' - this is not " "supported!", - joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info.joints[i].name.c_str()); ret_val = hardware_interface::return_type::ERROR; } } @@ -421,23 +429,24 @@ return_type GenericSystem::perform_command_mode_switch( for (const auto & key : start_interfaces) { // check if interface is joint + const auto & info = get_hardware_info(); auto joint_it_found = std::find_if( - info_.joints.begin(), info_.joints.end(), + info.joints.begin(), info.joints.end(), [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); - if (joint_it_found != info_.joints.end()) + if (joint_it_found != info.joints.end()) { - const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + const size_t joint_index = std::distance(info.joints.begin(), joint_it_found); - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; } @@ -565,7 +574,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::ERROR; } - for (const auto & mimic_joint : info_.mimic_joints) + for (const auto & mimic_joint : get_hardware_info().mimic_joints) { for (auto i = 0u; i < joint_states_.size(); ++i) { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4c990526fc..72e966fcab 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -35,6 +35,7 @@ #include "hardware_interface/system_interface.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" +#include "rclcpp/logging.hpp" #include "rcutils/logging_macros.h" namespace hardware_interface @@ -513,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; @@ -537,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); @@ -557,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); @@ -581,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); @@ -1461,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(); } }; @@ -1545,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 { @@ -1610,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 { @@ -1769,7 +1773,6 @@ void ResourceManager::shutdown_async_components() HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) { - std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); @@ -1830,7 +1833,6 @@ HardwareReadWriteStatus ResourceManager::read( HardwareReadWriteStatus ResourceManager::write( const rclcpp::Time & time, const rclcpp::Duration & period) { - std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9b7f1f69d6..2cffa649fd 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -34,156 +35,169 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} +Sensor::Sensor(Sensor && other) noexcept +{ + std::lock_guard lock(other.sensors_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & Sensor::initialize( const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + std::unique_lock lock(sensors_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + std::unique_lock lock(sensors_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + std::unique_lock lock(sensors_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + std::unique_lock lock(sensors_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + std::unique_lock lock(sensors_mutex_); + 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() { - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + std::unique_lock lock(sensors_mutex_); + 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() @@ -195,20 +209,31 @@ 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) { + std::unique_lock lock(sensors_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping read() call for the sensor '%s' since it is locked", + impl_->get_name().c_str()); + 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 ba327d8ab2..8f455a7bd5 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -34,156 +34,169 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} +System::System(System && other) noexcept +{ + std::lock_guard lock(other.system_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & System::initialize( const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + std::unique_lock lock(system_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + std::unique_lock lock(system_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + std::unique_lock lock(system_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + std::unique_lock lock(system_mutex_); + 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() { - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + std::unique_lock lock(system_mutex_); + 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() { - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + std::unique_lock lock(system_mutex_); + 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() @@ -214,20 +227,31 @@ 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) { + std::unique_lock lock(system_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping read() call for system '%s' since it is locked", + impl_->get_name().c_str()); + 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) @@ -240,16 +264,24 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(system_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping write() call for system '%s' since it is locked", + impl_->get_name().c_str()); + 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()); 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); +} diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index ff57d6a63d..acec0e5917 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -33,7 +33,7 @@ class TestForceTorqueSensor : public SensorInterface return CallbackReturn::ERROR; } - const auto & state_interfaces = info_.sensors[0].state_interfaces; + const auto & state_interfaces = get_hardware_info().sensors[0].state_interfaces; if (state_interfaces.size() != 6) { return CallbackReturn::ERROR; @@ -57,7 +57,7 @@ class TestForceTorqueSensor : public SensorInterface { std::vector state_interfaces; - const auto & sensor_name = info_.sensors[0].name; + const auto & sensor_name = get_hardware_info().sensors[0].name; state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "fx", &values_.fx)); state_interfaces.emplace_back( diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp index bb41f16228..0bd9e9121c 100644 --- a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -38,7 +38,7 @@ class TestIMUSensor : public SensorInterface return CallbackReturn::ERROR; } - const auto & state_interfaces = info_.sensors[0].state_interfaces; + const auto & state_interfaces = get_hardware_info().sensors[0].state_interfaces; if (state_interfaces.size() != 10) { return CallbackReturn::ERROR; @@ -65,7 +65,7 @@ class TestIMUSensor : public SensorInterface { std::vector state_interfaces; - const std::string & sensor_name = info_.sensors[0].name; + const std::string & sensor_name = get_hardware_info().sensors[0].name; state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "orientation.x", &orientation_.x)); state_interfaces.emplace_back( diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index dacbf308fa..eb61c3de5a 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -34,12 +34,12 @@ class TestSingleJointActuator : public ActuatorInterface } // can only control one joint - if (info_.joints.size() != 1) + if (get_hardware_info().joints.size() != 1) { return CallbackReturn::ERROR; } // can only control in position - const auto & command_interfaces = info_.joints[0].command_interfaces; + const auto & command_interfaces = get_hardware_info().joints[0].command_interfaces; if (command_interfaces.size() != 1) { return CallbackReturn::ERROR; @@ -49,7 +49,7 @@ class TestSingleJointActuator : public ActuatorInterface return CallbackReturn::ERROR; } // can only give feedback state for position and velocity - const auto & state_interfaces = info_.joints[0].state_interfaces; + const auto & state_interfaces = get_hardware_info().joints[0].state_interfaces; if (state_interfaces.size() < 1) { return CallbackReturn::ERROR; @@ -71,7 +71,7 @@ class TestSingleJointActuator : public ActuatorInterface { std::vector state_interfaces; - const auto & joint_name = info_.joints[0].name; + const auto & joint_name = get_hardware_info().joints[0].name; state_interfaces.emplace_back(hardware_interface::StateInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_state_)); state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -84,7 +84,7 @@ class TestSingleJointActuator : public ActuatorInterface { std::vector command_interfaces; - const auto & joint_name = info_.joints[0].name; + const auto & joint_name = get_hardware_info().joints[0].name; command_interfaces.emplace_back(hardware_interface::CommandInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_command_)); diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index aba2f86fe5..a511344f52 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -32,11 +32,11 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface } // Can only control two joints - if (info_.joints.size() != 2) + if (get_hardware_info().joints.size() != 2) { return CallbackReturn::ERROR; } - for (const hardware_interface::ComponentInfo & joint : info_.joints) + for (const hardware_interface::ComponentInfo & joint : get_hardware_info().joints) { // Can control in position or velocity const auto & command_interfaces = joint.command_interfaces; @@ -77,14 +77,17 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector export_state_interfaces() override { std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, + &velocity_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_ACCELERATION, + &acceleration_state_[i])); } return state_interfaces; @@ -93,12 +96,14 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector export_command_interfaces() override { std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_command_[i])); command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, + &velocity_command_[i])); } return command_interfaces; @@ -127,20 +132,20 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface stop_modes_.clear(); for (const auto & key : start_interfaces) { - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) + if (key == get_hardware_info().joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { start_modes_.push_back(hardware_interface::HW_IF_POSITION); } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) + if (key == get_hardware_info().joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } } } // Example Criteria 1 - Starting: All interfaces must be given a new mode at the same time - if (start_modes_.size() != 0 && start_modes_.size() != info_.joints.size()) + if (start_modes_.size() != 0 && start_modes_.size() != get_hardware_info().joints.size()) { return hardware_interface::return_type::ERROR; } @@ -148,9 +153,9 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface // Stopping interfaces for (const auto & key : stop_interfaces) { - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { - if (key.find(info_.joints[i].name) != std::string::npos) + if (key.find(get_hardware_info().joints[i].name) != std::string::npos) { stop_modes_.push_back(true); } diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index 5942f14d42..7bc06a3e1e 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -35,11 +35,11 @@ class TestTwoJointSystem : public SystemInterface } // can only control two joint - if (info_.joints.size() != 2) + if (get_hardware_info().joints.size() != 2) { return CallbackReturn::ERROR; } - for (const auto & joint : info_.joints) + for (const auto & joint : get_hardware_info().joints) { // can only control in position const auto & command_interfaces = joint.command_interfaces; @@ -70,10 +70,11 @@ class TestTwoJointSystem : public SystemInterface std::vector export_state_interfaces() override { std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_state_[i])); } return state_interfaces; @@ -82,10 +83,11 @@ class TestTwoJointSystem : public SystemInterface std::vector export_command_interfaces() override { std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_command_[i])); } return command_interfaces; diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 520f28540d..f1cd9531d0 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- +* [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) +* Contributors: Sai Kishor Kothakota + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Fix typos in test_resource_manager.cpp (`#1609 `_) +* Contributors: Henry Moore, Parker Drouillard + 4.13.0 (2024-07-08) ------------------- * [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (`#1585 `_) diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index f65de8598d..a88ae6ba93 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.13.0 + 4.16.1 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index b7149369ed..86debc1f4d 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -31,7 +31,7 @@ class TestActuator : public ActuatorInterface return CallbackReturn::ERROR; } - if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") { return CallbackReturn::ERROR; } @@ -40,12 +40,12 @@ class TestActuator : public ActuatorInterface * a hardware can optional prove for incorrect info here. * * // can only control one joint - * if (info_.joints.size() != 1) {return CallbackReturn::ERROR;} + * if (get_hardware_info().joints.size() != 1) {return CallbackReturn::ERROR;} * // can only control in position - * if (info_.joints[0].command_interfaces.size() != 1) {return + * if (get_hardware_info().joints[0].command_interfaces.size() != 1) {return * CallbackReturn::ERROR;} * // can only give feedback state for position and velocity - * if (info_.joints[0].state_interfaces.size() != 2) {return + * if (get_hardware_info().joints[0].state_interfaces.size() != 2) {return * CallbackReturn::ERROR;} */ @@ -56,11 +56,13 @@ class TestActuator : public ActuatorInterface { std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[0].name, &position_state_)); + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[0].name, + &position_state_)); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[1].name, &velocity_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(info_.joints[0].name, "some_unlisted_interface", nullptr)); + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[1].name, + &velocity_state_)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[0].name, "some_unlisted_interface", nullptr)); return state_interfaces; } @@ -69,12 +71,14 @@ class TestActuator : public ActuatorInterface { std::vector command_interfaces; command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[0].name, &velocity_command_)); + get_hardware_info().joints[0].name, get_hardware_info().joints[0].command_interfaces[0].name, + &velocity_command_)); - if (info_.joints[0].command_interfaces.size() > 1) + if (get_hardware_info().joints[0].command_interfaces.size() > 1) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_velocity_command_)); + get_hardware_info().joints[0].name, + get_hardware_info().joints[0].command_interfaces[1].name, &max_velocity_command_)); } return command_interfaces; diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 7f5a476c14..16692f55d9 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -29,7 +29,7 @@ class TestSensor : public SensorInterface return CallbackReturn::ERROR; } // can only give feedback state for velocity - if (info_.sensors[0].state_interfaces.size() == 2) + if (get_hardware_info().sensors[0].state_interfaces.size() == 2) { return CallbackReturn::ERROR; } @@ -40,7 +40,8 @@ class TestSensor : public SensorInterface { std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[0].name, &velocity_state_)); + get_hardware_info().sensors[0].name, get_hardware_info().sensors[0].state_interfaces[0].name, + &velocity_state_)); return state_interfaces; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 6724a1d019..e30b74488e 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -34,7 +34,7 @@ class TestSystem : public SystemInterface } // Simulating initialization error - if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") { return CallbackReturn::ERROR; } @@ -44,22 +44,23 @@ class TestSystem : public SystemInterface std::vector export_state_interfaces() override { + const auto info = get_hardware_info(); std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < info.joints.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + info.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); + info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + info.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); } - if (info_.gpios.size() > 0) + if (info.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_)); + info.gpios[0].name, info.gpios[0].state_interfaces[0].name, &configuration_state_)); } return state_interfaces; @@ -67,22 +68,22 @@ class TestSystem : public SystemInterface std::vector export_command_interfaces() override { + const auto info = get_hardware_info(); std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < info.joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); } // Add max_acceleration command interface command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, - &max_acceleration_command_)); + info.joints[0].name, info.joints[0].command_interfaces[1].name, &max_acceleration_command_)); - if (info_.gpios.size() > 0) + if (info.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_)); + info.gpios[0].name, info.gpios[0].command_interfaces[0].name, &configuration_command_)); } return command_interfaces; diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 5fea915b6b..02f6673e88 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + 4.13.0 (2024-07-08) ------------------- * [JointLimits] Add Saturation Joint Limiter that uses clamping method (`#971 `_) diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 6e8f35255d..8305cff9e5 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.13.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 0861761188..f522eda772 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- + 4.13.0 (2024-07-08) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index c7687e3cd6..a8c423658d 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.13.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 7dac855794..d5e898924b 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + 4.13.0 (2024-07-08) ------------------- * [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 1912391b5a..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.13.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 7ed24372b0..639a295a06 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +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 + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- + 4.13.0 (2024-07-08) ------------------- * Remove ament linters (`#1601 `_) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index d1693cdf2a..176334ad54 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.13.0 + 4.16.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 31ce814865..b4ebff94cd 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 @@ -22,7 +21,7 @@ from ros2controlcli.api import add_controller_mgr_parsers -def print_controller_state(c, args): +def print_controller_state(c, args, col_width_name, col_width_state, col_width_type): state_color = "" if c.state == "active": state_color = bcolors.OKGREEN @@ -31,7 +30,9 @@ def print_controller_state(c, args): elif c.state == "unconfigured": state_color = bcolors.WARNING - print(f"{c.name:20s}[{c.type:20s}] {state_color}{c.state:10s}{bcolors.ENDC}") + print( + f"{state_color}{c.name:<{col_width_name}}{bcolors.ENDC} {c.type:<{col_width_type}} {state_color}{c.state:<{col_width_state}}{bcolors.ENDC}" + ) if args.claimed_interfaces or args.verbose: print("\tclaimed interfaces:") for claimed_interface in c.claimed_interfaces: @@ -96,7 +97,17 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with NodeStrategy(args) as node: response = list_controllers(node, args.controller_manager) + + if not response.controller: + print("No controllers are currently loaded!") + return 0 + + # Structure data as table for nicer output + col_width_name = max(len(ctrl.name) for ctrl in response.controller) + col_width_type = max(len(ctrl.type) for ctrl in response.controller) + col_width_state = max(len(ctrl.state) for ctrl in response.controller) + for c in response.controller: - print_controller_state(c, args) + print_controller_state(c, args, col_width_name, col_width_state, col_width_type) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index 8a5884f2cb..e3e2269920 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -12,8 +12,9 @@ # 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 from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -39,17 +40,26 @@ def main(self, *, args): hardware_components = list_hardware_components(node, args.controller_manager) for idx, component in enumerate(hardware_components.component): + # Set activity color for nicer visualization + activity_color = bcolors.FAIL + if component.state.id == State.PRIMARY_STATE_UNCONFIGURED: + activity_color = bcolors.WARNING + if component.state.id == State.PRIMARY_STATE_INACTIVE: + activity_color = bcolors.MAGENTA + if component.state.id == State.PRIMARY_STATE_ACTIVE: + activity_color = bcolors.OKGREEN + print( - f"Hardware Component {idx+1}\n\tname: {component.name}\n\ttype: {component.type}" + f"Hardware Component {idx+1}\n\tname: {activity_color}{component.name}{bcolors.ENDC}\n\ttype: {component.type}" ) if hasattr(component, "plugin_name"): - plugin_name = component.plugin_name + plugin_name = f"{component.plugin_name}" else: plugin_name = f"{bcolors.WARNING}plugin name missing!{bcolors.ENDC}" print( f"\tplugin name: {plugin_name}\n" - f"\tstate: id={component.state.id} label={component.state.label}\n" + f"\tstate: id={component.state.id} label={activity_color}{component.state.label}{bcolors.ENDC}\n" f"\tcommand interfaces" ) for cmd_interface in component.command_interfaces: 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 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 5205c0f024..c0395ec63d 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.13.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 065861ca07..abfab9c3c3 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +4.16.0 (2024-08-22) +------------------- + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- + 4.13.0 (2024-07-08) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index e4356b90fe..90e9fd1462 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.13.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 69d38c7273..cd0b3141a7 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.13.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 63bd6aeb89..523f72c1ee 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.16.1 (2024-08-24) +------------------- + +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 + +4.15.0 (2024-08-05) +------------------- + +4.14.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + 4.13.0 (2024-07-08) ------------------- diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index a8757e9c02..107133128c 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -289,7 +289,7 @@ inline void FourBarLinkageTransmission::actuator_to_joint() joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); joint_eff[1].set_value( - jr[1] * (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); + jr[1] * (act_eff[1].get_value() * ar[1] - jr[0] * act_eff[0].get_value() * ar[0])); } } 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 diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 726ddc70cc..6a982ddef9 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.13.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 diff --git a/transmission_interface/test/random_generator_utils.hpp b/transmission_interface/test/random_generator_utils.hpp index fbacd1ba3b..01e1245e48 100644 --- a/transmission_interface/test/random_generator_utils.hpp +++ b/transmission_interface/test/random_generator_utils.hpp @@ -26,12 +26,14 @@ using std::vector; /// \brief Generator of pseudo-random double in the range [min_val, max_val]. // NOTE: Based on example code available at: // http://stackoverflow.com/questions/2860673/initializing-a-c-vector-to-random-values-fast +// Use a user specified seed instead of system time for deterministic tests struct RandomDoubleGenerator { public: - RandomDoubleGenerator(double min_val, double max_val) : min_val_(min_val), max_val_(max_val) + RandomDoubleGenerator(double min_val, double max_val, unsigned int seed = 1234) + : min_val_(min_val), max_val_(max_val) { - srand(static_cast(time(nullptr))); + srand(seed); } double operator()() {