From 3450fdbacc180b219474fb5df1546447fe492033 Mon Sep 17 00:00:00 2001
From: LiXiaoqi <2360211477@qq.com>
Date: Wed, 9 Oct 2024 16:36:27 +0800
Subject: [PATCH] 123

---
 src/action_test/CMakeLists.txt    |   1 +
 src/action_test/srv/AddTwoInt.srv |   3 +
 src/cpp_srvcli/CMakeLists.txt     |   8 +-
 src/cpp_srvcli/src/act_client.cpp |  25 +++-
 src/cpp_srvcli/src/act_server.cpp |  15 ++-
 src/test_arr/CMakeLists.txt       |  20 +++
 src/test_arr/LICENSE              | 202 ++++++++++++++++++++++++++++++
 src/test_arr/package.xml          |  20 +++
 src/test_arr/src/main.cpp         |  26 ++++
 test.json                         |  10 ++
 10 files changed, 322 insertions(+), 8 deletions(-)
 create mode 100644 src/action_test/srv/AddTwoInt.srv
 create mode 100644 src/test_arr/CMakeLists.txt
 create mode 100644 src/test_arr/LICENSE
 create mode 100644 src/test_arr/package.xml
 create mode 100644 src/test_arr/src/main.cpp
 create mode 100644 test.json

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 <memory>
 #include <string>
 #include <sstream>
-
+#include <iostream>
+#include <array>
 #include "action_test/action/fibonacci.hpp"
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
-
+#include <thread>
 namespace action_tutorials_cpp
 {
+using namespace std;
 class FibonacciActionClient : public rclcpp::Node
 {
 public:
   using Fibonacci = action_test::action::Fibonacci;
   using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
+  using CancelResponse = rclcpp_action::Client<Fibonacci>::CancelResponse;
+  using CancelRequest = rclcpp_action::Client<Fibonacci>::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<Fibonacci>::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<const Fibonacci::Goal> 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<GoalHandleFibonacci> 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<Fibonacci::Feedback>();
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 @@
+<?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>test_arr</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="2360211477@qq.com">user</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+
+  <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/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 <rclcpp/rclcpp.hpp>
+#include <action_test/srv/add_two_int.hpp>
+#include <memory>
+
+using namespace std;
+void add(const shared_ptr<action_test::srv::AddTwoInt::Request> req,
+         shared_ptr<action_test::srv::AddTwoInt::Response> 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<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_int_server");
+
+    rclcpp::Service<action_test::srv::AddTwoInt>::SharedPtr service =
+        node->create_service<action_test::srv::AddTwoInt>("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