ROS2 服务通信——自定义服务接口（C++）

实验目的：

         自定义服务接口，要求客户端发送两个整型数据给服务端，服务端进行求和并且反馈结果给客户端。

1、创建工作空间和自定义接口功能包

        请参考：https://blog.csdn.net/qq_58457030/article/details/141336256

2、自定义服务接口

        在interface_pkg功能包中新建srv文件夹，用于存放服务接口文件，在srv目录中创建Addints.srv文件

mkdir -p ./text_w/src/interface_pkg/srv
touch ./text_w/src/interface_pkg/srv/Addints.srv

       在Addints.srv文件定义以下数据格式 

int32 num1
int32 num2
---
int32 sum

        修改interface_pkg中的CMakeLists.txt文件

         保存然后编译

cd text_w/
colcon build --packages-select interface_pkg 

        在install目录下面生成了我们所需要的文件，即编译成功 

        创建服务功能包service_pkg，新建服务端和客户端节点

cd ./text_w/src/
ros2 pkg create service_pkg --build-type ament_cmake --dependencies rclcpp interface_pkg --node-name server
touch ./service_pkg/src/client.cpp

        编写服务端节点

#include "rclcpp/rclcpp.hpp"                         // ROS2 C++接口库
#include "interface_pkg/srv/addints.hpp"             //导入自定义接口

using interface_pkg::srv::Addints;

using std::placeholders::_1;
using std::placeholders::_2;


class AdderServer: public rclcpp::Node{
    public:
        AdderServer(): Node("Add_server_node"){
            RCLCPP_INFO(this->get_logger(), "服务端节点创建！");
            //创建服务端
            server_ = this->create_service<Addints>("add_ints",std::bind(&AdderServer::add,this,_1,_2));
        }

    private:
        void add(const Addints::Request::SharedPtr req, const Addints::Response::SharedPtr res){
            //解析提交的数据并发送响应
            res->sum = req->num1 + req->num2;
            RCLCPP_INFO(this->get_logger(), "%d + %d =%d " ,req->num1,req->num2,res->sum);
        }
        rclcpp::Service<Addints>::SharedPtr server_;
};



// ROS2节点主入口main函数
int main(int argc, char **argv)                                                                 
{
    // ROS2 C++接口初始化
    rclcpp::init(argc, argv);
                                                                
    rclcpp::spin(std::make_shared<AdderServer>());
    
    // 关闭ROS2 C++接口                                                                     
    rclcpp::shutdown();                                                                         
}

        编写客户端节点

#include "rclcpp/rclcpp.hpp"                         // ROS2 C++接口库
#include "interface_pkg/srv/addints.hpp"             //导入自定义接口

using interface_pkg::srv::Addints;

using namespace std::chrono_literals;

class AdderClient: public rclcpp::Node{
    public:
        AdderClient(): Node("Add_client_node"){
            RCLCPP_INFO(this->get_logger(), "客户端节点创建！");
            //创建客户端(模板：服务接口  参数：服务话题名称  返回值：服务对象指针)
            client_= this->create_client<Addints>("add_ints");
        }

        //连接服务器
        bool connect_server(){
            while(!client_->wait_for_service(1s))//循环一秒为超时间连接服务器，直到连接到服务器才退出循环
            {
              //对ctrl+c 作出处理
              if(!rclcpp::ok())
              {
                RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "强行终止客户端！");
                return false;
              }
              RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "服务器连接中......");
            }
            return true;
        }

        //发送请求
        rclcpp::Client<Addints>::FutureAndRequestId send_request(int num1, int num2)
        {
          //组织请求数据
          //发送
          auto request = std::make_shared<Addints::Request>();
          request->num1 = num1;
          request->num2 = num2;

          return client_->async_send_request(request);
        }

    private:

        rclcpp::Client<Addints>::SharedPtr client_;
};



// ROS2节点主入口main函数
int main(int argc, char **argv)                                                                 
{
    //argc 第一个参数num1,第二个参数num2,第三个参数sum，判断是否已经提交两个整型数据了
    if(argc != 3)
    {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"请提交两个整型数据！");
      return 1;
    }
    // ROS2 C++接口初始化
    rclcpp::init(argc, argv);

    //创建客户端对象
    auto client = std::make_shared<AdderClient>();                                                   
    
    //调用客户端对象连接服务器功能
    bool flag = client->connect_server();
    //判断是否连接上服务器
    if(!flag)
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "服务器连接失败！");
        return 0;
    }

    //调用请求提交函数，接收并处理响应结果
    auto future = client->send_request(atoi(argv[1]),atoi(argv[2]));
    //处理响应
    if (rclcpp::spin_until_future_complete(client,future)==rclcpp::FutureReturnCode::SUCCESS)//如果成功
    {
      RCLCPP_INFO(client->get_logger(), "响应成功！sum = %d",future.get()->sum);
    }
    else
    {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "响应失败！");
    }
    

    // 关闭ROS2 C++接口                                                                     
    rclcpp::shutdown();                                                                         
}

        修改CMakeLists.txt

        分别开两个终端先编译一下，然后运行服务端和客户端节点

colcon build --packages-select service_pkg
source install/setup.bash 
ros2 run service_pkg server
 
 
source install/setup.bash 
ros2 run service_pkg client 10 20

        可以看到客户端提交两个整形数据10和20，然后服务端处理求和并且反馈给客户端结果为30，测试成功。