diff --git a/README.md b/README.md index 438ddcc..74da78f 100644 --- a/README.md +++ b/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 diff --git a/yasmin_demos/CMakeLists.txt b/yasmin_demos/CMakeLists.txt index f0a3a32..d6ad377 100644 --- a/yasmin_demos/CMakeLists.txt +++ b/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}) diff --git a/yasmin_demos/src/add_two_ints_server.cpp b/yasmin_demos/src/add_two_ints_server.cpp new file mode 100644 index 0000000..b4b78a0 --- /dev/null +++ b/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 . + +#include +#include +#include + +#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 request_header, + const std::shared_ptr + request, + std::shared_ptr + 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( + "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::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(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/yasmin_demos/src/fibonacci_action_server.cpp b/yasmin_demos/src/fibonacci_action_server.cpp new file mode 100644 index 0000000..e4fb635 --- /dev/null +++ b/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 . + +#include + +#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; + + /** + * @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 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 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 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( + 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::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 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(); + + // 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(); + + // 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(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/yasmin_demos/yasmin_demos/action_client_demo.py b/yasmin_demos/yasmin_demos/action_client_demo.py index af3968f..5a328d0 100755 --- a/yasmin_demos/yasmin_demos/action_client_demo.py +++ b/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: