Skip to content

Commit

Permalink
Merge branch 'master' into no_sim_time
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich authored Dec 3, 2024
2 parents 7b9dfed + 89fd4ef commit 999a93d
Show file tree
Hide file tree
Showing 13 changed files with 702 additions and 110 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,25 @@ struct ControllerUpdateStats
unsigned int total_triggers;
unsigned int failed_triggers;
};

/**
* Struct to store the status of the controller update method.
* The status contains information if the update was triggered successfully, the result of the
* update method and the execution duration of the update method. The status is used to provide
* feedback to the controller_manager.
* @var successful: true if the update was triggered successfully, false if not.
* @var result: return_type::OK if update is successfully, otherwise return_type::ERROR.
* @var execution_time: duration of the execution of the update method.
* @var period: period of the update method.
*/
struct ControllerUpdateStatus
{
bool successful = true;
return_type result = return_type::OK;
std::optional<std::chrono::nanoseconds> execution_time = std::nullopt;
std::optional<rclcpp::Duration> period = std::nullopt;
};

/**
* Base interface class for an controller. The interface may not be used to implement a controller.
* The class provides definitions for `ControllerInterface` and `ChainableControllerInterface`
Expand Down Expand Up @@ -175,13 +194,11 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
*
* \param[in] time The time at the start of this control loop iteration
* \param[in] period The measured time taken by the last control loop iteration
* \returns A pair with the first element being a boolean indicating if the async callback method
* was triggered and the second element being the last return value of the async function. For
* more details check the AsyncFunctionHandler implementation in `realtime_tools` package.
* \returns ControllerUpdateStatus. The status contains information if the update was triggered
* successfully, the result of the update method and the execution duration of the update method.
*/
CONTROLLER_INTERFACE_PUBLIC
std::pair<bool, return_type> trigger_update(
const rclcpp::Time & time, const rclcpp::Duration & period);
ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period);

CONTROLLER_INTERFACE_PUBLIC
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();
Expand Down
24 changes: 21 additions & 3 deletions controller_interface/src/controller_interface_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -159,12 +159,14 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c
return node_->get_current_state();
}

std::pair<bool, return_type> ControllerInterfaceBase::trigger_update(
ControllerUpdateStatus ControllerInterfaceBase::trigger_update(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
ControllerUpdateStatus status;
trigger_stats_.total_triggers++;
if (is_async())
{
const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time();
const auto result = async_handler_->trigger_async_callback(time, period);
if (!result.first)
{
Expand All @@ -174,12 +176,28 @@ std::pair<bool, return_type> ControllerInterfaceBase::trigger_update(
"The controller missed %u update cycles out of %u total triggers.",
trigger_stats_.failed_triggers, trigger_stats_.total_triggers);
}
return result;
status.successful = result.first;
status.result = result.second;
const auto execution_time = async_handler_->get_last_execution_time();
if (execution_time.count() > 0)
{
status.execution_time = execution_time;
}
if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED)
{
status.period = time - last_trigger_time;
}
}
else
{
return std::make_pair(true, update(time, period));
const auto start_time = std::chrono::steady_clock::now();
status.successful = true;
status.result = update(time, period);
status.execution_time = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::steady_clock::now() - start_time);
status.period = period;
}
return status;
}

std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node()
Expand Down
11 changes: 10 additions & 1 deletion controller_manager/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rclcpp
realtime_tools
std_msgs
libstatistics_collector
generate_parameter_library
)

find_package(ament_cmake REQUIRED)
Expand All @@ -27,6 +29,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()

generate_parameter_library(controller_manager_parameters
src/controller_manager_parameters.yaml
)

add_library(controller_manager SHARED
src/controller_manager.cpp
)
Expand All @@ -35,6 +41,9 @@ target_include_directories(controller_manager PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/controller_manager>
)
target_link_libraries(controller_manager PUBLIC
controller_manager_parameters
)
ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})

# Causes the visibility macros to use dllexport rather than dllimport,
Expand Down Expand Up @@ -236,7 +245,7 @@ install(
DESTINATION include/controller_manager
)
install(
TARGETS controller_manager
TARGETS controller_manager controller_manager_parameters
EXPORT export_controller_manager
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
Expand Down
21 changes: 21 additions & 0 deletions controller_manager/doc/parameters_context.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
hardware_components_initial_state: |
Map of parameters for controlled lifecycle management of hardware components.
The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated.
Detailed explanation of each parameter is given below.
The full structure of the map is given in the following example:
.. code-block:: yaml
hardware_components_initial_state:
unconfigured:
- "arm1"
- "arm2"
inactive:
- "base3"
diagnostics.threshold.controllers.periodicity: |
The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.
diagnostics.threshold.controllers.execution_time: |
The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle.
36 changes: 10 additions & 26 deletions controller_manager/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -57,32 +57,6 @@ robot_description [std_msgs::msg::String]
Parameters
-----------

hardware_components_initial_state
Map of parameters for controlled lifecycle management of hardware components.
The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated.
Detailed explanation of each parameter is given below.
The full structure of the map is given in the following example:

.. code-block:: yaml
hardware_components_initial_state:
unconfigured:
- "arm1"
- "arm2"
inactive:
- "base3"
hardware_components_initial_state.unconfigured (optional; list<string>; default: empty)
Defines which hardware components will be only loaded immediately when controller manager is started.

hardware_components_initial_state.inactive (optional; list<string>; default: empty)
Defines which hardware components will be configured immediately when controller manager is started.

update_rate (mandatory; integer)
The frequency of controller manager's real-time update loop.
This loop reads states from hardware, updates controller and writes commands to hardware.

<controller_name>.type
Name of a plugin exported using ``pluginlib`` for a controller.
This is a class from which controller's instance with name "``controller_name``" is created.
Expand All @@ -99,6 +73,16 @@ update_rate (mandatory; integer)
The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation.
It is recommended to test the fallback strategy in simulation before deploying it on the real robot.

.. generate_parameter_library_details::
../src/controller_manager_parameters.yaml
parameters_context.yaml

**An example parameter file:**

.. generate_parameter_library_default::
../src/controller_manager_parameters.yaml


Handling Multiple Controller Managers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@

namespace controller_manager
{
class ParamListener;
class Params;
using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;

CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options();
Expand Down Expand Up @@ -346,7 +348,7 @@ class ControllerManager : public rclcpp::Node

// Per controller update rate support
unsigned int update_loop_counter_ = 0;
unsigned int update_rate_ = 100;
unsigned int update_rate_;
std::vector<std::vector<std::string>> chained_controllers_configuration_;

std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
Expand All @@ -357,6 +359,8 @@ class ControllerManager : public rclcpp::Node
const std::string & command_interface);
void init_controller_manager();

void initialize_parameters();

/**
* Clear request lists used when switching controllers. The lists are shared between "callback"
* and "control loop" threads.
Expand Down Expand Up @@ -473,6 +477,8 @@ class ControllerManager : public rclcpp::Node
*/
rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const;

std::shared_ptr<controller_manager::ParamListener> cm_param_listener_;
std::shared_ptr<controller_manager::Params> params_;
diagnostic_updater::Updater diagnostics_updater_;

std::shared_ptr<rclcpp::Executor> executor_;
Expand Down Expand Up @@ -603,6 +609,8 @@ class ControllerManager : public rclcpp::Node
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;

controller_manager::MovingAverageStatistics periodicity_stats_;

struct SwitchParams
{
void reset()
Expand Down
13 changes: 13 additions & 0 deletions controller_manager/include/controller_manager/controller_spec.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,13 @@
#include <vector>
#include "controller_interface/controller_interface_base.hpp"
#include "hardware_interface/controller_info.hpp"
#include "libstatistics_collector/moving_average_statistics/moving_average.hpp"

namespace controller_manager
{

using MovingAverageStatistics =
libstatistics_collector::moving_average_statistics::MovingAverageStatistics;
/// Controller Specification
/**
* This struct contains both a pointer to a given controller, \ref c, as well
Expand All @@ -35,9 +39,18 @@ namespace controller_manager
*/
struct ControllerSpec
{
ControllerSpec()
{
last_update_cycle_time = std::make_shared<rclcpp::Time>(0, 0, RCL_CLOCK_UNINITIALIZED);
execution_time_statistics = std::make_shared<MovingAverageStatistics>();
periodicity_statistics = std::make_shared<MovingAverageStatistics>();
}

hardware_interface::ControllerInfo info;
controller_interface::ControllerInterfaceBaseSharedPtr c;
std::shared_ptr<rclcpp::Time> last_update_cycle_time;
std::shared_ptr<MovingAverageStatistics> execution_time_statistics;
std::shared_ptr<MovingAverageStatistics> periodicity_statistics;
};

struct ControllerChainSpec
Expand Down
2 changes: 2 additions & 0 deletions controller_manager/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@
<depend>ros2param</depend>
<depend>ros2run</depend>
<depend>std_msgs</depend>
<depend>libstatistics_collector</depend>
<depend>generate_parameter_library</depend>

<test_depend>ament_cmake_gmock</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
Expand Down
Loading

0 comments on commit 999a93d

Please sign in to comment.