diff --git a/prover_rclcpp/CMakeLists.txt b/prover_rclcpp/CMakeLists.txt index 80f2914..b3402b5 100644 --- a/prover_rclcpp/CMakeLists.txt +++ b/prover_rclcpp/CMakeLists.txt @@ -42,6 +42,7 @@ find_package(std_msgs REQUIRED) find_package(prover_interfaces REQUIRED) find_package(example_interfaces REQUIRED) find_package(action_tutorials_interfaces REQUIRED) +find_package(action_tutorials_cpp REQUIRED) function(custom_executable target) add_executable(${target} src/${target}.cpp) @@ -54,7 +55,8 @@ function(custom_executable target) "lifecycle_msgs" "rclcpp_lifecycle" "example_interfaces" - "action_tutorials_interfaces") + "action_tutorials_interfaces" + "action_tutorials_cpp") target_compile_options(${target} PRIVATE "-Wall" "-Wextra" "-Werror" "-Wpedantic" "-pthread") target_link_options(${target} @@ -100,6 +102,7 @@ custom_executable(rclcpp_1953) custom_executable(rclcpp_1968) custom_executable(rclcpp_2053) custom_executable(rclcpp_2062) +custom_executable(rclcpp_2101_server) custom_executable(rclcpp_2138) custom_executable(rclcpp_2144) custom_executable(rclcpp_2146) diff --git a/prover_rclcpp/package.xml b/prover_rclcpp/package.xml index b8a8976..9de5174 100644 --- a/prover_rclcpp/package.xml +++ b/prover_rclcpp/package.xml @@ -26,6 +26,7 @@ prover_interfaces example_interfaces action_tutorials_interfaces + action_tutorials_cpp lifecycle_msgs rclcpp @@ -39,6 +40,7 @@ prover_interfaces example_interfaces action_tutorials_interfaces + action_tutorials_cpp ament_cmake_pytest ament_lint_auto diff --git a/prover_rclcpp/src/rclcpp_2101_server.cpp b/prover_rclcpp/src/rclcpp_2101_server.cpp new file mode 100644 index 0000000..cc3d36f --- /dev/null +++ b/prover_rclcpp/src/rclcpp_2101_server.cpp @@ -0,0 +1,78 @@ +#include +#include +#include + +#include "action_tutorials_interfaces/action/fibonacci.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +class FibonacciActionServer : public rclcpp::Node +{ +public: + using Fibonacci = action_tutorials_interfaces::action::Fibonacci; + using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; + + explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("fibonacci_action_server", options) + { + using namespace std::placeholders; + + auto serverOptions = rcl_action_server_get_default_options(); + serverOptions.result_timeout.nanoseconds = 0; + + this->action_server_ = rclcpp_action::create_server( + this, + "fibonacci", + std::bind(&FibonacciActionServer::handle_goal, this, _1, _2), + std::bind(&FibonacciActionServer::handle_cancel, this, _1), + std::bind(&FibonacciActionServer::handle_accepted, this, _1), + serverOptions); + } + +private: + rclcpp_action::Server::SharedPtr action_server_; + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr goal_handle) + { + using namespace std::placeholders; + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach(); + } + + void execute(const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + result->sequence.push_back(42); + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Executing done"); + } +}; // class FibonacciActionServer + +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/prover_rclpy/setup.py b/prover_rclpy/setup.py index fa9282d..0d8723a 100644 --- a/prover_rclpy/setup.py +++ b/prover_rclpy/setup.py @@ -36,6 +36,7 @@ 'listener = src.listener:main', #'rclpy_585 = src.rclpy_585:main', #'rclpy_760 = src.rclpy_760:main', + 'rclcpp_2101_client = src.rclcpp_2101_client:main', 'rclpy_792 = src.rclpy_792:main', 'rclpy_client_822 = src.rclpy_client_822:main', 'rclpy_server_822 = src.rclpy_server_822:main', diff --git a/prover_rclpy/src/rclcpp_2101_client.py b/prover_rclpy/src/rclcpp_2101_client.py new file mode 100644 index 0000000..4f7941f --- /dev/null +++ b/prover_rclpy/src/rclcpp_2101_client.py @@ -0,0 +1,55 @@ +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from action_tutorials_interfaces.action import Fibonacci + + +class FibonacciActionClient(Node): + + def __init__(self): + super().__init__('fibonacci_action_client') + self._action_client = ActionClient(self, Fibonacci, 'fibonacci') + + def send_goal(self, order): + goal_msg = Fibonacci.Goal() + goal_msg.order = order + + self._action_client.wait_for_server() + + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self.get_logger().info("goal_response_callback: get_result_async") + self._get_result_future = goal_handle.get_result_async() + self.get_logger().info("goal_response_callback: add_done_callback(result_callback)") + self._get_result_future.add_done_callback(self.get_result_callback) + self.get_logger().info("goal_response_callback: done") + + def get_result_callback(self, future): + result = future.result() + self.get_logger().info('Result: status {}'.format(result.status)) + self.get_logger().info('Result: {0}'.format(result.result.sequence)) + rclpy.shutdown() + + +def main(args=None): + rclpy.init(args=args) + + action_client = FibonacciActionClient() + + action_client.send_goal(10) + + rclpy.spin(action_client) + + +if __name__ == '__main__': + main()