init

protocol_ws/cpp_prococol/src/cpp_client_acton.cpp

@@ -0,0 +1,15 @@
+/*
+本程序用于创建 ROS2 客户端Action节点，演示如何发送Action请求并处理结果。
+*/
+
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include "cpp_prococol/action/move_to.hpp"
+
+
+int main(int argc,char *argv[])
+{
+
+    
+}

protocol_ws/cpp_prococol/src/cpp_client_service.cpp

@@ -0,0 +1,71 @@
+/*
+本程序用于创建客户端节点,并调用服务端提供的服务接口，发送请求并处理响应。
+*/
+
+#include "rclcpp/rclcpp.hpp"
+#include "cpp_prococol/srv/position.hpp"
+#include <chrono>
+
+using Positon = cpp_prococol::srv::Position;
+
+class cppClientService : public rclcpp::Node
+{
+public:
+    // 构造函数，创建客户端对象
+    cppClientService() : Node("cpp_client_service")
+    {
+        client_ = this->create_client<Positon>("position");
+    }
+
+    // 发送请求的函数
+    void send_request()
+    {
+        // 等待服务端准备就绪
+        while (!client_->wait_for_service(std::chrono::seconds(1)))
+        {
+            if (!rclcpp::ok())
+            {
+                RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
+                return;
+            }
+            RCLCPP_INFO(this->get_logger(), "Waiting for service to be available...");
+        }
+
+        // 构建请求体
+        auto request = std::make_shared<Positon::Request>();
+        request->target_x = 1.0;
+        request->target_y = 2.0;
+
+        // 定义处理响应的回调函数
+        auto res_callback = [this](rclcpp::Client<Positon>::SharedFuture future)
+        {
+            auto res = future.get();
+            if (res->result == Positon::Response::SUCCESS)
+            {
+                RCLCPP_INFO(this->get_logger(), "Service call succeeded: target position is valid.");
+            }
+            else
+            {
+                RCLCPP_WARN(this->get_logger(), "Service call failed: target position is invalid.");
+            }
+        };
+
+        // 发送请求体并处理响应
+        auto res = client_->async_send_request(request, res_callback);
+    }
+
+private:
+    // 声明客户端对象
+    rclcpp::Client<Positon>::SharedPtr client_;
+};
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+
+    auto node = std::make_shared<cppClientService>();
+    node->send_request();
+
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+}

protocol_ws/cpp_prococol/src/cpp_pub_topic.cpp

@@ -0,0 +1,50 @@
+/*
+本程序用于演示如何在ROS2中创建一个简单的发布者节点，发布字符串消息到一个主题上。
+*/
+
+#include <chrono> // 包含时间相关的头文件
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp" // 包含字符串消息类型的头文件
+
+using StringMsg = std_msgs::msg::String; // 定义一个别名，方便后续使用
+
+
+// 简单的话题发布者节点类，继承自rclcpp::Node
+// 作用: 创建一个发布者节点，定期发布字符串消息到指定主题上。
+class MinimalPublisher : public rclcpp::Node
+{
+public:
+    MinimalPublisher():Node("minimal_publisher"),count_(0) // 构造函数，初始化节点名称和消息计数器
+    {
+        // 创建一个发布者对象，发布到"chatter"主题，使用默认QoS设置
+        publisher_ = this->create_publisher<StringMsg>("chatter", 10);
+        // 创建一个定时器对象,每隔5s钟调用一次发送一次消息的函数
+        timer_ = this->create_wall_timer(std::chrono::seconds(5),std::bind(&MinimalPublisher::timer_callback, this));
+    }
+
+    // 定时器回调函数，每次被调用时发布一条消息
+    void timer_callback()
+    {
+        StringMsg msg;
+        msg.data = "Hello ROS2! -- " + std::to_string(count_++); // 设置消息内容，包含计数器值
+        RCLCPP_INFO(this->get_logger(),"Publishing: '%s'", msg.data.c_str()); // 打印日志，显示发布的消息内容
+        publisher_->publish(msg); // 发布消息
+    }
+
+private:
+    rclcpp::Publisher<StringMsg>::SharedPtr publisher_; // 发布者对象
+    rclcpp::TimerBase::SharedPtr timer_; // 定时器对象
+    size_t count_; // 消息计数器
+};
+
+int main(int argc,char *argv[])
+{
+    rclcpp::init(argc,argv); // 初始化ROS2客户端库
+
+    auto node = std::make_shared<MinimalPublisher>(); // 创建发布者节点对象
+
+    rclcpp::spin(node); // 进入循环，等待回调函数被调用
+    rclcpp::shutdown(); // 关闭ROS2客户端库
+    return 0;
+}

protocol_ws/cpp_prococol/src/cpp_server_action.cpp

@@ -0,0 +1,140 @@
+/*
+本程序用于创建 action 服务器，接收客户端发送的 goal，并执行相应的操作。
+*/
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "cpp_prococol/action/move_to.hpp"
+
+using MoveTo = cpp_prococol::action::MoveTo;
+
+class MoveToActionServer : public rclcpp::Node
+{
+public:
+    MoveToActionServer() : Node("move_to_action_server")
+    {
+        /*
+         * 创建一个 action 服务器，指定 action 类型为 MoveTo，名称为 "move_to"。
+         * 绑定三个回调函数：handle_goal 用于处理接收到的 goal 请求，handle_cancel 用于处理取消请求，handle_accepted 用于处理接受请求。
+         */
+        action_server_ = rclcpp_action::create_server<MoveTo>(
+            this,
+            "move_to",
+            std::bind(&MoveToActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
+            std::bind(&MoveToActionServer::handle_cancel, this, std::placeholders::_1),
+            std::bind(&MoveToActionServer::handle_accepted, this, std::placeholders::_1));
+
+        RCLCPP_INFO(this->get_logger(), "Action server 'move_to' has been started.");
+    }
+
+    /*
+        * 处理接收到的 goal 请求的回调函数。
+        Params:
+        - uuid: 目标的唯一标识符。
+        - goal: 接收到的目标消息。
+        Returns:
+        - GoalResponse: 表示是否接受该目标的响应。
+    */
+    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const MoveTo::Goal> goal)
+    {
+        RCLCPP_INFO(this->get_logger(), "Received goal request with target position:(%.2f, %.2f)", goal->target_x, goal->target_y);
+        // 目标位置验证
+        if (goal->target_x < 0 || goal->target_y < 0)
+        {
+            // 如果目标位置无效，拒绝该目标请求
+            return rclcpp_action::GoalResponse::REJECT;
+        }
+        // 目标位置有效,接收此请求并开始执行
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    }
+
+    /*
+        * 处理接收到的取消请求的回调函数。
+        Params:
+        - goal_handle: 目标句柄。
+        Returns:
+        - CancelResponse: 表示是否接受取消请求的响应。
+    */
+    rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveTo>> goal_handle)
+    {
+        RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+        // 这里可以添加取消目标的逻辑
+        auto goal = goal_handle->get_goal();
+        RCLCPP_INFO(this->get_logger(), "Canceling goal with target position: (%.2f, %.2f)", goal->target_x, goal->target_y);
+
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    /*
+        * 处理接受请求的回调函数。
+        Params:
+        - goal_handle: 目标句柄。
+    */
+    void handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveTo>> goal_handle)
+    {
+        std::thread{std::bind(&MoveToActionServer::execute, this, std::placeholders::_1), goal_handle}.detach();
+    }
+
+    /*
+        * 执行目标的函数。
+        Params:
+        - goal_handle: 目标句柄。
+    */
+    void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveTo>> goal_handle)
+    {
+        RCLCPP_INFO(this->get_logger(), "Executing goal");
+        // 这里可以添加执行目标的逻辑，并在适当的时候调用 goal_handle->succeed() 或 goal_handle->abort()
+        // 设置每个周期的频率:10hz = 0.1s/次
+        rclcpp::Rate loop_rate(10);
+        // 初始化progress == 0
+        auto feedback = std::make_shared<MoveTo::Feedback>();
+        feedback->progress = 0.0;
+        // 发布初始反馈
+        goal_handle->publish_feedback(feedback);
+
+        for (int ii = 0; ii < 100; ii++)
+        {
+            // 如果节点被关闭
+            if (!rclcpp::ok())
+            {
+                return;
+            }
+
+            // 处理中途取消请求
+            if (goal_handle->is_canceling())
+            {
+                RCLCPP_INFO(this->get_logger(), "Goal is canceling");
+                auto result = std::make_shared<MoveTo::Result>();
+                result->success = false;
+                result->status = MoveTo::Result::STATUS_CANCELED;
+                result->message = "Goal was canceled";
+                goal_handle->canceled(result);
+                return;
+            }
+
+            // 模拟执行过程
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            feedback->progress = (ii + 1) / 100.0f;
+            goal_handle->publish_feedback(feedback);
+        }
+
+        // 执行完成，设置目标状态为成功
+        auto result = std::make_shared<MoveTo::Result>();
+        result->success = true;
+        result->status = MoveTo::Result::STATUS_SUCCEEDED;
+        result->message = "Goal was succeeded";
+        goal_handle->succeed(result);
+    }
+
+private:
+    rclcpp_action::Server<MoveTo>::SharedPtr action_server_;
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<MoveToActionServer>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

protocol_ws/cpp_prococol/src/cpp_server_service.cpp

@@ -0,0 +1,58 @@
+/*
+本程序用于使用cpp编写的ros2服务端节点
+*/
+#include "rclcpp/rclcpp.hpp"
+#include "cpp_prococol/srv/position.hpp"
+
+using Positon = cpp_prococol::srv::Position;
+
+class cppServerService:public rclcpp::Node
+{
+public:
+    cppServerService():Node("cpp_server_service")
+    {
+        // 创建服务器对象
+        service_ = this->create_service<Positon>("position",std::bind(&cppServerService::handle_service,this,std::placeholders::_1,std::placeholders::_2));
+        // 打印服务端启动成功的日志信息
+        RCLCPP_INFO(this->get_logger(),"cpp server service is ready.");
+    }
+
+    // 处理服务请求的回调函数
+    /*
+    Params:
+    request: 请求数据，包含客户端发送的请求内容
+    response: 响应数据，服务器将通过这个参数发送响应内容给客户端
+    */
+    void handle_service(const std::shared_ptr<Positon::Request> request,std::shared_ptr<Positon::Response> response)
+    {
+        // 打印接收到的请求数据
+        RCLCPP_INFO(this->get_logger(),"Receiverd request: target_x =%f, target_y = %f",request->target_x,request->target_y);   
+
+        //处理请求数据,生成结果
+        if(request->target_x == 0.0 && request->target_y == 0.0)
+        {
+            response->result = Positon::Response::FALL;
+        }
+        else
+        {
+            response->result = Positon::Response::SUCCESS;
+        }
+    }
+private:
+    // 声明服务端对象
+    rclcpp::Service<Positon>::SharedPtr service_;
+};
+
+
+
+int main(int argc, char **argv)
+{
+    // 初始化ROS2客户端库
+    rclcpp::init(argc,argv);
+    
+    auto node = std::make_shared<cppServerService>();
+
+    rclcpp::spin(node);
+
+    rclcpp::shutdown();
+}

protocol_ws/cpp_prococol/src/cpp_sub_topic.cpp

@@ -0,0 +1,41 @@
+/*
+本程序为 ROS2 订阅话题的示例代码,订阅话题,当有消息的时候,打印出来
+*/
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp" // 包含字符串消息类型的头文件
+
+using StringMsg = std_msgs::msg::String; // 定义一个别名，方便后续使用
+
+class MinimalSubscriber : public rclcpp::Node
+{
+public:
+    MinimalSubscriber() : Node("minimal_subscriber") // 构造函数，初始化节点名称
+    {
+        // 创建一个订阅者对象，订阅"chatter"主题，使用默认QoS设置，并绑定回调函数
+        subscription_ = this->create_subscription<StringMsg>(
+            "chatter",10,std::bind(&MinimalSubscriber::topic_callback, this, std::placeholders::_1)
+        );
+    }
+
+    // 订阅回调函数，接收消息并打印
+    void topic_callback(const StringMsg::SharedPtr msg)
+    {
+        RCLCPP_INFO(this->get_logger(), "Subscriber: '%s'", msg->data.c_str()); // 打印日志，显示接收到的消息内容
+    }
+
+private:
+    rclcpp::Subscription<StringMsg>::SharedPtr subscription_; // 订阅者对象
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv); // 初始化ROS2客户端库
+
+    auto node = std::make_shared<MinimalSubscriber>(); // 创建订阅者节点对象
+
+    rclcpp::spin(node); // 进入循环，等待回调函数被调用
+    rclcpp::shutdown(); // 关闭ROS2客户端库
+    return 0;
+
+}