From f800470a07e91cd4a50789fce867b885f6fdca16 Mon Sep 17 00:00:00 2001 From: LiXiaoqi <2360211477@qq.com> Date: Sat, 28 Sep 2024 17:49:30 +0800 Subject: [PATCH] action_server --- .gitignore | 4 + src/action_test/CMakeLists.txt | 16 ++ src/action_test/action/Fibonacci.action | 5 + src/action_test/package.xml | 22 +++ src/cpp_srvcli/CMakeLists.txt | 79 ++++++++ src/cpp_srvcli/LICENSE | 202 +++++++++++++++++++++ src/cpp_srvcli/action/Fibonacci.action | 5 + src/cpp_srvcli/include/cpp_srvcli/aaaa.hpp | 44 +++++ src/cpp_srvcli/msg/Num.msg | 1 + src/cpp_srvcli/msg/Sphere.msg | 2 + src/cpp_srvcli/package.xml | 28 +++ src/cpp_srvcli/src/act_client.cpp | 111 +++++++++++ src/cpp_srvcli/src/act_server.cpp | 99 ++++++++++ src/cpp_srvcli/src/server.cpp | 28 +++ src/cpp_srvcli/src/testact.cpp | 16 ++ src/cpp_srvcli/srv/AddThreeInts.srv | 5 + 16 files changed, 667 insertions(+) create mode 100644 .gitignore create mode 100644 src/action_test/CMakeLists.txt create mode 100644 src/action_test/action/Fibonacci.action create mode 100644 src/action_test/package.xml create mode 100644 src/cpp_srvcli/CMakeLists.txt create mode 100644 src/cpp_srvcli/LICENSE create mode 100644 src/cpp_srvcli/action/Fibonacci.action create mode 100644 src/cpp_srvcli/include/cpp_srvcli/aaaa.hpp create mode 100644 src/cpp_srvcli/msg/Num.msg create mode 100644 src/cpp_srvcli/msg/Sphere.msg create mode 100644 src/cpp_srvcli/package.xml create mode 100644 src/cpp_srvcli/src/act_client.cpp create mode 100644 src/cpp_srvcli/src/act_server.cpp create mode 100644 src/cpp_srvcli/src/server.cpp create mode 100644 src/cpp_srvcli/src/testact.cpp create mode 100644 src/cpp_srvcli/srv/AddThreeInts.srv diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ab20d13 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +.vscode +build +log +install diff --git a/src/action_test/CMakeLists.txt b/src/action_test/CMakeLists.txt new file mode 100644 index 0000000..709309f --- /dev/null +++ b/src/action_test/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(action_test) +set(CMAKE_BUILD_TYPE Debug) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "action/Fibonacci.action" +) +# find dependencies +find_package(ament_cmake REQUIRED) + + +ament_package() diff --git a/src/action_test/action/Fibonacci.action b/src/action_test/action/Fibonacci.action new file mode 100644 index 0000000..32b18f2 --- /dev/null +++ b/src/action_test/action/Fibonacci.action @@ -0,0 +1,5 @@ +int32 order +--- +int32[] sequence +--- +int32[] partial_sequence \ No newline at end of file diff --git a/src/action_test/package.xml b/src/action_test/package.xml new file mode 100644 index 0000000..6323510 --- /dev/null +++ b/src/action_test/package.xml @@ -0,0 +1,22 @@ +<?xml version="1.0"?> +<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> +<package format="3"> + <name>action_test</name> + <version>0.0.0</version> + <description>TODO: Package description</description> + <maintainer email="user@todo.todo">user</maintainer> + <license>TODO: License declaration</license> + + <buildtool_depend>ament_cmake</buildtool_depend> + + <buildtool_depend>rosidl_default_generators</buildtool_depend> + <depend>action_msgs</depend> + <member_of_group>rosidl_interface_packages</member_of_group> + + <test_depend>ament_lint_auto</test_depend> + <test_depend>ament_lint_common</test_depend> + + <export> + <build_type>ament_cmake</build_type> + </export> +</package> diff --git a/src/cpp_srvcli/CMakeLists.txt b/src/cpp_srvcli/CMakeLists.txt new file mode 100644 index 0000000..1bffbd8 --- /dev/null +++ b/src/cpp_srvcli/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.8) +project(cpp_srvcli) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +set(CMAKE_BUILD_TYPE Debug) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(example_interfaces REQUIRED) +find_package(action_test REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Num.msg" + "msg/Sphere.msg" + "srv/AddThreeInts.srv" + DEPENDENCIES geometry_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg +) + + +# add_executable(server src/server.cpp) +# ament_target_dependencies(server rclcpp example_interfaces) + +# action +add_library(action_server SHARED src/act_server.cpp) +target_include_directories(action_server PRIVATE + $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> + $<INSTALL_INTERFACE:include>) +target_compile_definitions(action_server + PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL") +ament_target_dependencies(action_server + "action_test" + "rclcpp" + "rclcpp_action" + "rclcpp_components") +rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE act_server) + +# install( +# TARGETS +# server +# DESTINATION lib/${PROJECT_NAME} +# ) +add_library(action_client SHARED + src/act_client.cpp) +target_include_directories(action_client PRIVATE + $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> + $<INSTALL_INTERFACE:include>) +target_compile_definitions(action_client + PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL") +ament_target_dependencies(action_client + "action_test" + "rclcpp" + "rclcpp_action" + "rclcpp_components") +rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::FibonacciActionClient" EXECUTABLE act_client) + +# add_executable(testact src/testact.cpp) +# target_include_directories(testact PRIVATE +# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> +# $<INSTALL_INTERFACE:include>) +# ament_target_dependencies(testact +# "rclcpp" +# "rclcpp_action") + + +install(TARGETS + action_server + action_client + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +ament_package() diff --git a/src/cpp_srvcli/LICENSE b/src/cpp_srvcli/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/cpp_srvcli/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/cpp_srvcli/action/Fibonacci.action b/src/cpp_srvcli/action/Fibonacci.action new file mode 100644 index 0000000..32b18f2 --- /dev/null +++ b/src/cpp_srvcli/action/Fibonacci.action @@ -0,0 +1,5 @@ +int32 order +--- +int32[] sequence +--- +int32[] partial_sequence \ No newline at end of file diff --git a/src/cpp_srvcli/include/cpp_srvcli/aaaa.hpp b/src/cpp_srvcli/include/cpp_srvcli/aaaa.hpp new file mode 100644 index 0000000..077b967 --- /dev/null +++ b/src/cpp_srvcli/include/cpp_srvcli/aaaa.hpp @@ -0,0 +1,44 @@ +#ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_ +#define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport)) + #define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport)) + #else + #define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport) + #define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport) + #endif + #ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL + #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT + #else + #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT + #endif + #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC + #define ACTION_TUTORIALS_CPP_LOCAL +#else + #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default"))) + #define ACTION_TUTORIALS_CPP_IMPORT + #if __GNUC__ >= 4 + #define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default"))) + #define ACTION_TUTORIALS_CPP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ACTION_TUTORIALS_CPP_PUBLIC + #define ACTION_TUTORIALS_CPP_LOCAL + #endif + #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/src/cpp_srvcli/msg/Num.msg b/src/cpp_srvcli/msg/Num.msg new file mode 100644 index 0000000..5a7c026 --- /dev/null +++ b/src/cpp_srvcli/msg/Num.msg @@ -0,0 +1 @@ +int64 num \ No newline at end of file diff --git a/src/cpp_srvcli/msg/Sphere.msg b/src/cpp_srvcli/msg/Sphere.msg new file mode 100644 index 0000000..5a43e79 --- /dev/null +++ b/src/cpp_srvcli/msg/Sphere.msg @@ -0,0 +1,2 @@ +geometry_msgs/Point center +float64 radius \ No newline at end of file diff --git a/src/cpp_srvcli/package.xml b/src/cpp_srvcli/package.xml new file mode 100644 index 0000000..b9b5c1a --- /dev/null +++ b/src/cpp_srvcli/package.xml @@ -0,0 +1,28 @@ +<?xml version="1.0"?> +<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> +<package format="3"> + <name>cpp_srvcli</name> + <version>0.0.0</version> + <description>TODO: Package description</description> + <maintainer email="user@todo.todo">user</maintainer> + <license>Apache-2.0</license> + + <buildtool_depend>ament_cmake</buildtool_depend> + + <depend>rclcpp</depend> + <depend>example_interfaces</depend> + <depend>action_msgs</depend> + <depend>geometry_msgs</depend> + + + <buildtool_depend>rosidl_default_generators</buildtool_depend> + <exec_depend>rosidl_default_runtime</exec_depend> + <member_of_group>rosidl_interface_packages</member_of_group> + + <test_depend>ament_lint_auto</test_depend> + <test_depend>ament_lint_common</test_depend> + + <export> + <build_type>ament_cmake</build_type> + </export> +</package> diff --git a/src/cpp_srvcli/src/act_client.cpp b/src/cpp_srvcli/src/act_client.cpp new file mode 100644 index 0000000..ffbdb2f --- /dev/null +++ b/src/cpp_srvcli/src/act_client.cpp @@ -0,0 +1,111 @@ +#include <functional> +#include <future> +#include <memory> +#include <string> +#include <sstream> + +#include "action_test/action/fibonacci.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace action_tutorials_cpp +{ +class FibonacciActionClient : public rclcpp::Node +{ +public: + using Fibonacci = action_test::action::Fibonacci; + using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>; + + explicit FibonacciActionClient(const rclcpp::NodeOptions & options) + : Node("fibonacci_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client<Fibonacci>( + this, + "fibonacci"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&FibonacciActionClient::send_goal, this)); + } + + void send_goal() + { + using namespace std::placeholders; + + this->timer_->cancel(); + + if (!this->client_ptr_->wait_for_action_server()) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = Fibonacci::Goal(); + goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&FibonacciActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&FibonacciActionClient::result_callback, this, _1); + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + } + +private: + rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_; + rclcpp::TimerBase::SharedPtr timer_; + + void goal_response_callback(const GoalHandleFibonacci::SharedPtr & goal_handle) + { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + } + + void feedback_callback( + GoalHandleFibonacci::SharedPtr, + const std::shared_ptr<const Fibonacci::Feedback> feedback) + { + std::stringstream ss; + ss << "Next number in sequence received: "; + for (auto number : feedback->partial_sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + } + + void result_callback(const GoalHandleFibonacci::WrappedResult & result) + { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + std::stringstream ss; + ss << "Result received: "; + for (auto number : result.result->sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + rclcpp::shutdown(); + } +}; // class FibonacciActionClient + +} // namespace action_tutorials_cpp + +RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient) \ No newline at end of file diff --git a/src/cpp_srvcli/src/act_server.cpp b/src/cpp_srvcli/src/act_server.cpp new file mode 100644 index 0000000..31f757e --- /dev/null +++ b/src/cpp_srvcli/src/act_server.cpp @@ -0,0 +1,99 @@ +#include <functional> +#include <memory> +#include <thread> + +#include "action_test/action/fibonacci.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "cpp_srvcli/aaaa.hpp" + +namespace action_tutorials_cpp +{ +class FibonacciActionServer : public rclcpp::Node +{ +public: + using Fibonacci = action_test::action::Fibonacci; + using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>; + + ACTION_TUTORIALS_CPP_PUBLIC + explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("fibonacci_action_server", options) + { + using namespace std::placeholders; + + this->action_server_ = rclcpp_action::create_server<Fibonacci>( + this, + "fibonacci", + std::bind(&FibonacciActionServer::handle_goal, this, _1, _2), + std::bind(&FibonacciActionServer::handle_cancel, this, _1), + std::bind(&FibonacciActionServer::handle_accepted, this, _1)); + } + +private: + rclcpp_action::Server<Fibonacci>::SharedPtr action_server_; + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr<const Fibonacci::Goal> 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<GoalHandleFibonacci> 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<GoalHandleFibonacci> 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<GoalHandleFibonacci> goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + rclcpp::Rate loop_rate(1); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared<Fibonacci::Feedback>(); + auto & sequence = feedback->partial_sequence; + sequence.push_back(0); + sequence.push_back(1); + auto result = std::make_shared<Fibonacci::Result>(); + + 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 sequence + sequence.push_back(sequence[i] + sequence[i - 1]); + // Publish feedback + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), "Publish feedback"); + + loop_rate.sleep(); + } + + // Check if goal is done + if (rclcpp::ok()) { + result->sequence = sequence; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } + } +}; // class FibonacciActionServer + +} // namespace action_tutorials_cpp + +RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer) \ No newline at end of file diff --git a/src/cpp_srvcli/src/server.cpp b/src/cpp_srvcli/src/server.cpp new file mode 100644 index 0000000..43ca4d5 --- /dev/null +++ b/src/cpp_srvcli/src/server.cpp @@ -0,0 +1,28 @@ +#include "rclcpp/rclcpp.hpp" +#include "example_interfaces/srv/add_two_ints.hpp" + +#include <memory> + +void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request, + std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response) +{ + response->sum = request->a + request->b; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld", + request->a, request->b); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum); +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server"); + + rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service = + node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints."); + + rclcpp::spin(node); + rclcpp::shutdown(); +} \ No newline at end of file diff --git a/src/cpp_srvcli/src/testact.cpp b/src/cpp_srvcli/src/testact.cpp new file mode 100644 index 0000000..add345b --- /dev/null +++ b/src/cpp_srvcli/src/testact.cpp @@ -0,0 +1,16 @@ +#include <iostream> +#include "rclcpp/rclcpp.hpp" +#include <rclcpp_action/rclcpp_action.hpp> +#include "rosidl_runtime_c/action_type_support_struct.h" +using namespace std; + +int main() { + + auto ts = ROSIDL_GET_ACTION_TYPE_SUPPORT("action_test", "Fibonacci"); + if (ts == nullptr) { + cout << "ts is null" << endl; + } else { + cout << "ts is not null" << endl; + } + +} \ No newline at end of file diff --git a/src/cpp_srvcli/srv/AddThreeInts.srv b/src/cpp_srvcli/srv/AddThreeInts.srv new file mode 100644 index 0000000..9bf1218 --- /dev/null +++ b/src/cpp_srvcli/srv/AddThreeInts.srv @@ -0,0 +1,5 @@ +int64 a +int64 b +int64 c +--- +int64 sum \ No newline at end of file