diff --git a/examples/ros2/publisher/complex_data.cc b/examples/ros2/publisher/complex_data.cc index fb40cde..0db4cf6 100644 --- a/examples/ros2/publisher/complex_data.cc +++ b/examples/ros2/publisher/complex_data.cc @@ -93,15 +93,13 @@ class RTROS2PublisherThread : public cactus_rt::CyclicThread, public cactus_rt:: } }; -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - +int main(int argc, const char* argv[]) { const cactus_rt::AppConfig app_config; cactus_rt::ros2::Ros2Adapter::Config ros2_adapter_config; ros2_adapter_config.timer_interval = std::chrono::milliseconds(50); - cactus_rt::ros2::App app("ComplexDataROS2Publisher", app_config, ros2_adapter_config); + cactus_rt::ros2::App app(argc, argv, "ComplexDataROS2Publisher", app_config, ros2_adapter_config); app.StartTraceSession("build/publisher.perfetto"); constexpr std::chrono::seconds time(30); @@ -112,9 +110,12 @@ int main(int argc, char* argv[]) { app.Start(); - std::this_thread::sleep_for(time); + std::thread t([&app, &time]() { + std::this_thread::sleep_for(time); + app.RequestStop(); + }); + t.detach(); - app.RequestStop(); app.Join(); std::cout << "Done\n"; diff --git a/examples/ros2/publisher/simple_data.cc b/examples/ros2/publisher/simple_data.cc index 77853e8..575d4ba 100644 --- a/examples/ros2/publisher/simple_data.cc +++ b/examples/ros2/publisher/simple_data.cc @@ -53,15 +53,13 @@ class RTROS2PublisherThread : public cactus_rt::CyclicThread, public cactus_rt:: } }; -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - +int main(int argc, const char* argv[]) { const cactus_rt::AppConfig app_config; cactus_rt::ros2::Ros2Adapter::Config ros2_adapter_config; ros2_adapter_config.timer_interval = std::chrono::milliseconds(50); - cactus_rt::ros2::App app("SimpleDataROS2Publisher", app_config, ros2_adapter_config); + cactus_rt::ros2::App app(argc, argv, "SimpleDataROS2Publisher", app_config, ros2_adapter_config); app.StartTraceSession("build/publisher.perfetto"); constexpr std::chrono::seconds time(30); @@ -72,9 +70,11 @@ int main(int argc, char* argv[]) { app.Start(); - std::this_thread::sleep_for(time); + std::thread t([&app, &time]() { + std::this_thread::sleep_for(time); + app.RequestStop(); + }); - app.RequestStop(); app.Join(); std::cout << "Done\n"; diff --git a/examples/ros2/subscriber/complex_data.cc b/examples/ros2/subscriber/complex_data.cc index a0b16cf..75a8a19 100644 --- a/examples/ros2/subscriber/complex_data.cc +++ b/examples/ros2/subscriber/complex_data.cc @@ -78,10 +78,8 @@ class RTROS2SubscriberThread : public cactus_rt::CyclicThread, public cactus_rt: } }; -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - - cactus_rt::ros2::App app("SimpleDataROS2Subscriber"); +int main(int argc, const char* argv[]) { + cactus_rt::ros2::App app(argc, argv, "SimpleDataROS2Subscriber"); app.StartTraceSession("build/subscriber.perfetto"); constexpr std::chrono::seconds time(30); @@ -92,9 +90,11 @@ int main(int argc, char* argv[]) { app.Start(); - std::this_thread::sleep_for(time); + std::thread t([&app, &time]() { + std::this_thread::sleep_for(time); + app.RequestStop(); + }); - app.RequestStop(); app.Join(); std::cout << "Done\n"; diff --git a/examples/ros2/subscriber/simple_data.cc b/examples/ros2/subscriber/simple_data.cc index a805ac0..b40d3d1 100644 --- a/examples/ros2/subscriber/simple_data.cc +++ b/examples/ros2/subscriber/simple_data.cc @@ -53,10 +53,8 @@ class RTROS2SubscriberThread : public cactus_rt::CyclicThread, public cactus_rt: } }; -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - - cactus_rt::ros2::App app("SimpleDataROS2Subscriber"); +int main(int argc, const char* argv[]) { + cactus_rt::ros2::App app(argc, argv, "SimpleDataROS2Subscriber"); app.StartTraceSession("build/subscriber.perfetto"); constexpr std::chrono::seconds time(30); @@ -67,9 +65,11 @@ int main(int argc, char* argv[]) { app.Start(); - std::this_thread::sleep_for(time); + std::thread t([&app, &time]() { + std::this_thread::sleep_for(time); + app.RequestStop(); + }); - app.RequestStop(); app.Join(); std::cout << "Done\n"; diff --git a/include/cactus_rt/ros2/app.h b/include/cactus_rt/ros2/app.h index 9e93815..848b9fc 100644 --- a/include/cactus_rt/ros2/app.h +++ b/include/cactus_rt/ros2/app.h @@ -41,16 +41,23 @@ class Ros2ExecutorThread : public cactus_rt::Thread, public cactus_rt::ros2::Ros }; class App : public cactus_rt::App { - std::shared_ptr ros2_adapter_; + std::shared_ptr ros2_adapter_; + std::shared_ptr ros2_executor_thread_; + std::thread signal_handling_thread_; + public: - explicit App( + App( + int argc, + const char* argv[], // NOLINT std::string name = "RTROS2App", cactus_rt::AppConfig config = cactus_rt::AppConfig(), Ros2Adapter::Config ros2_adapter_config = Ros2Adapter::Config() ); + ~App() override; + template std::shared_ptr CreateROS2EnabledThread(Args&&... args) { static_assert(std::is_base_of_v, "Must derive ROS2 thread from Ros2ThreadMixin"); diff --git a/include/cactus_rt/signal_handler.h b/include/cactus_rt/signal_handler.h index 7f01cbe..22b5ad5 100644 --- a/include/cactus_rt/signal_handler.h +++ b/include/cactus_rt/signal_handler.h @@ -6,8 +6,6 @@ #include #include -#include "app.h" - /// Namespace comment is needed for doxygen to generate cross references correctly. namespace cactus_rt { /** diff --git a/src/cactus_rt/ros2/app.cc b/src/cactus_rt/ros2/app.cc index db39dd3..a9e97fc 100644 --- a/src/cactus_rt/ros2/app.cc +++ b/src/cactus_rt/ros2/app.cc @@ -3,11 +3,8 @@ #include #include -#include "cactus_rt//utils.h" -#include "cactus_rt/app.h" -#include "cactus_rt/config.h" -#include "cactus_rt/ros2/ros2_adapter.h" -#include "cactus_rt/thread.h" +#include "cactus_rt/signal_handler.h" +#include "cactus_rt/utils.h" namespace cactus_rt::ros2 { @@ -34,13 +31,37 @@ void Ros2ExecutorThread::Run() { executor_->remove_node(node_ptr); } -App::App(std::string name, cactus_rt::AppConfig config, Ros2Adapter::Config ros2_adapter_config) - : cactus_rt::App(name, config), - ros2_adapter_(std::make_shared(name, ros2_adapter_config)) { +App::App( + int argc, + const char* argv[], // NOLINT + std::string name, + cactus_rt::AppConfig config, + Ros2Adapter::Config ros2_adapter_config +) : cactus_rt::App(name, config) { + rclcpp::init(argc, argv, rclcpp::InitOptions(), rclcpp::SignalHandlerOptions::None); + + cactus_rt::SetUpTerminationSignalHandler(); + + signal_handling_thread_ = std::thread([this]() { + cactus_rt::WaitForAndHandleTerminationSignal(); + + RequestStop(); + Join(); + rclcpp::shutdown(); + }); + + signal_handling_thread_.detach(); + + // Must initialize rclcpp before making the Ros2Adapter; + ros2_adapter_ = std::make_shared(name, ros2_adapter_config); ros2_executor_thread_ = CreateROS2EnabledThread(); SetupTraceAggregator(*ros2_executor_thread_); } +App::~App() { + rclcpp::shutdown(); +} + void App::Start(int64_t start_monotonic_time_ns) { // Start the Ros2ExecutorThread first. Don't think it is 100% necessary but why not get a head start. if (start_monotonic_time_ns == -1) { diff --git a/src/cactus_rt/signal_handler.cc b/src/cactus_rt/signal_handler.cc index 3225eae..e135f8b 100644 --- a/src/cactus_rt/signal_handler.cc +++ b/src/cactus_rt/signal_handler.cc @@ -4,6 +4,8 @@ #include #include +#include +#include namespace cactus_rt { /// @private