action 客户端服务端写完

protocol_ws/cpp_prococol/src/cpp_client_acton.cpp

@@ -2,14 +2,102 @@
 本程序用于创建 ROS2 客户端Action节点，演示如何发送Action请求并处理结果。
 */
 
-
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "cpp_prococol/action/move_to.hpp"
 
+using MoveTo = cpp_prococol::action::MoveTo;
 
-int main(int argc,char *argv[])
+class MoveToClient : public rclcpp::Node
 {
+public:
+    MoveToClient() : Node("move_to_client")
+    {
+        // 创建一个 action 客户端，指定 action 类型为 MoveTo，名称为 "move_to"。
+        action_client_ = rclcpp_action::create_client<MoveTo>(this, "move_to");
 
-    
+        if (!WaitServer())
+        {
+            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
+            rclcpp::shutdown();
+        }
+        MoveToPos(5.0f, 5.0f);
+    }
+
+    // 发送一个目标请求，并处理结果的回调函数。
+    void MoveToPos(float x, float y)
+    {
+        MoveTo::Goal goal_msg;
+        goal_msg.target_x = x;
+        goal_msg.target_y = y;
+
+        // 发送目标请求，并绑定结果回调函数。
+        rclcpp_action::Client<MoveTo>::SendGoalOptions options;
+        options.goal_response_callback = std::bind(&MoveToClient::goal_response_callback, this, std::placeholders::_1);
+        options.feedback_callback = std::bind(&MoveToClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
+        options.result_callback = std::bind(&MoveToClient::result_callback, this, std::placeholders::_1);
+
+        // 异步发送目标请求，结果通过回调函数处理。
+        action_client_->async_send_goal(goal_msg, options);
+    }
+
+    // *** 定义回调 ***
+    //  绑定目标响应回调函数，当服务器接受或拒绝目标时调用。
+    void goal_response_callback(std::shared_ptr<rclcpp_action::ClientGoalHandle<MoveTo>> 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(std::shared_ptr<rclcpp_action::ClientGoalHandle<MoveTo>> goal_handle, const std::shared_ptr<const MoveTo::Feedback> feedback)
+    {
+        RCLCPP_INFO(this->get_logger(), "Received feedback: current progress:(%.2f)", feedback->progress);
+    }
+
+    // 绑定结果回调函数，当服务器发送结果时调用。
+    void result_callback(const rclcpp_action::ClientGoalHandle<MoveTo>::WrappedResult & result)
+    {
+        switch (result.code)
+        {
+        case rclcpp_action::ResultCode::SUCCEEDED:
+            RCLCPP_INFO(this->get_logger(), "Goal succeeded: %s", result.result->message.c_str());
+            break;
+        case rclcpp_action::ResultCode::ABORTED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
+            break;
+        case rclcpp_action::ResultCode::CANCELED:
+            RCLCPP_WARN(this->get_logger(), "Goal was canceled");
+            break;
+        default:
+            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+            break;
+        }
+    }
+
+
+    // 等待服务端就绪就绪
+    bool WaitServer()
+    {
+        // 等待 action 服务器就绪，超时时间为 10 秒。
+        return action_client_->wait_for_action_server(std::chrono::seconds(10));
+    }
+
+private:
+    rclcpp_action::Client<MoveTo>::SharedPtr action_client_;
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<MoveToClient>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
 }