diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index fc78e25e34..61a5f1f29c 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Humble - ABI Compatibility Check on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index e10120d18f..a07fecc660 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Rolling - ABI Compatibility Check on: workflow_dispatch: - branches: - - master pull_request: branches: - master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 35e7a034cb..49ad7afbb7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -36,7 +36,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.16.0 + rev: v3.17.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -132,7 +132,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 07048edc70..f0b17d558b 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- +* [ControllerInterface] Avoid warning about conversion from `int64_t` to `unsigned int` (backport `#1173 `_) (`#1631 `_) +* Fix dependencies for source build (`#1533 `_) (`#1535 `_) +* Contributors: mergify[bot] + 2.41.0 (2024-04-30) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 2f615c1529..7f9869851c 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 2.41.0 + 2.42.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 035d2572e1..26c8c41cf6 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() // 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) { - update_rate_ = get_node()->get_parameter("update_rate").as_int(); + update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); } return get_node()->configure(); diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 8ce63b6415..b1ecc019f8 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- +* Remove noqa (`#1626 `_) (`#1628 `_) +* Bump version of pre-commit hooks (backport `#1556 `_) (`#1557 `_) +* Contributors: mergify[bot] + 2.41.0 (2024-04-30) ------------------- * check for state of the controller node before cleanup (backport `#1363 `_) (`#1378 `_) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 29d489ad81..53999d4f70 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -169,11 +169,15 @@ if(BUILD_TESTING) ament_add_gmock( test_spawner_unspawner test/test_spawner_unspawner.cpp + TIMEOUT 120 ) target_include_directories(test_spawner_unspawner PRIVATE include) target_link_libraries(test_spawner_unspawner ${PROJECT_NAME} test_controller) ament_target_dependencies(test_spawner_unspawner ros2_control_test_assets) + install(FILES test/test_controller_spawner_with_type.yaml + DESTINATION test) + ament_add_gmock( test_hardware_management_srvs test/test_hardware_management_srvs.cpp @@ -185,6 +189,11 @@ if(BUILD_TESTING) controller_manager_msgs ros2_control_test_assets ) + + 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 Python modules diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index cbc40541b6..e65413e37c 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -28,15 +28,20 @@ import rclpy -def service_caller(node, service_name, service_type, request, service_timeout=10.0): +class ServiceNotFoundError(Exception): + pass + + +def service_caller(node, service_name, service_type, request, service_timeout=0.0): 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) @@ -47,7 +52,7 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 raise RuntimeError(f"Exception while calling service: {future.exception()}") -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 +64,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 +75,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 +86,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 +97,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 +108,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 +120,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( @@ -127,7 +132,9 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi ) -def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state): +def set_hardware_component_state( + node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 +): request = SetHardwareComponentState.Request() request.name = component_name request.target_state = lifecyle_state @@ -162,7 +169,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( diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 13c004082e..3e3a487c6a 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -15,13 +15,15 @@ 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, +) +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 @@ -43,18 +45,6 @@ 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: - result = function() - if result: - 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 @@ -76,35 +66,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( @@ -168,10 +134,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", @@ -203,13 +168,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) @@ -219,6 +186,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 5a23c02cec..e3ef28e928 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -29,7 +29,7 @@ def generate_load_controller_launch_description( 'unload_on_kill' LaunchArguments and a Node action that runs the controller_manager spawner node to load and activate a controller - Examples # noqa: D416 + Examples -------- # Assuming the controller type and controller parameters are known to the controller_manager generate_load_controller_launch_description('joint_state_broadcaster') diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 6f4bb19892..0dd66b840a 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,6 +19,7 @@ import sys import time import warnings +import yaml from controller_manager import ( configure_controller, @@ -27,10 +28,10 @@ switch_controllers, unload_controller, ) +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 from rclpy.signals import SignalHandlerOptions from ros2param.api import call_set_parameters @@ -55,18 +56,6 @@ 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: - result = function() - if result: - 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 @@ -87,47 +76,27 @@ 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", - ) +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) - # 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", +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}" ) - - return False - - -def is_controller_loaded(node, controller_manager, controller_name): - controllers = list_controllers(node, controller_manager).controller - return any(c.name == controller_name for c in controllers) + parameters = yaml.safe_load(f) + if namespaced_controller in parameters: + value = parameters[namespaced_controller] + if not isinstance(value, dict) or "ros__parameters" not in value: + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" + ) + if parameter_name in parameters[namespaced_controller]["ros__parameters"]: + return parameters[namespaced_controller]["ros__parameters"][parameter_name] + else: + return None def main(args=None): @@ -186,8 +155,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", @@ -201,9 +170,7 @@ def main(args=None): 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_type = args.controller_type controller_manager_timeout = args.controller_manager_timeout if param_file and not os.path.isfile(param_file): @@ -211,35 +178,44 @@ def main(args=None): 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!" + ) + + spawner_namespace = args.namespace if args.namespace else node.get_namespace() + + 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("Controller manager not available") - return 1 - for controller_name in controller_names: - 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: + controller_type = ( + args.controller_type + if param_file is None + else get_parameter_from_param_file( + controller_name, spawner_namespace, param_file, "type" + ) + ) if controller_type: parameter = Parameter() - parameter.name = prefixed_controller_name + ".type" + parameter.name = controller_name + ".type" parameter.value = get_parameter_value(string_value=controller_type) response = call_set_parameters( @@ -254,7 +230,7 @@ def main(args=None): + controller_type + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -264,14 +240,14 @@ def main(args=None): + controller_type + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 if param_file: parameter = Parameter() - parameter.name = prefixed_controller_name + ".params_file" + parameter.name = controller_name + ".params_file" parameter.value = get_parameter_value(string_value=param_file) response = call_set_parameters( @@ -286,7 +262,7 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -296,7 +272,7 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 @@ -307,16 +283,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: @@ -341,7 +313,7 @@ def main(args=None): bcolors.OKGREEN + "Configured and activated " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) @@ -393,6 +365,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/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 28d94dfe58..d7cab49cbd 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 @@ -58,6 +59,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 9c2e3a5990..8f00bef4b9 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -81,6 +81,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`` @@ -127,6 +128,26 @@ 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. @@ -142,6 +163,7 @@ It can be launched independently using the following command or as rqt plugin. * 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 b74eb1557b..4f28bf933b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -509,14 +509,25 @@ class ControllerManager : public rclcpp::Node 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 fd81ba6193..2bacfd6289 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 2.41.0 + 2.42.0 Description of controller_manager Bence Magyar Denis Štogl @@ -28,6 +28,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 f0fc8b11d3..7ec2216588 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -821,6 +821,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 @@ -840,7 +841,8 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { - switch_params_ = SwitchParams(); + // reset the switch param internal variables + switch_params_.reset(); if (!deactivate_request_.empty() || !activate_request_.empty()) { @@ -1201,19 +1203,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 @@ -1306,6 +1316,12 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co 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_)) @@ -1330,6 +1346,9 @@ void ControllerManager::manage_switch() } // TODO(destogl): move here "do_switch = false" + + switch_params_.do_switch = false; + switch_params_.cv.notify_all(); } void ControllerManager::deactivate_controllers() diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 5e6e02029f..9b38200f1c 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -67,7 +67,7 @@ class ControllerManagerFixture : public ::testing::Test public: explicit ControllerManagerFixture( const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, - const bool & pass_urdf_as_parameter = false) + const bool & pass_urdf_as_parameter = false, const std::string & cm_namespace = "") : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) { executor_ = std::make_shared(); @@ -75,7 +75,8 @@ class ControllerManagerFixture : public ::testing::Test if (robot_description_.empty()) { cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); + std::make_unique(), executor_, TEST_CM_NAME, + cm_namespace); } else { @@ -85,7 +86,7 @@ class ControllerManagerFixture : public ::testing::Test { cm_ = std::make_shared( std::make_unique(robot_description_, true, true), - executor_, TEST_CM_NAME); + executor_, TEST_CM_NAME, cm_namespace); } else { @@ -94,11 +95,10 @@ class ControllerManagerFixture : public ::testing::Test // this is just a workaround to skip passing cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); + std::make_unique(), executor_, TEST_CM_NAME, + cm_namespace); // mimic topic call - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; - cm_->robot_description_callback(msg); + pass_robot_description_to_cm_and_rm(robot_description_); } } } @@ -134,6 +134,17 @@ class ControllerManagerFixture : public ::testing::Test } } + void pass_robot_description_to_cm_and_rm( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + { + // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // not receive msg. Have to check this... + // this is just a workaround to skip passing - mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description; + cm_->robot_description_callback(msg); + } + void switch_test_controllers( const std::vector & start_controllers, const std::vector & stop_controllers, const int strictness, @@ -157,6 +168,7 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; const std::string robot_description_; + rclcpp::Time time_; const bool pass_urdf_as_parameter_; }; diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 6b04063f7b..217f753327 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -166,9 +166,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()); diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml new file mode 100644 index 0000000000..892427bab7 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -0,0 +1,27 @@ +ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint0"] + +chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +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_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 7c03a8862d..e4cdf8a94f 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -23,10 +23,12 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; using ::testing::Return; +const char coveragepy_script[] = "python3 -m coverage run --append --branch"; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture @@ -67,14 +69,18 @@ class TestLoadController : public ControllerManagerFixtureget_loaded_controllers().size(), 0ul); } + +class TestLoadControllerWithoutRobotDescription +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithoutRobotDescription() + : ControllerManagerFixture("") + { + } + + 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(); } + + rclcpp::TimerBase::SharedPtr robot_description_sending_timer_; + +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(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spawner_times_out) +{ + 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 --controller-manager-timeout 1.0"), 256) + << "could not spawn controller because not robot description and not services for controller " + "manager are active"; +} + +TEST_F( + TestLoadControllerWithoutRobotDescription, + controller_starting_with_later_load_of_robot_description) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + // Delay sending robot description + robot_description_sending_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); }); + + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0) + << "could not activate control because not robot description"; + } + + 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_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +} + +class TestLoadControllerWithNamespacedCM +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithNamespacedCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf, false, "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_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"; + + // 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_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_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_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); +} + +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"; + + // 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_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_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_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); +} diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 869c849bb0..0623724f71 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- + 2.41.0 (2024-04-30) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index b3f7b0a91a..f3e25c1948 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 2.41.0 + 2.42.0 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..5349c98a52 --- /dev/null +++ b/doc/debugging.rst @@ -0,0 +1,80 @@ +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 new file mode 100644 index 0000000000..cb11ab2449 --- /dev/null +++ b/doc/migration.rst @@ -0,0 +1,9 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/migration.rst + +Migration Guides: Galactic to Humble +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes important changes between Galactic (previous) and Humble (current) releases, where changes to user code might be necessary. + +.. note:: + + This list was created in July 2024, earlier changes are not included. diff --git a/doc/migration/Galactic.rst b/doc/migration/Galactic.rst deleted file mode 100644 index 4c6e958951..0000000000 --- a/doc/migration/Galactic.rst +++ /dev/null @@ -1,53 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Galactic.rst - -Foxy to Galactic -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -hardware_interface -************************************** -Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. -The following list shows a set of quick changes to port existing hardware components to Galactic: - -1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn`` - -2. If using BaseInterface as base class then you should remove it. Specifically, change: - - .. code-block:: c++ - - hardware_interface::BaseInterface - - to - - .. code-block:: c++ - - hardware_interface::[Actuator|Sensor|System]Interface - -3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` - -4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary - -5. replace first three lines in ``on_init`` to - - .. code-block:: c++ - - if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } - - -6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` - -7. Remove all lines with ``status_ = ...`` or ``status::...`` - -8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and - ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)`` - -9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn`` - -10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;`` - -11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR`` - -12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add - ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml`` diff --git a/doc/release_notes.rst b/doc/release_notes.rst new file mode 100644 index 0000000000..a1c76f6caf --- /dev/null +++ b/doc/release_notes.rst @@ -0,0 +1,9 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst + +Release Notes: Galactic to Humble +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes the changes between Galactic (previous) and Humble (current) releases. Bugfixes are not included in this list. + +.. note:: + + This list was created in July 2024, earlier changes may not be included. diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 4352036be5..03f9766b26 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- +* Small improvements to the error output in component parser to make debugging easier. (backport `#1580 `_) (`#1581 `_) +* Update joints_userdoc.rst (`#1604 `_) +* Fix link to gazebosim.org (`#1563 `_) (`#1564 `_) +* Add doc page about joint kinematics (`#1497 `_) (`#1559 `_) +* Bump version of pre-commit hooks (backport `#1556 `_) (`#1557 `_) +* Contributors: Christoph Fröhlich, mergify[bot] + 2.41.0 (2024-04-30) ------------------- * Add missing calculate_dynamics (`#1498 `_) (`#1511 `_) diff --git a/hardware_interface/doc/joints_userdoc.rst b/hardware_interface/doc/joints_userdoc.rst index f4332410d5..fccb8e42cc 100644 --- a/hardware_interface/doc/joints_userdoc.rst +++ b/hardware_interface/doc/joints_userdoc.rst @@ -86,7 +86,7 @@ From the officially released packages, the following packages are already using * :ref:`mock_components (generic system) ` * :ref:`gazebo_ros2_control ` -* :ref:`gz_ros2_control ` +* :ref:`ign_ros2_control ` As the URDF specifies only the kinematics, the mimic tag has to be independent of the hardware interface type used in ros2_control. This means that we interpret this info in the following way: diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 89d27297b2..6c66b6b33e 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 2.41.0 + 2.42.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index da83d69bee..7792b0fbf4 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -563,11 +563,13 @@ std::vector parse_control_resources_from_urdf(const std::string & tinyxml2::XMLDocument doc; if (!doc.Parse(urdf.c_str()) && doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); + throw std::runtime_error( + "invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr())); } if (doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); + throw std::runtime_error( + "invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr())); } // Find robot tag diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 6b23698248..070c360488 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- + 2.41.0 (2024-04-30) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index a7a72f5b3b..7eb47ecd9b 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 2.41.0 + 2.42.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 933ecc4ab3..580aecb42a 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- +* Bump version of pre-commit hooks (backport `#1556 `_) (`#1557 `_) +* Fix dependencies for source build (`#1533 `_) (`#1535 `_) +* Contributors: mergify[bot] + 2.41.0 (2024-04-30) ------------------- * Bump version of pre-commit hooks (backport `#1430 `_) (`#1434 `_) diff --git a/joint_limits/package.xml b/joint_limits/package.xml index f1d1cdfc77..7380f29fcf 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 2.41.0 + 2.42.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 8fa306d2fd..9e5b2442f0 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- + 2.41.0 (2024-04-30) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index ac8566b484..4fcf1dfa10 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 2.41.0 + 2.42.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index f2efb09bd7..a51a4a37f1 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- + 2.41.0 (2024-04-30) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index df353fde68..fdcbe23195 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 2.41.0 + 2.42.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 482f5c3571..eb65ffe809 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- + 2.41.0 (2024-04-30) ------------------- * [CI] Specify runner/container images and codecov for joint_limits (`#1504 `_) (`#1519 `_) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index d37ed8f911..cd483a36ce 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 2.41.0 + 2.42.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 7ec9a6a63e..81dac6bb25 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="2.41.0", + version="2.42.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index fa0eb391e0..05577fe0a0 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- + 2.41.0 (2024-04-30) ------------------- * Add cm as dependency to rqt_cm (`#1447 `_) (`#1459 `_) diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index f411f3a1f2..a3173ea619 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 2.41.0 + 2.42.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 553b3a59ee..dce61021f8 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.41.0", + version="2.42.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 6d6c0fd365..0581f0071e 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.42.0 (2024-07-23) +------------------- + 2.41.0 (2024-04-30) ------------------- * rosdoc2 for transmission_interface (`#1496 `_) (`#1509 `_) 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 39f5df6f61..863c20da9b 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -288,7 +288,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/package.xml b/transmission_interface/package.xml index d6732fd43c..b53bfdfa4f 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 2.41.0 + 2.42.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl diff --git a/transmission_interface/test/random_generator_utils.hpp b/transmission_interface/test/random_generator_utils.hpp index e6088f1b2f..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(time(nullptr)); + srand(seed); } double operator()() {