Skip to content

Commit

Permalink
fixups
Browse files Browse the repository at this point in the history
Signed-off-by: William Woodall <william@osrfoundation.org>
  • Loading branch information
wjwwood committed Apr 16, 2024
1 parent 8e6c9e3 commit 05a8b4c
Show file tree
Hide file tree
Showing 10 changed files with 17 additions and 42 deletions.
8 changes: 5 additions & 3 deletions rclcpp/include/rclcpp/executor.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -82,10 +82,11 @@ class Executor
virtual void
spin() = 0;

/// Version of spin() that takes an exception handler to be called when a callback throws.
/**
* \sa rclcpp::Executor:spin() for more details
* \throws std::runtime_error when spin() called while already spinning
* @param exception_handler will be called for every exception in the processing threads
* \param[in] exception_handler will be called for every exception in the processing threads
*
* The exception_handler can be called from multiple threads at the same time.
* The exception_handler shall rethrow the exception it if wants to terminate the program.
Expand Down Expand Up @@ -474,7 +475,7 @@ class Executor

/// Find the next available executable and do the work associated with it.
/**
* \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
* \param[in] any_exec Structure that can hold any executable type (timer, subscription,
* service, client).
* \throws std::runtime_error if there is an issue triggering the guard condition
*/
Expand All @@ -484,8 +485,9 @@ class Executor

/// Find the next available executable and do the work associated with it.
/**
* \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
* \param[in] any_exec Structure that can hold any executable type (timer, subscription,
* service, client).
* \param[in] exception_handler will be called for every exception in the processing threads
* \throws std::runtime_error if there is an issue triggering the guard condition
*/
RCLCPP_PUBLIC
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ class MultiThreadedExecutor : public rclcpp::Executor
/**
* \sa rclcpp::Executor:spin() for more details
* \throws std::runtime_error when spin() called while already spinning
* @param exception_handler will be called for every exception in the processing threads
* \param[in] exception_handler will be called for every exception in the processing threads
*
* The exception_handler can be called from multiple threads at the same time.
* The exception_handler shall rethrow the exception it if wants to terminate the program.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ class SingleThreadedExecutor : public rclcpp::Executor
/**
* \sa rclcpp::SingleThreadedExecutor:spin() for more details
* \throws std::runtime_error when spin() called while already spinning
* @param exception_handler will be called for every exception in the processing threads
* \param[in] exception_handler will be called for every exception in the processing threads
*
* The exception_handler shall rethrow the exception it if wants to terminate the program.
*/
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor
/**
* \sa rclcpp::SingleThreadedExecutor:spin() for more details
* \throws std::runtime_error when spin() called while already spinning
* @param exception_handler will be called for every exception in the processing threads
* \param[in] exception_handler will be called for every exception in the processing threads
*
* The exception_handler shall rethrow the exception it if wants to terminate the program.
*/RCLCPP_PUBLIC
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ class EventsExecutor : public rclcpp::Executor
/**
* \sa rclcpp::Executor:spin() for more details
* \throws std::runtime_error when spin() called while already spinning
* @param exception_handler will be called for every exception in the processing threads
* \param exception_handler will be called for every exception in the processing threads
*
* The exception_handler can be called from multiple threads at the same time.
* The exception_handler shall rethrow the exception it if wants to terminate the program.
Expand Down
10 changes: 5 additions & 5 deletions rclcpp/src/rclcpp/executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -396,16 +396,17 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
}

template<typename Function>
void execute_guarded(
const Function & fun,
void
execute_guarded(
const Function & function,
const std::function<void(const std::exception & e)> & exception_handler)
{
try {
fun();
function();
} catch (const std::exception & e) {
RCLCPP_ERROR_STREAM(
rclcpp::get_logger("rclcpp"),
"Exception while spinning : " << e.what());
"Exception while spinning: " << e.what());

exception_handler(e);
}
Expand Down Expand Up @@ -457,7 +458,6 @@ Executor::execute_any_executable(
any_exec.callback_group->can_be_taken_from().store(true);
}


template<typename Taker, typename Handler>
static
void
Expand Down
1 change: 0 additions & 1 deletion rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,6 @@ MultiThreadedExecutor::spin()
}

void

MultiThreadedExecutor::spin(const std::function<void(const std::exception & e)> & exception_handler)
{
if (spinning.exchange(true)) {
Expand Down
19 changes: 2 additions & 17 deletions rclcpp/src/rclcpp/executors/single_threaded_executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,27 +27,12 @@ SingleThreadedExecutor::~SingleThreadedExecutor() {}
void
SingleThreadedExecutor::spin()
{
if (spinning.exchange(true)) {
throw std::runtime_error("spin() called while already spinning");
}
RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );

// Clear any previous result and rebuild the waitset
this->wait_result_.reset();
this->entities_need_rebuild_ = true;

while (rclcpp::ok(this->context_) && spinning.load()) {
rclcpp::AnyExecutable any_executable;
if (get_next_executable(any_executable)) {
execute_any_executable(any_executable);
}
}
spin([](const std::exception & e) {throw e;});
}


void
SingleThreadedExecutor::spin(
const std::function<void(const std::exception & e)> & exception_handler)
const std::function<void(const std::exception &)> & exception_handler)
{
if (spinning.exchange(true)) {
throw std::runtime_error("spin() called while already spinning");
Expand Down
12 changes: 1 addition & 11 deletions rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,17 +30,7 @@ StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {}
void
StaticSingleThreadedExecutor::spin()
{
if (spinning.exchange(true)) {
throw std::runtime_error("spin() called while already spinning");
}
RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );

// This is essentially the contents of the rclcpp::Executor::wait_for_work method,
// except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor
// behavior.
while (rclcpp::ok(this->context_) && spinning.load()) {
this->spin_once_impl(std::chrono::nanoseconds(-1));
}
spin([](const std::exception & e) {throw e;});
}


Expand Down
1 change: 0 additions & 1 deletion rclcpp/src/rclcpp/experimental/timers_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,6 @@ void TimersManager::add_timer(rclcpp::TimerBase::SharedPtr timer)
}
}

RCLCPP_PUBLIC
void TimersManager::start(const std::function<void(const std::exception & e)> & exception_handler)
{
// Make sure that the thread is not already running
Expand Down

0 comments on commit 05a8b4c

Please sign in to comment.