Signal handling with ROS2

Takes over signal handling in `cactus_rt::ros2::App` by taking over `rclcpp::init` and `rclcpp::shutdown`. This is necessary as rclcpp automatically handles signals otherwise and the signals will not propagate to cactus-rt threads. Fixes #92.
cactusdynamics · Aug 5, 2024 · fc33587 · fc33587
1 parent 23c59c8
commit fc33587
Show file tree

Hide file tree

Showing 8 changed files with 65 additions and 36 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -41,16 +41,23 @@ class Ros2ExecutorThread : public cactus_rt::Thread, public cactus_rt::ros2::Ros
 };
 
 class App : public cactus_rt::App {
- std::shared_ptr<Ros2Adapter> ros2_adapter_;
+ std::shared_ptr<Ros2Adapter> ros2_adapter_;
+
  std::shared_ptr<Ros2ExecutorThread> 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 <typename ThreadT, typename... Args>
  std::shared_ptr<ThreadT> CreateROS2EnabledThread(Args&&... args) {
  static_assert(std::is_base_of_v<Ros2ThreadMixin, ThreadT>, "Must derive ROS2 thread from Ros2ThreadMixin");

diff --git a/include/cactus_rt/signal_handler.h b/include/cactus_rt/signal_handler.h
@@ -6,8 +6,6 @@
 #include <csignal>
 #include <vector>
 
-#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
@@ -3,11 +3,8 @@
 #include <rclcpp/executors/single_threaded_executor.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#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<Ros2Adapter>(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<Ros2Adapter>(name, ros2_adapter_config);
  ros2_executor_thread_ = CreateROS2EnabledThread<Ros2ExecutorThread>();
  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
@@ -4,6 +4,8 @@
 
 #include <atomic>
 #include <cstring>
+#include <stdexcept>
+#include <string>
 
 namespace cactus_rt {
 /// @private