adding servers for demos

README.md

@@ -204,7 +204,7 @@ if __name__ == "__main__":
 #### Service Demo (FSM + ROS 2 Service Client)
 
 ```shell
-$ ros2 run demo_nodes_py add_two_ints_server
+$ ros2 run yasmin_demos add_two_ints_server
 ```
 
 ```shell
@@ -325,7 +325,7 @@ if __name__ == "__main__":
 #### Action Demo (FSM + ROS 2 Action)
 
 ```shell
-$ ros2 run action_tutorials_cpp fibonacci_action_server
+$ ros2 run yasmin_demos fibonacci_action_server
 ```
 
 ```shell
@@ -376,7 +376,7 @@ class FibonacciState(ActionState):
     def print_feedback(
         self, blackboard: Blackboard, feedback: Fibonacci.Feedback
     ) -> None:
-        yasmin.YASMIN_LOG_INFO(f"Received feedback: {list(feedback.partial_sequence)}")
+        yasmin.YASMIN_LOG_INFO(f"Received feedback: {list(feedback.sequence)}")
 
 
 def print_result(blackboard: Blackboard) -> str:
@@ -813,7 +813,7 @@ int main(int argc, char *argv[]) {
 #### Service Demo (FSM + ROS 2 Service Client)
 
 ```shell
-$ ros2 run demo_nodes_py add_two_ints_server
+$ ros2 run yasmin_demos add_two_ints_server
 ```
 
 ```shell
@@ -956,7 +956,7 @@ int main(int argc, char *argv[]) {
 #### Action Demo (FSM + ROS 2 Action)
 
 ```shell
-$ ros2 run action_tutorials_cpp fibonacci_action_server
+$ ros2 run yasmin_demos fibonacci_action_server
 ```
 
 ```shell

yasmin_demos/CMakeLists.txt

@@ -39,6 +39,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(yasmin REQUIRED)
 find_package(yasmin_ros REQUIRED)
@@ -53,6 +54,7 @@ include_directories(src)
 set(LIB ${CMAKE_PROJECT_NAME}_lib)
 set(DEPENDENCIES
   rclcpp
+  rclcpp_action
   yasmin
   yasmin_ros
   yasmin_viewer
@@ -92,6 +94,22 @@ install(TARGETS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+# add_two_ints service server
+add_executable(add_two_ints_server src/add_two_ints_server.cpp)
+ament_target_dependencies(add_two_ints_server ${DEPENDENCIES})
+install(TARGETS
+add_two_ints_server
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# fibonacci action server
+add_executable(fibonacci_action_server src/fibonacci_action_server.cpp)
+ament_target_dependencies(fibonacci_action_server ${DEPENDENCIES})
+install(TARGETS
+fibonacci_action_server
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_export_include_directories(include)
 ament_export_libraries(${LIB})
 

yasmin_demos/src/add_two_ints_server.cpp

@@ -0,0 +1,95 @@
+// Copyright (C) 2024  Miguel Ángel González Santamarta
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+#include <chrono>
+#include <cinttypes>
+#include <memory>
+
+#include "example_interfaces/srv/add_two_ints.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+/**
+ * @class ServerNode
+ * @brief A ROS 2 service server node for adding two integers.
+ *
+ * This node provides a service named "add_two_ints" that accepts two integers
+ * as input and returns their sum. It also supports an optional one-shot mode
+ * that shuts down the server after handling the first request.
+ */
+class ServerNode final : public rclcpp::Node {
+public:
+  /**
+   * @brief Constructor for the ServerNode class.
+   *
+   * Initializes the service server and an optional one-shot timer.
+   * @param options Node options for initialization.
+   */
+  explicit ServerNode() : Node("add_two_ints_server") {
+
+    // Callback to handle "add_two_ints" service requests.
+    auto handle_add_two_ints =
+        [this](
+            const std::shared_ptr<rmw_request_id_t> request_header,
+            const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request>
+                request,
+            std::shared_ptr<example_interfaces::srv::AddTwoInts::Response>
+                response) -> void {
+      (void)request_header;
+      RCLCPP_INFO(this->get_logger(),
+                  "Incoming request\na: %" PRId64 " b: %" PRId64, request->a,
+                  request->b);
+      response->sum = request->a + request->b;
+    };
+
+    // Create a service that will use the callback function to handle requests.
+    this->srv_ = create_service<example_interfaces::srv::AddTwoInts>(
+        "add_two_ints", handle_add_two_ints);
+
+    RCLCPP_INFO(this->get_logger(), "Add Two Ints Service started");
+  }
+
+private:
+  /**
+   * @brief Shared pointer to the "add_two_ints" service.
+   */
+  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_;
+
+  /**
+   * @brief Timer for one-shot mode to shut down the node after a request.
+   */
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  /**
+   * @brief Flag indicating if a request has been received.
+   */
+  bool saw_request_{false};
+};
+
+/**
+ * @brief Main entry point for the ServerNode application.
+ *
+ * Initializes the ROS 2 node, spins it to handle incoming service requests,
+ * and shuts down gracefully when done.
+ * @param argc Number of command-line arguments.
+ * @param argv Array of command-line arguments.
+ * @return Exit status.
+ */
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<ServerNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

yasmin_demos/src/fibonacci_action_server.cpp

@@ -0,0 +1,158 @@
+// Copyright (C) 2024  Miguel Ángel González Santamarta
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+#include <memory>
+
+#include "example_interfaces/action/fibonacci.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+using namespace std::placeholders;
+
+/**
+ * @class FibonacciActionServer
+ * @brief A ROS 2 action server node for calculating Fibonacci sequences.
+ */
+class FibonacciActionServer : public rclcpp::Node {
+public:
+  /**
+   * @brief Alias for the Fibonacci action type.
+   */
+  using Fibonacci = example_interfaces::action::Fibonacci;
+
+  /**
+   * @brief Alias for the goal handle of the Fibonacci action.
+   */
+  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  /**
+   * @brief Constructor for the FibonacciActionServer.
+   *
+   * Initializes the action server and sets up the goal, cancel, and accepted
+   * callbacks.
+   * @param options Node options for initialization.
+   */
+  explicit FibonacciActionServer(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : Node("fibonacci_action_server", options) {
+
+    // Callback to handle goal requests.
+    auto handle_goal = [this](const rclcpp_action::GoalUUID &uuid,
+                              std::shared_ptr<const Fibonacci::Goal> goal) {
+      (void)uuid;
+      RCLCPP_INFO(this->get_logger(), "Received goal request with order %d",
+                  goal->order);
+      if (goal->order > 46) {
+        return rclcpp_action::GoalResponse::REJECT;
+      }
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+    // Callback to handle goal cancellation requests.
+    auto handle_cancel =
+        [this](const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
+          RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+          (void)goal_handle;
+          return rclcpp_action::CancelResponse::ACCEPT;
+        };
+
+    // Callback to handle accepted goals.
+    auto handle_accepted =
+        [this](const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
+          auto execute_in_thread = [this, goal_handle]() {
+            return this->execute(goal_handle);
+          };
+          std::thread{execute_in_thread}.detach();
+        };
+
+    // Create the Fibonacci action server.
+    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
+        this, "fibonacci", handle_goal, handle_cancel, handle_accepted);
+
+    RCLCPP_INFO(this->get_logger(), "Fibonacci Server started");
+  }
+
+private:
+  /**
+   * @brief The action server instance for Fibonacci calculations.
+   */
+  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
+
+  /**
+   * @brief Executes the Fibonacci calculation for a given goal.
+   *
+   * Generates the Fibonacci sequence up to the requested order, providing
+   * feedback to the client and handling cancellation requests.
+   * @param goal_handle Shared pointer to the goal handle.
+   */
+  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
+    RCLCPP_INFO(this->get_logger(), "Executing goal");
+
+    rclcpp::Rate rate(1);
+
+    // Retrieve the goal details.
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<Fibonacci::Feedback>();
+
+    // Initialize the Fibonacci sequence with the first two numbers.
+    auto &sequence = feedback->sequence;
+    sequence.push_back(0);
+    sequence.push_back(1);
+
+    auto result = std::make_shared<Fibonacci::Result>();
+
+    // Generate the Fibonacci sequence up to the requested order.
+    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
+      // Check if there is a cancel request.
+      if (goal_handle->is_canceling()) {
+        result->sequence = sequence;
+        goal_handle->canceled(result);
+        RCLCPP_INFO(this->get_logger(), "Goal canceled");
+        return;
+      }
+      // Update the sequence with the next number.
+      sequence.push_back(sequence[i] + sequence[i - 1]);
+      // Publish feedback to the client.
+      goal_handle->publish_feedback(feedback);
+      RCLCPP_INFO(this->get_logger(), "Publish feedback");
+
+      rate.sleep();
+    }
+
+    // Complete the goal if the process is not canceled.
+    if (rclcpp::ok()) {
+      result->sequence = sequence;
+      goal_handle->succeed(result);
+      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+    }
+  }
+}; // class FibonacciActionServer
+
+/**
+ * @brief Main entry point for the Fibonacci action server.
+ *
+ * Initializes the ROS 2 node, spins it to handle incoming requests, and shuts
+ * down gracefully.
+ * @param argc Number of command-line arguments.
+ * @param argv Array of command-line arguments.
+ * @return Exit status.
+ */
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<FibonacciActionServer>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

yasmin_demos/yasmin_demos/action_client_demo.py

@@ -121,7 +121,7 @@ def print_feedback(
         Raises:
             None
         """
-        yasmin.YASMIN_LOG_INFO(f"Received feedback: {list(feedback.partial_sequence)}")
+        yasmin.YASMIN_LOG_INFO(f"Received feedback: {list(feedback.sequence)}")
 
 
 def print_result(blackboard: Blackboard) -> str: