Skip to content

Commit

Permalink
[AsyncFunctionHandler] Add exception handling (#172)
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor authored Oct 28, 2024
1 parent 7bd84f6 commit 144b12c
Show file tree
Hide file tree
Showing 3 changed files with 200 additions and 8 deletions.
48 changes: 42 additions & 6 deletions include/realtime_tools/async_function_handler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,10 @@ class AsyncFunctionHandler
* If the async callback method is still running, it will return the last return value from the
* last trigger cycle.
*
* \note If an exception is caught in the async callback thread, it will be rethrown in the current
* thread, so in order to have the trigger_async_callback method working again, the exception should
* be caught and the `reset_variables` method should be invoked.
*
* \note In the case of controllers, The controller manager is responsible
* for triggering and maintaining the controller's update rate, as it should be only acting as a
* scheduler. Same applies to the resource manager when handling the hardware components.
Expand All @@ -120,6 +124,12 @@ class AsyncFunctionHandler
if (!is_initialized()) {
throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!");
}
if (async_exception_ptr_) {
RCLCPP_ERROR(
rclcpp::get_logger("AsyncFunctionHandler"),
"AsyncFunctionHandler: Exception caught in the async callback thread!");
std::rethrow_exception(async_exception_ptr_);
}
if (!is_running()) {
throw std::runtime_error(
"AsyncFunctionHandler: need to start the async callback thread first before triggering!");
Expand All @@ -140,6 +150,30 @@ class AsyncFunctionHandler
return std::make_pair(trigger_status, return_value);
}

/// Get the last return value of the async callback method
/**
* @return The last return value of the async callback method
*/
T get_last_return_value() const { return async_callback_return_; }

/// Resets the internal variables of the AsyncFunctionHandler
/**
* A method to reset the internal variables of the AsyncFunctionHandler.
* It will reset the async callback return value, exception pointer, and the trigger status.
*
* \note This method should be invoked after catching an exception in the async callback thread,
* to be able to trigger the async callback method again.
*/
void reset_variables()
{
std::unique_lock<std::mutex> lock(async_mtx_);
stop_async_callback_ = false;
trigger_in_progress_ = false;
last_execution_time_ = 0.0;
async_callback_return_ = T();
async_exception_ptr_ = nullptr;
}

/// Waits until the current async callback method trigger cycle is finished
/**
* If the async method is running, it will wait for the current async method call to finish.
Expand Down Expand Up @@ -229,10 +263,7 @@ class AsyncFunctionHandler
throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!");
}
if (!thread_.joinable()) {
stop_async_callback_ = false;
trigger_in_progress_ = false;
last_execution_time_ = 0.0;
async_callback_return_ = T();
reset_variables();
thread_ = std::thread([this]() -> void {
if (!realtime_tools::configure_sched_fifo(thread_priority_)) {
RCLCPP_WARN(
Expand All @@ -251,8 +282,12 @@ class AsyncFunctionHandler
lock, [this] { return trigger_in_progress_ || stop_async_callback_; });
if (!stop_async_callback_) {
const auto start_time = std::chrono::steady_clock::now();
async_callback_return_ =
async_function_(current_callback_time_, current_callback_period_);
try {
async_callback_return_ =
async_function_(current_callback_time_, current_callback_period_);
} catch (...) {
async_exception_ptr_ = std::current_exception();
}
const auto end_time = std::chrono::steady_clock::now();
last_execution_time_ = std::chrono::duration<double>(end_time - start_time).count();
}
Expand Down Expand Up @@ -281,6 +316,7 @@ class AsyncFunctionHandler
std::condition_variable cycle_end_condition_;
std::mutex async_mtx_;
std::atomic<double> last_execution_time_;
std::exception_ptr async_exception_ptr_;
};
} // namespace realtime_tools

Expand Down
156 changes: 154 additions & 2 deletions test/test_async_function_handler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "test_async_function_handler.hpp"
#include <limits>

#include "gmock/gmock.h"
#include "rclcpp/rclcpp.hpp"
#include "test_async_function_handler.hpp"

namespace realtime_tools
{
Expand All @@ -23,14 +25,19 @@ TestAsyncFunctionHandler::TestAsyncFunctionHandler()
counter_(0),
return_state_(return_type::OK)
{
reset_counter(0);
}

void TestAsyncFunctionHandler::initialize()
{
handler_.init(
std::bind(
&TestAsyncFunctionHandler::update, this, std::placeholders::_1, std::placeholders::_2),
[this]() { return state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; });
[this]() {
return (
state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE &&
handler_.get_last_return_value() != realtime_tools::return_type::DEACTIVATE);
});
}

std::pair<bool, return_type> TestAsyncFunctionHandler::trigger()
Expand All @@ -41,6 +48,9 @@ std::pair<bool, return_type> TestAsyncFunctionHandler::trigger()
return_type TestAsyncFunctionHandler::update(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
if (counter_ == std::numeric_limits<int>::max()) {
throw std::overflow_error("Counter reached maximum value");
}
// to simulate some work being done
std::this_thread::sleep_for(std::chrono::microseconds(10));
counter_++;
Expand All @@ -60,6 +70,13 @@ void TestAsyncFunctionHandler::deactivate()
state_ =
rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state_.label());
}
void TestAsyncFunctionHandler::reset_counter(int counter) { counter_ = counter; }

void TestAsyncFunctionHandler::set_return_state(return_type return_state)
{
return_state_ = return_state;
}

} // namespace realtime_tools
class AsyncFunctionHandlerTest : public ::testing::Test
{
Expand Down Expand Up @@ -255,3 +272,138 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles)
ASSERT_FALSE(async_class.get_handler().is_running());
ASSERT_TRUE(async_class.get_handler().is_stopped());
}

TEST_F(AsyncFunctionHandlerTest, check_triggering_with_different_return_state_and_predicate)
{
realtime_tools::TestAsyncFunctionHandler async_class;
async_class.initialize();
async_class.get_handler().start_thread();

EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
auto trigger_status = async_class.trigger();
ASSERT_TRUE(trigger_status.first);
ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second);
ASSERT_TRUE(async_class.get_handler().is_initialized());
ASSERT_TRUE(async_class.get_handler().is_running());
ASSERT_FALSE(async_class.get_handler().is_stopped());
ASSERT_TRUE(async_class.get_handler().get_thread().joinable());
ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress());
async_class.get_handler().wait_for_trigger_cycle_to_finish();
async_class.get_handler().get_last_execution_time();
ASSERT_EQ(async_class.get_handler().get_last_return_value(), realtime_tools::return_type::OK);
ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress());
ASSERT_EQ(async_class.get_counter(), 1);

// Trigger one more cycle to return ERROR at the end of cycle,
// so return from this cycle should be last cycle's return
async_class.set_return_state(realtime_tools::return_type::ERROR);
trigger_status = async_class.trigger();
ASSERT_TRUE(trigger_status.first);
ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second);
ASSERT_TRUE(async_class.get_handler().is_initialized());
ASSERT_TRUE(async_class.get_handler().is_running());
ASSERT_FALSE(async_class.get_handler().is_stopped());
ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress());
ASSERT_EQ(async_class.get_handler().get_last_return_value(), realtime_tools::return_type::OK);
async_class.get_handler().wait_for_trigger_cycle_to_finish();
ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress());
ASSERT_EQ(async_class.get_handler().get_last_return_value(), realtime_tools::return_type::ERROR);
ASSERT_LE(async_class.get_counter(), 2);

// Trigger one more cycle to return DEACTIVATE at the end of cycle,
async_class.set_return_state(realtime_tools::return_type::DEACTIVATE);
trigger_status = async_class.trigger();
ASSERT_TRUE(trigger_status.first);
ASSERT_EQ(realtime_tools::return_type::ERROR, trigger_status.second);
ASSERT_TRUE(async_class.get_handler().is_initialized());
ASSERT_TRUE(async_class.get_handler().is_running());
ASSERT_FALSE(async_class.get_handler().is_stopped());
ASSERT_FALSE(async_class.get_handler().is_stopped());
ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress());
ASSERT_EQ(async_class.get_handler().get_last_return_value(), realtime_tools::return_type::ERROR);
async_class.get_handler().wait_for_trigger_cycle_to_finish();
ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress());
ASSERT_EQ(
async_class.get_handler().get_last_return_value(), realtime_tools::return_type::DEACTIVATE);
ASSERT_LE(async_class.get_counter(), 3);

// Now the next trigger shouldn't happen as the predicate is set to DEACTIVATE
trigger_status = async_class.trigger();
ASSERT_FALSE(trigger_status.first) << "The trigger should fail as the predicate is DEACTIVATE";
ASSERT_EQ(realtime_tools::return_type::DEACTIVATE, trigger_status.second);
ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress());

async_class.get_handler().stop_thread();
// now the async update should be preempted
ASSERT_FALSE(async_class.get_handler().is_running());
ASSERT_TRUE(async_class.get_handler().is_stopped());
async_class.get_handler().wait_for_trigger_cycle_to_finish();
}

TEST_F(AsyncFunctionHandlerTest, check_exception_handling)
{
realtime_tools::TestAsyncFunctionHandler async_class;
async_class.initialize();
async_class.get_handler().start_thread();

EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
auto trigger_status = async_class.trigger();
ASSERT_TRUE(trigger_status.first);
ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second);
ASSERT_TRUE(async_class.get_handler().is_initialized());
ASSERT_TRUE(async_class.get_handler().is_running());
ASSERT_FALSE(async_class.get_handler().is_stopped());
ASSERT_TRUE(async_class.get_handler().get_thread().joinable());
ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress());
async_class.get_handler().wait_for_trigger_cycle_to_finish();
async_class.get_handler().get_last_execution_time();
ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress());
ASSERT_EQ(async_class.get_counter(), 1);

// Trigger one more cycle
async_class.reset_counter(std::numeric_limits<int>::max());
trigger_status = async_class.trigger();
ASSERT_TRUE(trigger_status.first);
ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second);
ASSERT_TRUE(async_class.get_handler().is_initialized());
ASSERT_TRUE(async_class.get_handler().is_running());
ASSERT_FALSE(async_class.get_handler().is_stopped());
async_class.get_handler().wait_for_trigger_cycle_to_finish();
ASSERT_LE(async_class.get_counter(), std::numeric_limits<int>::max());

std::this_thread::sleep_for(std::chrono::microseconds(10));
// Trigger one more cycle to see exception handling
ASSERT_THROW(async_class.trigger(), std::overflow_error);

// now the async update should be preempted as there was an exception
std::this_thread::sleep_for(std::chrono::microseconds(10));
ASSERT_TRUE(async_class.get_handler().is_running());
ASSERT_FALSE(async_class.get_handler().is_stopped());
async_class.get_handler().wait_for_trigger_cycle_to_finish();

// Should rethrow the exception unless the reset_variables is called
for (int i = 0; i < 50; i++) {
ASSERT_TRUE(async_class.get_handler().is_running());
ASSERT_THROW(async_class.trigger(), std::overflow_error);
}
async_class.get_handler().reset_variables();

async_class.reset_counter(0);
async_class.get_handler().start_thread();
trigger_status = async_class.trigger();
ASSERT_TRUE(trigger_status.first);
ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second);
ASSERT_TRUE(async_class.get_handler().is_initialized());
ASSERT_TRUE(async_class.get_handler().is_running());
ASSERT_FALSE(async_class.get_handler().is_stopped());
ASSERT_TRUE(async_class.get_handler().get_thread().joinable());
ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress());
async_class.get_handler().wait_for_trigger_cycle_to_finish();
async_class.get_handler().get_last_execution_time();
ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress());
ASSERT_EQ(async_class.get_counter(), 1);
async_class.get_handler().stop_thread();
ASSERT_FALSE(async_class.get_handler().is_running());
ASSERT_TRUE(async_class.get_handler().is_stopped());
async_class.get_handler().wait_for_trigger_cycle_to_finish();
}
4 changes: 4 additions & 0 deletions test/test_async_function_handler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,10 @@ class TestAsyncFunctionHandler

void deactivate();

void reset_counter(int counter = 0);

void set_return_state(return_type return_state);

private:
rclcpp_lifecycle::State state_;
int counter_;
Expand Down

0 comments on commit 144b12c

Please sign in to comment.