examples: use NodeWithMode/Executor classes

Auterion · Feb 26, 2024 · ababec0 · ababec0
1 parent 8a49459
commit ababec0
Show file tree

Hide file tree

Showing 12 changed files with 36 additions and 174 deletions.
diff --git a/examples/cpp/modes/fw_attitude/include/mode.hpp b/examples/cpp/modes/fw_attitude/include/mode.hpp
@@ -12,7 +12,6 @@
 #include <rclcpp/rclcpp.hpp>
 
 static const std::string kName = "FW Attitude Example";
-static const std::string kNodeName = "example_mode_fw_attitude";
 
 class FwAttModeTest : public px4_ros2::ModeBase
 {
@@ -39,29 +38,3 @@ class FwAttModeTest : public px4_ros2::ModeBase
   std::shared_ptr<px4_ros2::AttitudeSetpointType> _att_setpoint;
 
 };
-
-class TestNode : public rclcpp::Node
-{
-public:
-  TestNode()
-  : Node(kNodeName)
-  {
-    // Enable debug output
-    auto ret =
-      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
-
-    if (ret != RCUTILS_RET_OK) {
-      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
-      rcutils_reset_error();
-    }
-
-    _mode = std::make_unique<FwAttModeTest>(*this);
-
-    if (!_mode->doRegister()) {
-      throw std::runtime_error("Registration failed");
-    }
-  }
-
-private:
-  std::unique_ptr<FwAttModeTest> _mode;
-};
diff --git a/examples/cpp/modes/fw_attitude/src/main.cpp b/examples/cpp/modes/fw_attitude/src/main.cpp
@@ -6,12 +6,17 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include <mode.hpp>
+#include <px4_ros2/components/node_with_mode.hpp>
 
+using MyNodeWithMode = px4_ros2::NodeWithMode<FwAttModeTest>;
+
+static const std::string kNodeName = "example_mode_fw_attitude";
+static const bool kEnableDebugOutput = true;
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<TestNode>());
+  rclcpp::spin(std::make_shared<MyNodeWithMode>(kNodeName, kEnableDebugOutput));
   rclcpp::shutdown();
   return 0;
 }
diff --git a/examples/cpp/modes/goto/include/mode.hpp b/examples/cpp/modes/goto/include/mode.hpp
@@ -15,7 +15,6 @@
 #include <algorithm>
 
 static const std::string kName = "Go-to Example";
-static const std::string kNodeName = "example_mode_goto";
 
 using namespace px4_ros2::literals; // NOLINT
 
@@ -195,29 +194,3 @@ class FlightModeTest : public px4_ros2::ModeBase
     return fabsf(heading_error_wrapped) < kHeadingErrorThreshold;
   }
 };
-
-class TestNode : public rclcpp::Node
-{
-public:
-  TestNode()
-  : Node(kNodeName)
-  {
-    // Enable debug output
-    auto ret =
-      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
-
-    if (ret != RCUTILS_RET_OK) {
-      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
-      rcutils_reset_error();
-    }
-
-    _mode = std::make_unique<FlightModeTest>(*this);
-
-    if (!_mode->doRegister()) {
-      throw std::runtime_error("Registration failed");
-    }
-  }
-
-private:
-  std::unique_ptr<FlightModeTest> _mode;
-};
diff --git a/examples/cpp/modes/goto/src/main.cpp b/examples/cpp/modes/goto/src/main.cpp
@@ -6,12 +6,17 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include <mode.hpp>
+#include <px4_ros2/components/node_with_mode.hpp>
 
+using MyNodeWithMode = px4_ros2::NodeWithMode<FlightModeTest>;
+
+static const std::string kNodeName = "example_mode_goto";
+static const bool kEnableDebugOutput = true;
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<TestNode>());
+  rclcpp::spin(std::make_shared<MyNodeWithMode>(kNodeName, kEnableDebugOutput));
   rclcpp::shutdown();
   return 0;
 }
diff --git a/examples/cpp/modes/goto_global/include/mode.hpp b/examples/cpp/modes/goto_global/include/mode.hpp
@@ -16,7 +16,6 @@
 #include <algorithm>
 
 static const std::string kName = "Go-to Global Example";
-static const std::string kNodeName = "example_mode_goto_global";
 
 using namespace px4_ros2::literals; // NOLINT
 
@@ -196,29 +195,3 @@ class FlightModeTest : public px4_ros2::ModeBase
     return fabsf(heading_error_wrapped) < kHeadingErrorThreshold;
   }
 };
-
-class TestNode : public rclcpp::Node
-{
-public:
-  TestNode()
-  : Node(kNodeName)
-  {
-    // Enable debug output
-    auto ret =
-      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
-
-    if (ret != RCUTILS_RET_OK) {
-      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
-      rcutils_reset_error();
-    }
-
-    _mode = std::make_unique<FlightModeTest>(*this);
-
-    if (!_mode->doRegister()) {
-      throw std::runtime_error("Registration failed");
-    }
-  }
-
-private:
-  std::unique_ptr<FlightModeTest> _mode;
-};
diff --git a/examples/cpp/modes/goto_global/src/main.cpp b/examples/cpp/modes/goto_global/src/main.cpp
@@ -6,12 +6,17 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include <mode.hpp>
+#include <px4_ros2/components/node_with_mode.hpp>
 
+using MyNodeWithMode = px4_ros2::NodeWithMode<FlightModeTest>;
+
+static const std::string kNodeName = "example_mode_goto_global";
+static const bool kEnableDebugOutput = true;
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<TestNode>());
+  rclcpp::spin(std::make_shared<MyNodeWithMode>(kNodeName, kEnableDebugOutput));
   rclcpp::shutdown();
   return 0;
 }
diff --git a/examples/cpp/modes/manual/include/mode.hpp b/examples/cpp/modes/manual/include/mode.hpp
@@ -17,7 +17,6 @@
 using namespace std::chrono_literals; // NOLINT
 
 static const std::string kName = "My Manual Mode";
-static const std::string kNodeName = "example_mode_manual";
 
 class FlightModeTest : public px4_ros2::ModeBase
 {
@@ -74,29 +73,3 @@ class FlightModeTest : public px4_ros2::ModeBase
   std::shared_ptr<px4_ros2::PeripheralActuatorControls> _peripheral_actuator_controls;
   float _yaw{0.f};
 };
-
-class TestNode : public rclcpp::Node
-{
-public:
-  TestNode()
-  : Node(kNodeName)
-  {
-    // Enable debug output
-    auto ret =
-      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
-
-    if (ret != RCUTILS_RET_OK) {
-      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
-      rcutils_reset_error();
-    }
-
-    _mode = std::make_unique<FlightModeTest>(*this);
-
-    if (!_mode->doRegister()) {
-      throw std::runtime_error("Registration failed");
-    }
-  }
-
-private:
-  std::unique_ptr<FlightModeTest> _mode;
-};
diff --git a/examples/cpp/modes/manual/src/main.cpp b/examples/cpp/modes/manual/src/main.cpp
@@ -6,12 +6,17 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include <mode.hpp>
+#include <px4_ros2/components/node_with_mode.hpp>
 
+using MyNodeWithMode = px4_ros2::NodeWithMode<FlightModeTest>;
+
+static const std::string kNodeName = "example_mode_manual";
+static const bool kEnableDebugOutput = true;
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<TestNode>());
+  rclcpp::spin(std::make_shared<MyNodeWithMode>(kNodeName, kEnableDebugOutput));
   rclcpp::shutdown();
   return 0;
 }
diff --git a/examples/cpp/modes/mode_with_executor/include/mode.hpp b/examples/cpp/modes/mode_with_executor/include/mode.hpp
@@ -16,7 +16,6 @@
 using namespace std::chrono_literals; // NOLINT
 
 static const std::string kName = "Autonomous Executor";
-static const std::string kNodeName = "example_mode_with_executor";
 
 class FlightModeTest : public px4_ros2::ModeBase
 {
@@ -124,35 +123,3 @@ class ModeExecutorTest : public px4_ros2::ModeExecutorBase
 private:
   rclcpp::Node & _node;
 };
-
-class TestNode : public rclcpp::Node
-{
-public:
-  TestNode()
-  : Node(kNodeName)
-  {
-    // Enable debug output
-    auto ret =
-      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
-
-    if (ret != RCUTILS_RET_OK) {
-      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
-      rcutils_reset_error();
-    }
-
-    if (!px4_ros2::waitForFMU(*this)) {
-      throw std::runtime_error("No message from FMU");
-    }
-
-    _mode = std::make_unique<FlightModeTest>(*this);
-    _mode_executor = std::make_unique<ModeExecutorTest>(*this, *_mode);
-
-    if (!_mode_executor->doRegister()) {
-      throw std::runtime_error("Registration failed");
-    }
-  }
-
-private:
-  std::unique_ptr<FlightModeTest> _mode;
-  std::unique_ptr<ModeExecutorTest> _mode_executor;
-};
diff --git a/examples/cpp/modes/mode_with_executor/src/main.cpp b/examples/cpp/modes/mode_with_executor/src/main.cpp
@@ -6,12 +6,17 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include <mode.hpp>
+#include <px4_ros2/components/node_with_mode.hpp>
 
+using MyNodeWithModeExecutor = px4_ros2::NodeWithModeExecutor<ModeExecutorTest, FlightModeTest>;
+
+static const std::string kNodeName = "example_mode_with_executor";
+static const bool kEnableDebugOutput = true;
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<TestNode>());
+  rclcpp::spin(std::make_shared<MyNodeWithModeExecutor>(kNodeName, kEnableDebugOutput));
   rclcpp::shutdown();
   return 0;
 }
diff --git a/examples/cpp/modes/rtl_replacement/include/mode.hpp b/examples/cpp/modes/rtl_replacement/include/mode.hpp
@@ -17,7 +17,6 @@
 using namespace std::chrono_literals; // NOLINT
 
 static const std::string kName = "Custom RTL";
-static const std::string kNodeName = "example_mode_rtl";
 
 class FlightModeTest : public px4_ros2::ModeBase
 {
@@ -59,29 +58,3 @@ class FlightModeTest : public px4_ros2::ModeBase
   rclcpp::Subscription<px4_msgs::msg::VehicleLandDetected>::SharedPtr _vehicle_land_detected_sub;
   std::shared_ptr<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
 };
-
-class TestNode : public rclcpp::Node
-{
-public:
-  TestNode()
-  : Node(kNodeName)
-  {
-    // Enable debug output
-    auto ret =
-      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
-
-    if (ret != RCUTILS_RET_OK) {
-      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
-      rcutils_reset_error();
-    }
-
-    _mode = std::make_unique<FlightModeTest>(*this);
-
-    if (!_mode->doRegister()) {
-      throw std::runtime_error("Registration failed");
-    }
-  }
-
-private:
-  std::unique_ptr<FlightModeTest> _mode;
-};
diff --git a/examples/cpp/modes/rtl_replacement/src/main.cpp b/examples/cpp/modes/rtl_replacement/src/main.cpp
@@ -6,12 +6,17 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include <mode.hpp>
+#include <px4_ros2/components/node_with_mode.hpp>
 
+using MyNodeWithMode = px4_ros2::NodeWithMode<FlightModeTest>;
+
+static const std::string kNodeName = "example_mode_rtl";
+static const bool kEnableDebugOutput = true;
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<TestNode>());
+  rclcpp::spin(std::make_shared<MyNodeWithMode>(kNodeName, kEnableDebugOutput));
   rclcpp::shutdown();
   return 0;
 }