From 4796ef3b0490ea1b39d1f92d75135a01afaca89e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 20:53:55 +0100 Subject: [PATCH] [ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (#1822) (cherry picked from commit e6e69c1052b91d52de792f49d89c6c54bf4ec18a) # Conflicts: # doc/release_notes.rst --- controller_manager/src/ros2_control_node.cpp | 7 ++ doc/release_notes.rst | 71 ++++++++++++++++++++ 2 files changed, 78 insertions(+) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index aa444cde69..263d0c48cb 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -44,6 +44,13 @@ int main(int argc, char ** argv) auto cm = std::make_shared(executor, manager_node_name); + const bool lock_memory = cm->get_parameter_or("lock_memory", true); + std::string message; + if (lock_memory && !realtime_tools::lock_memory(message)) + { + RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); + } + RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); RCLCPP_INFO( diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 85e4ca3e8f..4af7c60c6d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -17,4 +17,75 @@ controller_manager ****************** * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). +<<<<<<< HEAD * Added support for the wildcard entries for the controller configuration files (`#1724 `_). +======= +* The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). + +hardware_interface +****************** +* A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 `_) +* ``test_components`` was moved to its own package (`#1325 `_) +* The ``ros2_control`` tag now supports parsing of the limits from the URDF into the ``HardwareInfo`` structure. More conservative limits can be defined using the ``min`` and ``max`` attributes per interface (`#1472 `_) + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + + + +* Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) +* Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) +* Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. +* With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. + +joint_limits +************ +* Add header to import limits from standard URDF definition (`#1298 `_) + +Adaption of Command-/StateInterfaces +*************************************** +Changes from `(PR #1688) `_ for an overview of related changes and discussion refer to `(PR #1240) `_. + +* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). +* The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself. +* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function by creating some custom ``Command-/StateInterfaces``. + + +ros2controlcli +************** +* Spawner colours were added to ``list_controllers`` depending upon active or inactive (`#1409 `_) +* The ``set_hardware_component_state`` verb was added (`#1248 `_). Use the following command to set the state of a hardware component + + .. code-block:: bash + + ros2 control set_hardware_component_state + +* The ``load_controller`` now supports parsing of the params file (`#1703 `_). + + .. code-block:: bash + + ros2 control load_controller + +* All the ros2controlcli verbs now support the namespacing through the ROS 2 standard way (`#1703 `_). + + .. code-block:: bash + + ros2 control --ros-args -r __ns:= +>>>>>>> e6e69c1 ([ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (#1822))