diff --git a/src/action_test/CMakeLists.txt b/src/action_test/CMakeLists.txt index 709309f..c6c4791 100644 --- a/src/action_test/CMakeLists.txt +++ b/src/action_test/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "action/Fibonacci.action" + "srv/AddTwoInt.srv" ) # find dependencies find_package(ament_cmake REQUIRED) diff --git a/src/action_test/srv/AddTwoInt.srv b/src/action_test/srv/AddTwoInt.srv new file mode 100644 index 0000000..d3d17b8 --- /dev/null +++ b/src/action_test/srv/AddTwoInt.srv @@ -0,0 +1,3 @@ +int32[] nums +--- +int32 sum \ No newline at end of file diff --git a/src/cpp_srvcli/CMakeLists.txt b/src/cpp_srvcli/CMakeLists.txt index 1bffbd8..c41b1e2 100644 --- a/src/cpp_srvcli/CMakeLists.txt +++ b/src/cpp_srvcli/CMakeLists.txt @@ -24,8 +24,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) -# add_executable(server src/server.cpp) -# ament_target_dependencies(server rclcpp example_interfaces) +add_executable(server src/server.cpp) +ament_target_dependencies(server rclcpp example_interfaces) # action add_library(action_server SHARED src/act_server.cpp) @@ -67,7 +67,9 @@ rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::Fibo # ament_target_dependencies(testact # "rclcpp" # "rclcpp_action") - +install(TARGETS + server + DESTINATION lib/${PROJECT_NAME}) install(TARGETS action_server diff --git a/src/cpp_srvcli/src/act_client.cpp b/src/cpp_srvcli/src/act_client.cpp index ffbdb2f..5005937 100644 --- a/src/cpp_srvcli/src/act_client.cpp +++ b/src/cpp_srvcli/src/act_client.cpp @@ -3,20 +3,24 @@ #include #include #include - +#include +#include #include "action_test/action/fibonacci.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_components/register_node_macro.hpp" - +#include namespace action_tutorials_cpp { +using namespace std; class FibonacciActionClient : public rclcpp::Node { public: using Fibonacci = action_test::action::Fibonacci; using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle; + using CancelResponse = rclcpp_action::Client::CancelResponse; + using CancelRequest = rclcpp_action::Client::CancelRequest; explicit FibonacciActionClient(const rclcpp::NodeOptions & options) : Node("fibonacci_action_client", options) @@ -28,6 +32,8 @@ public: this->timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&FibonacciActionClient::send_goal, this)); + cout << sizeof(CancelRequest) << endl; + } void send_goal() @@ -43,7 +49,7 @@ public: auto goal_msg = Fibonacci::Goal(); goal_msg.order = 10; - + RCLCPP_INFO(this->get_logger(), "Sending goal"); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); @@ -53,7 +59,17 @@ public: 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); + auto f = client_ptr_->async_send_goal(goal_msg, send_goal_options); + this_thread::sleep_for(chrono::seconds(2)); + cout << "cancel goal: " << endl; + thread t1([this, f]() { + auto goal_handle = f.get(); + printf("cancel goal: \n"); + client_ptr_ ->async_cancel_goal(goal_handle, [](CancelResponse::SharedPtr) { + printf("cancel response: \n"); + }); + }); + t1.detach(); } private: @@ -79,6 +95,7 @@ private: ss << number << " "; } RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + } void result_callback(const GoalHandleFibonacci::WrappedResult & result) diff --git a/src/cpp_srvcli/src/act_server.cpp b/src/cpp_srvcli/src/act_server.cpp index 31f757e..ffe6791 100644 --- a/src/cpp_srvcli/src/act_server.cpp +++ b/src/cpp_srvcli/src/act_server.cpp @@ -38,7 +38,12 @@ private: std::shared_ptr goal) { RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); - (void)uuid; + printf("uuid: =============="); + for (int i = 0; i < 16; i++) + { + printf("%d ", uuid[i]); + } + printf("\n"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -60,6 +65,14 @@ private: void execute(const std::shared_ptr goal_handle) { RCLCPP_INFO(this->get_logger(), "Executing goal"); + + while(!goal_handle->is_canceling()) + { + //RCLCPP_INFO(this->get_logger(), "Waiting for canceling"); + } + RCLCPP_INFO(this->get_logger(), "Canceling"); + return; + rclcpp::Rate loop_rate(1); const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); diff --git a/src/test_arr/CMakeLists.txt b/src/test_arr/CMakeLists.txt new file mode 100644 index 0000000..0c4e2fe --- /dev/null +++ b/src/test_arr/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.8) +project(test_arr) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(action_test) + +add_executable(test_arr src/main.cpp) +ament_target_dependencies(test_arr rclcpp action_test) + +install(TARGETS + test_arr + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/src/test_arr/LICENSE b/src/test_arr/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/test_arr/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/test_arr/package.xml b/src/test_arr/package.xml new file mode 100644 index 0000000..8def411 --- /dev/null +++ b/src/test_arr/package.xml @@ -0,0 +1,20 @@ + + + + test_arr + 0.0.0 + TODO: Package description + user + Apache-2.0 + + ament_cmake + + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/test_arr/src/main.cpp b/src/test_arr/src/main.cpp new file mode 100644 index 0000000..9549eb3 --- /dev/null +++ b/src/test_arr/src/main.cpp @@ -0,0 +1,26 @@ +#include +#include +#include + +using namespace std; +void add(const shared_ptr req, + shared_ptr res) +{ + int sum = req->nums[0] + req->nums[1]; + res->sum = sum; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "request: %d + %d = %d", req->nums[0], req->nums[1], sum); +} +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_int_server"); + + rclcpp::Service::SharedPtr service = + node->create_service("add_two_int", &add); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two int."); + + rclcpp::spin(node); + rclcpp::shutdown(); +} diff --git a/test.json b/test.json new file mode 100644 index 0000000..9cdc0d9 --- /dev/null +++ b/test.json @@ -0,0 +1,10 @@ +{ + "op": "call_service", + "id": 1, + "service": "/add_two_ints", + "type": "/example_interfaces/srv/AddTwoInts", + "args": { + "a": 1, + "b": 2 + } +} \ No newline at end of file