Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
12 changes: 8 additions & 4 deletions ianvs/include/ianvs/spin_functions.h
Original file line number Diff line number Diff line change
Expand Up @@ -33,18 +33,22 @@
* purposes notwithstanding any copyright notation herein.
* -------------------------------------------------------------------------- */
#pragma once
#include <std_srvs/srv/empty.hpp>

#include "ianvs/node_handle.h"

namespace ianvs {

void spinWhileClockPresent(NodeHandle nh, rclcpp::Executor* executor = nullptr);
void spinWhileClockPresent(NodeHandle nh,
rclcpp::Executor* executor = nullptr,
const std::string& service_name = "~/shutdown");

void spinUntilExitRequested(NodeHandle nh, rclcpp::Executor* executor = nullptr);
void spinUntilExitRequested(NodeHandle nh,
rclcpp::Executor* executor = nullptr,
const std::string& service_name = "~/shutdown");

void spinAndWait(NodeHandle nh,
bool exit_after_clock = false,
rclcpp::Executor* executor = nullptr);
rclcpp::Executor* executor = nullptr,
const std::string& service_name = "~/shutdown");

} // namespace ianvs
33 changes: 20 additions & 13 deletions ianvs/src/spin_functions.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,10 +36,12 @@

#include <rclcpp/executors.hpp>
#include <rclcpp/rate.hpp>
#include <std_srvs/srv/empty.hpp>

namespace ianvs {

using BaseInterface = std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>;
using std_srvs::srv::Empty;

namespace {

Expand All @@ -54,27 +56,27 @@ inline void spin_some(const BaseInterface& node, rclcpp::Executor* exec) {
} // namespace

struct ShutdownMonitor {
explicit ShutdownMonitor(NodeHandle nh)
ShutdownMonitor(NodeHandle nh, const std::string& name)
: should_exit(false),
service(nh.create_service<std_srvs::srv::Empty>(
"shutdown", &ShutdownMonitor::callback, this)) {}
service(nh.create_service<Empty>(name, &ShutdownMonitor::callback, this)) {}

void callback(const std_srvs::srv::Empty::Request::SharedPtr&,
std_srvs::srv::Empty::Response::SharedPtr) {
void callback(const Empty::Request::SharedPtr&, Empty::Response::SharedPtr) {
should_exit = true;
}

bool should_exit;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service;
rclcpp::Service<Empty>::SharedPtr service;
};

bool haveClock(NodeHandle nh) {
auto graph = nh.node().get<rclcpp::node_interfaces::NodeGraphInterface>();
return graph->count_publishers("/clock") > 0;
}

void spinWhileClockPresent(NodeHandle nh, rclcpp::Executor* executor) {
ShutdownMonitor functor(nh);
void spinWhileClockPresent(NodeHandle nh,
rclcpp::Executor* executor,
const std::string& service_name) {
ShutdownMonitor functor(nh, service_name);

rclcpp::WallRate r(50);
auto base = nh.node().get<rclcpp::node_interfaces::NodeBaseInterface>();
Expand All @@ -98,8 +100,10 @@ void spinWhileClockPresent(NodeHandle nh, rclcpp::Executor* executor) {
RCLCPP_INFO(nh.logger(), "Exiting!");
}

void spinUntilExitRequested(NodeHandle nh, rclcpp::Executor* executor) {
ShutdownMonitor functor(nh);
void spinUntilExitRequested(NodeHandle nh,
rclcpp::Executor* executor,
const std::string& service_name) {
ShutdownMonitor functor(nh, service_name);

rclcpp::WallRate r(50);
auto base = nh.node().get<rclcpp::node_interfaces::NodeBaseInterface>();
Expand All @@ -117,11 +121,14 @@ void spinUntilExitRequested(NodeHandle nh, rclcpp::Executor* executor) {
RCLCPP_INFO(nh.logger(), "Exiting!");
}

void spinAndWait(NodeHandle nh, bool exit_after_clock, rclcpp::Executor* executor) {
void spinAndWait(NodeHandle nh,
bool exit_after_clock,
rclcpp::Executor* executor,
const std::string& service_name) {
if (exit_after_clock) {
spinWhileClockPresent(nh, executor);
spinWhileClockPresent(nh, executor, service_name);
} else {
spinUntilExitRequested(nh, executor);
spinUntilExitRequested(nh, executor, service_name);
}
}

Expand Down
Loading