Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Async Function Handler for Controllers #1489

Merged
merged 91 commits into from
Oct 17, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
91 commits
Select commit Hold shift + click to select a range
e8130a4
Add first version of the async controllers
saikishor Apr 9, 2024
062a445
removed extra notify one
saikishor Apr 10, 2024
fd270ca
added async_update_ready_ to use with conditional variable to avoid s…
saikishor Apr 10, 2024
703f91f
call notify_one after setting the async_update_ready
saikishor Apr 10, 2024
c09339f
add precommit changes
saikishor Apr 10, 2024
1f58a2d
add missing this in the conditional variable callback
saikishor Apr 10, 2024
c2f7192
change atomic bool to normal bool
saikishor Apr 10, 2024
4255cc3
add wait_for_update_to_finish method
saikishor Apr 10, 2024
9f7c0b4
add notify_one in different places to wait properly to finish update …
saikishor Apr 11, 2024
faba7cd
added unique lock and check with try_lock for triggering calls
saikishor Apr 11, 2024
4ffa885
use try_to_lock for better checking of owning the lock or not
saikishor Apr 11, 2024
f575440
add current update time and period to be able to update them
saikishor Apr 11, 2024
4348019
update documentation of the async method
saikishor Apr 11, 2024
a2469ca
added some comments about the issue with the get_state method
saikishor Apr 11, 2024
ffaad19
use atomic_bool
saikishor Apr 11, 2024
b7586a2
minor change in the logic
saikishor Apr 11, 2024
0772c49
initialize the duration to fix the compilation error
saikishor Apr 11, 2024
bf2a2d3
Move the main async logic into a separate method
saikishor Apr 11, 2024
89e6a7b
Add async function handler to handle parsed functions
saikishor Apr 11, 2024
2e0a0d2
add missing template typenames on functions
saikishor Apr 12, 2024
bfae6a1
set wait_for_update_to_finish to const
saikishor Apr 12, 2024
c2adc42
change async_update_ready_ to atomic bool
saikishor Apr 12, 2024
5ca2e5d
use also async_update_stop_ also in the conditional wait predicate fu…
saikishor Apr 12, 2024
c5a00e6
Add some utility methods
saikishor Apr 12, 2024
b83157d
modify the std::function arguments of async_update method
saikishor Apr 12, 2024
225c96f
Fix the issue with the overidding template names and typo in the std:…
saikishor Apr 13, 2024
1f3377c
fix the wait for update to finish method
saikishor Apr 13, 2024
fd8be6d
Check if the parsed functions are valid or not in the init method
saikishor Apr 14, 2024
794a8c0
unlock the mutex before notifying the conditional variable
saikishor Apr 14, 2024
619f46d
Do not check if the thread is joinable in the is_initialized method
saikishor Apr 14, 2024
98aa3b0
added a explicit method to join the async thread
saikishor Apr 14, 2024
7f9e47f
Add missing includes
saikishor Apr 14, 2024
b5ad675
change the way the initialization in done
saikishor Apr 14, 2024
3cd856f
Added tests for the new async function handler
saikishor Apr 14, 2024
f88fe20
extend the tests with another loop
saikishor Apr 14, 2024
eb7da6b
Added test case to check for triggering
saikishor Apr 14, 2024
b61b8cd
Also test the case of triggering without initialization
saikishor Apr 14, 2024
3fe3d31
Add test triggering the handler for several cycles
saikishor Apr 14, 2024
1fb5955
Add the test cases of changes in the lifecycle state
saikishor Apr 14, 2024
6952577
remove is_async method and added is_preempted method + changes to the…
saikishor Apr 14, 2024
b3975db
Use the AsyncFunctionHandler instead of implementing in the controlle…
saikishor Apr 15, 2024
47d5928
Add documentation to the methods
saikishor Apr 15, 2024
d86be02
remove the arguments to initialize_async_update_thread method
saikishor Apr 15, 2024
76e3f30
simplify the is_initialized method check
saikishor Apr 15, 2024
f1634bc
rename preempt_async_update to stop_async_update
saikishor Apr 15, 2024
c094f81
Separate the exceptions for the parsed methods
saikishor Apr 15, 2024
88ceb3c
change the exception inside the initialize_async_update_thread
saikishor Apr 15, 2024
cecb00b
move the test implementations to the cpp file
saikishor Apr 15, 2024
f61e382
Remove the unlock when preempted as the lock goes out of scope
saikishor Apr 15, 2024
c070ed6
rename async_update_ready_ to trigger_in_progress_
saikishor Apr 15, 2024
fff989e
update the docs
saikishor Apr 15, 2024
71f7ad9
rename the method wait_for_update_to_finish to wait_for_trigger_cycle…
saikishor Apr 15, 2024
fa6a88c
Add documentation to the trigger_update method and add it to the cont…
saikishor Apr 15, 2024
91aab1f
added a method to stop the async update cycle before deactivating the…
saikishor Apr 15, 2024
6ad0c1b
return std::pair to check if the trigger is successful or not
saikishor Apr 16, 2024
d2c6692
update the runtime error throw message
saikishor Apr 16, 2024
c4b9f82
rename is_preempted to is_stopped for the context of the threads
saikishor Apr 18, 2024
48b2c2f
change EXPECT_THROW to ASSERT_THROW
saikishor Apr 18, 2024
a62da78
Update the documentation of the init return entity
saikishor Apr 18, 2024
f8a93ff
Fix the doc of stop_async_update_cycle
saikishor Apr 18, 2024
5d4b4e2
add minor information
saikishor Apr 18, 2024
20bc5d1
add missing functional include
saikishor Apr 18, 2024
600da5c
check if lock is owned or not before in the condition
saikishor Apr 21, 2024
7f2aa64
Use separated arguments for better clarity
saikishor Apr 21, 2024
679fbf9
Move the lock within a scope to avoid exclusive unlock within the thread
saikishor Apr 22, 2024
d1ac8df
notify other waiting threads before stopping
saikishor Apr 22, 2024
4e3dbb1
Scope the lock to avoid manual unlock in the trigger_async_update method
saikishor Apr 22, 2024
30be210
Update async_update_stop_ within the scope of the lock to avoid missi…
saikishor Apr 22, 2024
e54e613
Add new conditional variable to have unexpected behavior when working…
saikishor Apr 22, 2024
47f6b02
Changes for the starting thread upon activation
saikishor Apr 21, 2024
8525725
remove unused notify at the end of the thread
saikishor Apr 22, 2024
4eb74cc
simplify the logic inside the thread
saikishor May 17, 2024
a131029
add pre-commit formatting changes
saikishor May 17, 2024
b4b2afa
remove the AsyncControllerThread integration to replace it with Async…
saikishor May 17, 2024
ee64bc7
move the async function handler to the realtime_tools and integrate f…
saikishor Jun 12, 2024
469682a
start the async thread when configuring the controller
saikishor Jun 12, 2024
f3b5d43
remove the get_state bind for API change
saikishor Jun 13, 2024
408780c
add new API naming changes
saikishor Jul 17, 2024
8f10adc
added thread priority argument to be able to set the scheduler priority
saikishor Aug 5, 2024
a40b005
stop the thread on cleanup of the controller
saikishor Sep 13, 2024
aa9a81e
wait for cycle to finish in the switch controllers before deactivatin…
saikishor Sep 13, 2024
023a204
fix the thread_priority parameter declaration type
saikishor Sep 13, 2024
3bd19bf
change conditioning to not trigger logging for a failed return as well
saikishor Sep 13, 2024
ebb69c5
Add tests for the async controller
saikishor Sep 13, 2024
e38f567
Update controller_interface/src/controller_interface_base.cpp
saikishor Oct 17, 2024
cbc3638
Merge branch 'master' into async_controllers
saikishor Oct 17, 2024
00228c4
Update controller_interface/include/controller_interface/controller_i…
destogl Oct 17, 2024
df9378f
Fixing pre-commit.
destogl Oct 17, 2024
dd89180
skip triggeting update cycle before deactivating the async controller
saikishor Oct 17, 2024
5a85a24
fix the testing for the recent change in the deactivation scheme
saikishor Oct 17, 2024
75754de
Add controller update stats to print every 20 seconds when there is a…
saikishor Oct 17, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions controller_interface/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ endif()
set(THIS_PACKAGE_INCLUDE_DEPENDS
hardware_interface
rclcpp_lifecycle
realtime_tools
)

find_package(ament_cmake REQUIRED)
Expand Down
112 changes: 0 additions & 112 deletions controller_interface/include/controller_interface/async_controller.hpp

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,11 @@

#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "controller_interface/visibility_control.h"
#include "realtime_tools/async_function_handler.hpp"

#include "hardware_interface/handle.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
Expand Down Expand Up @@ -58,6 +60,17 @@ struct InterfaceConfiguration
std::vector<std::string> names = {};
};

struct ControllerUpdateStats
{
void reset()
{
total_triggers = 0;
failed_triggers = 0;
}

unsigned int total_triggers;
unsigned int failed_triggers;
};
/**
* 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 @@ -153,6 +166,23 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
CONTROLLER_INTERFACE_PUBLIC
virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;

/**
* Trigger update method. This method is used by the controller_manager to trigger the update
* method of the controller.
* The method is used to trigger the update method of the controller synchronously or
* asynchronously, based on the controller configuration.
* **The method called in the (real-time) control loop.**
*
* \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.
*/
CONTROLLER_INTERFACE_PUBLIC
std::pair<bool, return_type> trigger_update(
const rclcpp::Time & time, const rclcpp::Duration & period);

CONTROLLER_INTERFACE_PUBLIC
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();

Expand Down Expand Up @@ -270,15 +300,30 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
CONTROLLER_INTERFACE_PUBLIC
virtual bool is_in_chained_mode() const = 0;

/**
* Method to wait for any running async update cycle to finish after finishing the current cycle.
* This is needed to be called before deactivating the controller by the controller_manager, so
* that the interfaces still exist when the controller finishes its cycle and then it's exits.
*
* \note **The method is not real-time safe and shouldn't be called in the control loop.**
*
* If the controller is running in async mode, the method will wait for the current async update
* to finish. If the controller is not running in async mode, the method will do nothing.
*/
CONTROLLER_INTERFACE_PUBLIC
void wait_for_trigger_update_to_finish();

protected:
std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;

private:
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
unsigned int update_rate_ = 0;
bool is_async_ = false;
std::string urdf_ = "";
ControllerUpdateStats trigger_stats_;
};

using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;
Expand Down
4 changes: 4 additions & 0 deletions controller_interface/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,16 @@
<buildtool_depend>ament_cmake_gen_version_h</buildtool_depend>

<build_depend>hardware_interface</build_depend>
<build_depend>realtime_tools</build_depend>
<build_depend>rclcpp_lifecycle</build_depend>
<build_depend>sensor_msgs</build_depend>

<build_export_depend>hardware_interface</build_export_depend>
<build_export_depend>realtime_tools</build_export_depend>
<build_export_depend>rclcpp_lifecycle</build_export_depend>

<exec_depend>realtime_tools</exec_depend>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

For the future: why not just to have this once using <depend> tag?


<test_depend>ament_cmake_gmock</test_depend>
<test_depend>geometry_msgs</test_depend>
<test_depend>sensor_msgs</test_depend>
Expand Down
56 changes: 54 additions & 2 deletions controller_interface/src/controller_interface_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@

#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "lifecycle_msgs/msg/state.hpp"
Expand All @@ -37,6 +36,7 @@ return_type ControllerInterfaceBase::init(
{
auto_declare<int>("update_rate", update_rate_);
auto_declare<bool>("is_async", false);
auto_declare<int>("thread_priority", 50);
}
catch (const std::exception & e)
{
Expand All @@ -57,7 +57,14 @@ return_type ControllerInterfaceBase::init(
std::bind(&ControllerInterfaceBase::on_configure, this, std::placeholders::_1));

node_->register_on_cleanup(
std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1));
[this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
{
if (is_async() && async_handler_ && async_handler_->is_running())
{
async_handler_->stop_thread();
}
return on_cleanup(previous_state);
});

node_->register_on_activate(
std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1));
Expand Down Expand Up @@ -106,6 +113,21 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
}
is_async_ = get_node()->get_parameter("is_async").as_bool();
}
if (is_async_)
{
const unsigned int thread_priority =
static_cast<unsigned int>(get_node()->get_parameter("thread_priority").as_int());
RCLCPP_INFO(
get_node()->get_logger(), "Starting async handler with scheduler priority: %d",
thread_priority);
async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
async_handler_->init(
std::bind(
&ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2),
thread_priority);
async_handler_->start_thread();
}
trigger_stats_.reset();

return get_node()->configure();
}
Expand All @@ -129,6 +151,29 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c
return node_->get_current_state();
}

std::pair<bool, return_type> ControllerInterfaceBase::trigger_update(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
trigger_stats_.total_triggers++;
if (is_async())
{
const auto result = async_handler_->trigger_async_callback(time, period);
if (!result.first)
{
trigger_stats_.failed_triggers++;
RCLCPP_WARN_THROTTLE(
get_node()->get_logger(), *get_node()->get_clock(), 20000,
"The controller missed %u update cycles out of %u total triggers.",
trigger_stats_.failed_triggers, trigger_stats_.total_triggers);
}
return result;
}
else
{
return std::make_pair(true, update(time, period));
}
}

std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node()
{
if (!node_.get())
Expand All @@ -153,4 +198,11 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; }

const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; }

void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
{
if (is_async() && async_handler_ && async_handler_->is_running())
{
async_handler_->wait_for_trigger_cycle_to_finish();
}
}
} // namespace controller_interface
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
#include <utility>
#include <vector>

#include "controller_interface/async_controller.hpp"
#include "controller_interface/chainable_controller_interface.hpp"
#include "controller_interface/controller_interface.hpp"
#include "controller_interface/controller_interface_base.hpp"
Expand Down Expand Up @@ -628,9 +627,6 @@ class ControllerManager : public rclcpp::Node
};

SwitchParams switch_params_;

std::unordered_map<std::string, std::unique_ptr<controller_interface::AsyncControllerThread>>
async_controller_threads_;
};

} // namespace controller_manager
Expand Down
Loading
Loading