diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 14fdef5c1e..ab6d2c3c45 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- + +2.32.0 (2023-10-03) +------------------- + +2.31.0 (2023-09-11) +------------------- +* add a broadcaster for range sensor (`#1091 `_) (`#1100 `_) +* Contributors: flochre + +2.30.0 (2023-08-14) +------------------- + 2.29.0 (2023-07-09) ------------------- diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp new file mode 100644 index 0000000000..baa9022c75 --- /dev/null +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 flochre +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ +#define SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ + +#include +#include +#include + +#include "semantic_components/semantic_component_interface.hpp" +#include "sensor_msgs/msg/range.hpp" + +namespace semantic_components +{ +class RangeSensor : public SemanticComponentInterface +{ +public: + explicit RangeSensor(const std::string & name) : SemanticComponentInterface(name, 1) + { + interface_names_.emplace_back(name_ + "/" + "range"); + } + + virtual ~RangeSensor() = default; + + /** + * Return Range reported by a sensor + * + * \return value of the range in meters + */ + float get_range() { return state_interfaces_[0].get().get_value(); } + + /// Return Range message with range in meters + /** + * Constructs and return a Range message from the current values. + * \return Range message from values; + */ + bool get_values_as_message(sensor_msgs::msg::Range & message) + { + // update the message values + message.range = get_range(); + + return true; + } +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 3de23b788d..8751107192 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 2.29.0 + 2.33.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index c917fdd05d..770b81b341 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- +* Export of the get_cm_node_options() from robostack (`#1129 `_) (`#1130 `_) +* Contributors: mergify[bot] + +2.32.0 (2023-10-03) +------------------- +* Fix multiple calls to export reference interfaces (backport `#1108 `_) (`#1114 `_) +* Contributors: Sai Kishor Kothakota, Dr Denis + +2.31.0 (2023-09-11) +------------------- +* [Docs] Fix information about activation and deactivation of chainable controllers (`#1104 `_) (`#1106 `_) +* Contributors: mergify[bot] + +2.30.0 (2023-08-14) +------------------- +* [CM] Fixes the issue with individual controller's update rate (`#1082 `_) (`#1097 `_) +* Contributors: Sai Kishor Kothakota + 2.29.0 (2023-07-09) ------------------- * [CM] Make error message after trying to active controller more informative. (`#1066 `_) (`#1072 `_) diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst new file mode 100644 index 0000000000..d79e730d3f --- /dev/null +++ b/controller_manager/doc/controller_chaining.rst @@ -0,0 +1,85 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_manager/doc/controller_chaining.rst + +.. _controller_chaining: + +Controller Chaining / Cascade Control +====================================== + +This document proposes a minimal-viable-implementation of serial controller chaining as described in `Chaining Controllers design document `__. +Cascade control is a specific type of controller chaining. + + +Scope of the Document and Background Knowledge +------------------------------------------------------- + +This approach focuses only on serial chaining of controllers and tries to reuse existing mechanisms for it. +It focuses on `inputs and outputs of a controller `__ and their management in the controller manager. +The concept of `controller groups `__ will be introduced only for clarity reasons, and its only meaning is that controllers in that group can be updated in arbitrary order. +This doesn't mean that the controller groups as described `in the controller chaining document `__ will not be introduced and used in the future. +Nevertheless, the author is convinced that this would add only unnecessary complexity at this stage, although in the long term they *could* provide clearer structure and interfaces. + +Motivation, Purpose and Use +--------------------------------- + +To describe the intent of this document, lets focus on the simple yet sufficient example `Example 2 from 'controllers_chaining' design docs `__: + +.. image:: images/chaining_example2.png + :alt: Example2 + + +In this example, we want to chain 'position_tracking' controller with 'diff_drive_controller' and two PID controllers. +Let's now imagine a use-case where we don't only want to run all those controllers as a group, but also flexibly add preceding steps. +This means the following: + + 1. When a robot is started, we want to check if motor velocity control is working properly and therefore only PID controllers are activated. + At this stage we can control the input of PID controller also externally using topics. + However, these controllers also provide virtual interfaces, so we can chain them. + 2. Then "diff_drive_controller" is activated and attaches itself to the virtual input interfaces of PID controllers. + PID controllers also get informed that they are working in chained mode and therefore disable their external interface through subscriber. + Now we check if kinematics of differential robot is running properly. + 3. After that, "position_tracking" can be activated and attached to "diff_drive_controller" that disables its external interfaces. + 4. If any of the controllers is deactivated, also all preceding controllers are deactivated. + + +Implementation +-------------- + +A Controller Base-Class: ChainableController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +A ``ChainableController`` extends ``ControllerInterface`` class with ``virtual InterfaceConfiguration input_interface_configuration() const = 0`` method. +This method should implement for each controller that **can be preceded** by another controller exporting all the input interfaces. +For simplicity reasons, it is assumed for now that controller's all input interfaces are used. +Therefore, do not try to implement any exclusive combinations of input interfaces, but rather write multiple controllers if you need exclusivity. + +The ``ChainableController`` base class implements ``void set_chained_mode(bool activate)`` that sets an internal flag that a controller is used by another controller (in chained mode) and calls ``virtual void on_set_chained_mode(bool activate) = 0`` that implements controller's specific actions when chained modes is activated or deactivated, e.g., deactivating subscribers. + +As an example, PID controllers export one virtual interface ``pid_reference`` and stop their subscriber ``/pid_reference`` when used in chained mode. 'diff_drive_controller' controller exports list of virtual interfaces ``/v_x``, ``/v_y``, and ``/w_z``, and stops subscribers from topics ``/cmd_vel`` and ``/cmd_vel_unstamped``. Its publishers can continue running. + +Inner Resource Management +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +After configuring a chainable controller, controller manager calls ``input_interface_configuration`` method and takes ownership over controller's input interfaces. +This is the same process as done by ``ResourceManager`` and hardware interfaces. +Controller manager maintains "claimed" status of interface in a vector (the same as done in ``ResourceManager``). + + +Activation and Deactivation Chained Controllers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Chained controllers must be activated and deactivated together or in the proper order. +This means you must first activate all following controllers to have the preceding one activated. +For the deactivation there is the inverse rule - all preceding controllers have to be deactivated before the following controller is deactivated. +One can also think of it as an actual chain, you can not add a chain link or break the chain in the middle. + + +Debugging outputs +---------------------------- + +Flag ``unavailable`` on reference interface does not provide much information about anything at the moment. So don't get confused by it. The reason we have it are internal implementation reasons irelevant for the usage. + + +Closing remarks +---------------------------- + +- Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add implementation of ``input_interface_configuration()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``. diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 8741df8470..69268d5ca1 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -55,7 +55,7 @@ namespace controller_manager { using ControllersListIterator = std::vector::const_iterator; -rclcpp::NodeOptions get_cm_node_options(); +CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); class ControllerManager : public rclcpp::Node { diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 02eec626be..8ab2d8a7d7 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 2.29.0 + 2.33.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6806f226dc..0024359d1e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1362,11 +1362,17 @@ void ControllerManager::list_controllers_srv_cb( } } // check reference interfaces only if controller is inactive or active - auto references = controllers[i].c->export_reference_interfaces(); - controller_state.reference_interfaces.reserve(references.size()); - for (const auto & reference : references) + if (controllers[i].c->is_chainable()) { - controller_state.reference_interfaces.push_back(reference.get_interface_name()); + auto references = + resource_manager_->get_controller_reference_interface_names(controllers[i].info.name); + controller_state.reference_interfaces.reserve(references.size()); + for (const auto & reference : references) + { + const std::string prefix_name = controllers[i].c->get_node()->get_name(); + const std::string interface_name = reference.substr(prefix_name.size() + 1); + controller_state.reference_interfaces.push_back(interface_name); + } } } response->controller.push_back(controller_state); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index adb227d6fe..68fe01946e 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -269,7 +269,9 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) ASSERT_EQ(result->controller[0].is_chainable, true); ASSERT_EQ(result->controller[0].is_chained, false); ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u); - ; + ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]); + ASSERT_EQ(result->controller[0].chain_connections.size(), 0u); // check test controller ASSERT_EQ(result->controller[1].state, "inactive"); diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 9da870680e..ec22935d97 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- + +2.32.0 (2023-10-03) +------------------- + +2.31.0 (2023-09-11) +------------------- + +2.30.0 (2023-08-14) +------------------- + 2.29.0 (2023-07-09) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 93218f67d6..ae0956913d 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 2.29.0 + 2.33.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 57f72fe3b1..dd4678b623 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- +* [MockHardware] Added dynamic simulation functionality. (`#1028 `_) (`#1125 `_) +* Contributors: mergify[bot] + +2.32.0 (2023-10-03) +------------------- +* Add GPIO tag description to docs (`#1109 `_) (`#1120 `_) +* Contributors: Christoph Froehlich + +2.31.0 (2023-09-11) +------------------- + +2.30.0 (2023-08-14) +------------------- +* Add checks if hardware is initialized. (backport `#1054 `_) (`#1081 `_) +* Contributors: Denis Stogl + 2.29.0 (2023-07-09) ------------------- diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index cd93f472a5..1a8c5e611b 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -14,20 +14,21 @@ Guidelines and Best Practices .. toctree:: :titlesonly: - Writing a Hardware Interface + Hardware Interface Types + Writing a Hardware Component Handling of errors that happen during read() and write() calls ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``write()`` methods of a hardware interface, ``on_error(previous_state)`` method will be called to handle any error that happened. +If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``write()`` methods of a hardware_interface class, ``on_error(previous_state)`` method will be called to handle any error that happened. Error handling follows the `node lifecycle `_. If successful ``CallbackReturn::SUCCESS`` is returned and hardware is again in ``UNCONFIGURED`` state, if any ``ERROR`` or ``FAILURE`` happens the hardware ends in ``FINALIZED`` state and can not be recovered. The only option is to reload the complete plugin, but there is currently no service for this in the Controller Manager. -Migration from Foxy to Galactic -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Migration from Foxy to newer versions +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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: @@ -36,20 +37,29 @@ The following list shows a set of quick changes to port existing hardware compon 2. If using BaseInterface as base class then you should remove it. Specifically, change: -hardware_interface::BaseInterface to hardware_interface::[Actuator|Sensor|System]Interface + .. 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: +5. replace first three lines in ``on_init`` to + + .. code-block:: c++ -.. code-block:: c++ + if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } - 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;`` diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst new file mode 100644 index 0000000000..54b2003568 --- /dev/null +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -0,0 +1,150 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/hardware_interface_types_userdoc.rst + +.. _hardware_interface_types_userdoc: + +``ros2_control`` hardware interface types +--------------------------------------------------------- + +The ``ros2_control`` framework provides a set of hardware interface types that can be used to implement +a hardware component for a specific robot or device. +The following sections describe the different hardware interface types and their usage. + +Joints +***************************** +````-tag groups the interfaces associated with the joints of physical robots and actuators. +They have command and state interfaces to set the goal values for hardware and read its current state. + +State interfaces of joints can be published as a ROS topic by means of the :ref:`joint_state_broadcaster ` + +Sensors +***************************** +````-tag groups multiple state interfaces describing, e.g., internal states of hardware. + +Depending on the type of sensor, there exist a couple of specific semantic components with broadcasters shipped with ros2_controllers, e.g. + +- :ref:`Imu Sensor Broadcaster ` +- :ref:`Force Torque Sensor Broadcaster ` + +GPIOs +***************************** +The ````-tag is used for describing input and output ports of a robotic device that cannot be associated with any joint or sensor. +Parsing of ````-tag is similar to this of a ````-tag having command and state interfaces. +The tag must have at least one ````- or ````-tag as a child. + +The keyword "gpio" is chosen for its generality. +Although strictly used for digital signals, it describes any electrical analog, digital signal, or physical value. + +The ```` tag can be used as a child of all three types of hardware components, i.e., system, sensor, or actuator. + +Because ports implemented as ````-tag are typically very application-specific, there exists no generic publisher +within the ros2_control framework. A custom gpio-controller has to be implemented for each application. As an example, see :ref:`the GPIO controller example ` as part of the demo repository. + +Examples +***************************** +The following examples show how to use the different hardware interface types in a ``ros2_control`` URDF. +They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation `) as follows + +1. Robot with multiple GPIO interfaces + + - RRBot System + - Digital: 4 inputs and 2 outputs + - Analog: 2 inputs and 1 output + - Vacuum valve at the flange (on/off) + + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + + + + + + + + + + +2. Gripper with electrical and suction grasping possibilities + + - Multimodal gripper + - 1-DoF parallel gripper + - suction on/off + + .. code:: xml + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + + + +3. Force-Torque-Sensor with temperature feedback and adjustable calibration + + - 2D FTS + - Temperature feedback in °C + - Choice between 3 calibration matrices, i.e., calibration ranges + + .. code:: xml + + + + ros2_control_demo_hardware/ForceTorqueSensor2DHardware + 0.43 + + + + + kuka_tcp + 100 + 100 + + + + + + + + + diff --git a/hardware_interface/doc/writing_new_hardware_interface.rst b/hardware_interface/doc/writing_new_hardware_component.rst similarity index 98% rename from hardware_interface/doc/writing_new_hardware_interface.rst rename to hardware_interface/doc/writing_new_hardware_component.rst index 1ff4dc4420..698f6cf6e2 100644 --- a/hardware_interface/doc/writing_new_hardware_interface.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -1,9 +1,9 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/writing_new_hardware_interface.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/writing_new_hardware_component.rst -.. _writing_new_hardware_interface: +.. _writing_new_hardware_component: -Writing a new hardware interface -================================= +Writing a Hardware Component +============================ In ros2_control hardware system components are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new hardware interface. diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index c244ee1254..e9b38de65d 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -32,6 +32,10 @@ namespace mock_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +static constexpr size_t POSITION_INTERFACE_INDEX = 0; +static constexpr size_t VELOCITY_INTERFACE_INDEX = 1; +static constexpr size_t ACCELERATION_INTERFACE_INDEX = 2; + class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface { public: @@ -41,6 +45,14 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector export_command_interfaces() override; + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + + return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -60,8 +72,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - const size_t POSITION_INTERFACE_INDEX = 0; - struct MimicJoint { std::size_t joint_index; @@ -115,6 +125,9 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::string custom_interface_with_following_offset_; size_t index_custom_interface_with_following_offset_; + bool calculate_dynamics_; + std::vector joint_control_mode_; + bool command_propagation_disabled_; }; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 5d1ca1a913..a8bbbae9ca 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 2.29.0 + 2.33.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 8b48b2efdf..f34c08c9a4 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -118,6 +118,17 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i command_propagation_disabled_ = false; } + // check if there is parameter that enables dynamic calculation + it = info_.hardware_parameters.find("calculate_dynamics"); + if (it != info.hardware_parameters.end()) + { + calculate_dynamics_ = hardware_interface::parse_bool(it->second); + } + else + { + calculate_dynamics_ = false; + } + // process parameters about state following position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; @@ -299,7 +310,7 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + for (size_t i = 0; i < info_.joints.size(); ++i) { const auto & joint = info_.joints[i]; for (const auto & interface : joint.command_interfaces) @@ -320,6 +331,8 @@ std::vector GenericSystem::export_command_ } } } + // Set position control mode per default + joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) @@ -356,7 +369,135 @@ std::vector GenericSystem::export_command_ return command_interfaces; } -return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +return_type GenericSystem::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + + const size_t FOUND_ONCE_FLAG = 1000000; + + std::vector joint_found_in_x_requests_; + 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(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info_.joints.end()) + { + 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) + { + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + if (!calculate_dynamics_) + { + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "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()); + } + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + if (!calculate_dynamics_) + { + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "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()); + } + joint_found_in_x_requests_[joint_index] += 1; + } + } + else + { + RCUTILS_LOG_DEBUG_NAMED( + "mock_generic_system", "Got interface '%s' that is not joint - nothing to do!", + key.c_str()); + } + } + + 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) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", + 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; + } + + // Currently we don't support multiple interface request + if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", + "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()); + ret_val = hardware_interface::return_type::ERROR; + } + } + + return ret_val; +} + +return_type GenericSystem::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + if (!calculate_dynamics_) + { + return hardware_interface::return_type::OK; + } + + for (const auto & key : start_interfaces) + { + // check if interface is joint + auto joint_it_found = std::find_if( + 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()) + { + 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) + { + joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; + } + 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) + { + joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; + } + } + } + + return hardware_interface::return_type::OK; +} + +return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { if (command_propagation_disabled_) { @@ -379,19 +520,97 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } }; - // apply offset to positions only for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if (calculate_dynamics_) { - joint_states_[POSITION_INTERFACE_INDEX][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + switch (joint_control_mode_[j]) + { + case ACCELERATION_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] += + joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + + if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) + { + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; + } + break; + } + case VELOCITY_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) + { + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + joint_commands_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + case POSITION_INTERFACE_INDEX: + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + } + } + else + { + for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + } + } } } - // do loopback on all other interfaces - starts from 1 because 0 index is position interface - mirror_command_to_state(joint_states_, joint_commands_, 1); + // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, + // velocity, and acceleration interface + if (calculate_dynamics_) + { + mirror_command_to_state(joint_states_, joint_commands_, 3); + } + else + { + mirror_command_to_state(joint_states_, joint_commands_, 1); + } for (const auto & mimic_joint : mimic_joints_) { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7efb7e2cd1..e71a41139b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -490,54 +490,138 @@ class ResourceStorage // TODO(destogl): Propagate "false" up, if happens in initialize_hardware void load_and_initialize_actuator(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, actuator_loader_, actuators_); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto load_and_init_actuators = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, actuator_loader_, container); + if (initialize_hardware(hardware_info, container.back())) + { + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "Actuator hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_class_type.c_str()); + } + }; + + load_and_init_actuators(actuators_); } void load_and_initialize_sensor(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, sensor_loader_, sensors_); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto load_and_init_sensors = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, sensor_loader_, container); + if (initialize_hardware(hardware_info, container.back())) + { + import_state_interfaces(container.back()); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "Sensor hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_class_type.c_str()); + } + }; + + load_and_init_sensors(sensors_); } void load_and_initialize_system(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, system_loader_, systems_); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto load_and_init_systems = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, system_loader_, container); + if (initialize_hardware(hardware_info, container.back())) + { + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "System hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_class_type.c_str()); + } + }; + + load_and_init_systems(systems_); } void initialize_actuator( std::unique_ptr actuator, const HardwareInfo & hardware_info) { - this->actuators_.emplace_back(Actuator(std::move(actuator))); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto init_actuators = [&](auto & container) + { + container.emplace_back(Actuator(std::move(actuator))); + if (initialize_hardware(hardware_info, container.back())) + { + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "Actuator hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_class_type.c_str()); + } + }; + + init_actuators(actuators_); } void initialize_sensor( std::unique_ptr sensor, const HardwareInfo & hardware_info) { - this->sensors_.emplace_back(Sensor(std::move(sensor))); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto init_sensors = [&](auto & container) + { + container.emplace_back(Sensor(std::move(sensor))); + if (initialize_hardware(hardware_info, container.back())) + { + import_state_interfaces(container.back()); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "Sensor hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_class_type.c_str()); + } + }; + + init_sensors(sensors_); } void initialize_system( std::unique_ptr system, const HardwareInfo & hardware_info) { - this->systems_.emplace_back(System(std::move(system))); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto init_systems = [&](auto & container) + { + container.emplace_back(System(std::move(system))); + if (initialize_hardware(hardware_info, container.back())) + { + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "System hardware component '%s' from plugin '%s' failed to initialize.", + hardware_info.name.c_str(), hardware_info.hardware_class_type.c_str()); + } + }; + + init_systems(systems_); } // hardware plugins diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index e386c8ea73..c656d1a692 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -33,7 +33,8 @@ namespace { const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math +const auto COMPARE_DELTA = 0.0001; } // namespace class TestGenericSystem : public ::testing::Test @@ -484,6 +485,39 @@ class TestGenericSystem : public ::testing::Test )"; + hardware_system_2dof_standard_interfaces_with_different_control_modes_ = + R"( + + + mock_components/GenericSystem + true + + + + + + 3.45 + + + + + + + + + 2.78 + + + + + + + + + + +)"; + disabled_commands_ = R"( @@ -521,6 +555,7 @@ class TestGenericSystem : public ::testing::Test std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; + std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; }; @@ -1624,6 +1659,202 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) ASSERT_EQ(1, state.get_value()); } +TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_standard_interfaces_with_different_control_modes_ + + ros2_control_test_assets::urdf_tail; + + TestableResourceManager rm(urdf); + // Activate components to get all interfaces available + activate_components(rm); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(7u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint1/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("flange_vacuum/vacuum")); + + ASSERT_EQ(5u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.command_interface_exists("flange_vacuum/vacuum")); + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j1a_s = rm.claim_state_interface("joint1/acceleration"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedStateInterface j2a_s = rm.claim_state_interface("joint2/acceleration"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j2a_c = + rm.claim_command_interface("joint2/acceleration"); + + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_TRUE(std::isnan(j2a_c.get_value())); + + // Test error management in prepare mode switch + ASSERT_EQ( // joint2 has non 'position', 'velocity', or 'acceleration' interface + rm.prepare_command_mode_switch({"joint1/position", "joint2/effort"}, {}), false); + ASSERT_EQ( // joint1 has two interfaces + rm.prepare_command_mode_switch({"joint1/position", "joint1/acceleration"}, {}), false); + + // switch controller mode as controller manager is doing - gpio itf 'vacuum' will be ignored + ASSERT_EQ( + rm.prepare_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); + ASSERT_EQ( + rm.perform_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); + + // set some new values in commands + j1p_c.set_value(0.11); + j2a_c.set_value(3.5); + + // State values should not be changed + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(-33.4, j1v_s.get_value()); + EXPECT_NEAR(-334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_NEAR(334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_NEAR(0.35, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // switch controller mode as controller manager is doing + ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + + // set some new values in commands + j1v_c.set_value(0.5); + j2v_c.set_value(2.0); + + // State values should not be changed + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(5.0, j1a_s.get_value()); + EXPECT_EQ(2.885, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_NEAR(13.0, j2a_s.get_value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.16, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(3.085, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); +} + TEST_F(TestGenericSystem, disabled_commands_flag_is_active) { auto urdf = @@ -1641,7 +1872,6 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ASSERT_EQ(2u, rm.command_interface_keys().size()); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); - // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 3bcfa21a48..7b9139698e 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- + +2.32.0 (2023-10-03) +------------------- + +2.31.0 (2023-09-11) +------------------- + +2.30.0 (2023-08-14) +------------------- + 2.29.0 (2023-07-09) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 92a9b6f5fe..3ac8a32ffb 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 2.29.0 + 2.33.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 2ea117cc57..ba9cd0f390 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- + +2.32.0 (2023-10-03) +------------------- + +2.31.0 (2023-09-11) +------------------- + +2.30.0 (2023-08-14) +------------------- + 2.29.0 (2023-07-09) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index b91048aa20..3bf84197d9 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 2.29.0 + 2.33.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 1812fd4a3d..61e40b8513 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- + +2.32.0 (2023-10-03) +------------------- + +2.31.0 (2023-09-11) +------------------- + +2.30.0 (2023-08-14) +------------------- + 2.29.0 (2023-07-09) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index da7584c415..03f484e9f2 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 2.29.0 + 2.33.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 80fb831d7c..2ab3ce18e8 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- +* Fix doc of load_controller (`#1135 `_) +* Contributors: Christoph Fröhlich + +2.32.0 (2023-10-03) +------------------- + +2.31.0 (2023-09-11) +------------------- + +2.30.0 (2023-08-14) +------------------- +* Add info where the pdf is saved to view_controller_chains (`#1094 `_) (`#1096 `_) +* Contributors: Christoph Fröhlich + 2.29.0 (2023-07-09) ------------------- diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index d604a26fe8..bb21b7f005 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -148,18 +148,21 @@ load_controller .. code-block:: console $ ros2 control load_controller -h - usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set_state {configure,activate}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name + usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [-s] [--set-state {configured,inactive,active}] [-c CONTROLLER_MANAGER] + [--include-hidden-nodes] + controller_name Load a controller in a controller manager positional arguments: controller_name Name of the controller - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - --set_state {configured,inactive,active} + -s, --use-sim-time Enable ROS simulation time + --set-state {configured,inactive,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 11de1cc793..83ebdc472b 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 2.29.0 + 2.33.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index d39ed21a72..06745c30d7 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version='2.29.0', + version='2.33.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 01fcdbb0d6..e08ff844cb 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- + +2.32.0 (2023-10-03) +------------------- + +2.31.0 (2023-09-11) +------------------- + +2.30.0 (2023-08-14) +------------------- + 2.29.0 (2023-07-09) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 9e921c0355..bcaedcade5 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 2.29.0 + 2.33.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 0b6978e7de..606dcf04f2 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version='2.29.0', + version='2.33.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 5c1d085482..60cce5731d 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.33.0 (2023-10-11) +------------------- + +2.32.0 (2023-10-03) +------------------- + +2.31.0 (2023-09-11) +------------------- + +2.30.0 (2023-08-14) +------------------- + 2.29.0 (2023-07-09) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index c3237bddb7..5acedf7a48 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 2.29.0 + 2.33.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