服务通信案例

1.案例需求

在turtlesim_node节点的窗体中指定位置生成一只新乌龟并可以输出两只乌龟之间的直线距离

分为服务端和客户端

服务端：1.生成turtlesim_node节点

               2.计算距离

客户端：1.向服务端发送请求，要求在turtlesim_node中生成一只新乌龟

               2.向服务端发请求，发送新生成乌龟的坐标(x,y)到服务端

服务端要获取原生乌龟的坐标，再结合发送过来的指定位置的新乌龟的坐标，计算两乌龟的距离，将距离再响应回客户端。

 2.案例分析

在上述案例中，需要关注的问题有两个:
1.如何在指定位置生成一只新乌龟?
2.计算两只乌龟的距离应该使用何种通信模式又如何实现?

思路:
1.问题1可以通过调用turtlesim node内置的名称为/spawn的服务功能来实现新乌龟的创建;

2.问题2可以通过服务通信来实现，客户端发送新生成的乌龟的位姿到服务端，服务端根据该坐标以及原生乌龟的坐标计算距离并响应。当然如果使用服务通信，还需要自定义服务接口。

3.最后，整个案例涉及到多个节点，我们可以通过launch文件集成这些节点。

3.流程实现：

3-1 编写服务接口文件

在 ws01_plumbing/src/base_interfaces_demo/srv 下新建Distance.srv文件

# 请求数据
float32 x
float32 y
float32 theta
---
# 响应数据
float32 distance

 其中，请求数据 x,y,theta 是客户端在指定位置生成的乌龟位姿

distance是计算距离，从服务端响应回客户端

3-2 编写服务端实现

/*
    需求：解析客户端提交的目标点坐标，获取原生乌龟坐标，计算二者距离并响应回客户端。
    流程：

        1.包含头文件
        2.初始化ROS2客户端
        3.自定义节点类：
            3-1 创建一个订阅方（原生乌龟位姿 /turtle1/pose）；
            3-2 创建一个服务端；
            3-3 回调函数解析请求客户端数据并响应结果到客户端。

        4.调用spin函数，传入自定义类对象指针
        5.释放资源
*/

#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp" //创建订阅方包含的头文件
#include "base_interfaces_demo/srv/distance.hpp" //创建服务端

using std::placeholders::_1; //占为符
using std::placeholders::_2;
using base_interfaces_demo::srv::Distance;

class Exer02Server:public rclcpp::Node{
public:
    Exer02Server():Node("exer02_server_node_cpp"),x(0.0),y(0.0){
        RCLCPP_INFO(this->get_logger(), "案例2服务端创建了");
        sub_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&Exer02Server::pose_cb, this, std::placeholders::_1));
        server_ = this->create_service<Distance>("distance", std::bind(&Exer02Server::distance_cb,this,_1,_2));
    }
private:
    //创建订阅方声明成员变量
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr sub_;
    rclcpp::Service<Distance>::SharedPtr server_;
    float x,y;
    void pose_cb(const turtlesim::msg::Pose::SharedPtr pose){
        x = pose->x;
        y = pose->y;
    }
    void distance_cb(const Distance::Request::SharedPtr request, Distance::Response::SharedPtr response){
        // 1.解析出目标点坐标
        float goal_x = request->x;
        float goal_y = request->y;
        // 2.计算距离
        float distance_x = goal_x - x;
        float distance_y = goal_y - y;
        float distance = std::sqrt(distance_x*distance_x+distance_y*distance_y);
        // 3.设置进响应
        response->distance = distance;
        RCLCPP_INFO(this->get_logger(),"目标点坐标(%.2f,%.2f),原生乌龟坐标(%.2f,%.2f),二者距离：%.2f",goal_x,goal_y,x,y,distance);
    }
};

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<Exer02Server>());
    rclcpp::shutdown();
    return 0;
}

3-3 编写客户端实现

/*
    需求：客户端需要提交目标点坐标，并解析响应的结果。
    流程：
        0.解析动态传入的数据，作为目标点坐标
        1.包含头文件
        2.初始化ROS2客户端
        3.自定义节点类：
            3-1.构造函数创建客户端
            3-2.客户端需要连接服务端
            3-3.发送请求数据
        4.调用节点对象指针的相关函数
        5.释放资源
*/

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/srv/distance.hpp"

using base_interfaces_demo::srv::Distance;
using namespace std::chrono_literals;
class Exer03Client:public rclcpp::Node{
public:
    Exer03Client():Node("exer03_client_node_cpp"){
        RCLCPP_INFO(this->get_logger(), "案例2客户端创建了");
        // 3-1.构造函数创建客户端
        client_ = this->create_client<Distance>("distance");
        
    }
    // 3-2.客户端需要连接服务端
    bool connect_server(){
        while(!client_->wait_for_service(1s))
        {
            if(!rclcpp::ok())
            {
                RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"节点被强制退出");
                return false;
            }
            RCLCPP_INFO(this->get_logger(),"服务连接中");
        }
        return true;
    }
    // 3-3.发送请求数据
    rclcpp::Client<Distance>::FutureAndRequestId send_goal(float x,float y,float theta){
        auto request = std::make_shared<Distance::Request>();
        request->x = x;
        request->y = y;
        request->theta = theta;
        return client_->async_send_request(request);
    }
private:
    rclcpp::Client<Distance>::SharedPtr client_;
};

int main(int argc, char const *argv[])
{
    // 0.解析动态传入的数据，作为目标点坐标
    if (argc != 5){
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"请提交x坐标，y坐标与theta三个参数");
        return 1;
    }
    // 解析提交的参数
    float goal_x = atof(argv[1]);
    float goal_y = atof(argv[2]);
    float goal_theta = atof(argv[3]);
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"%.2f,%.2f,%.2f",goal_x,goal_y,goal_theta);

    rclcpp::init(argc, argv);
    //4.调用节点对象指针的相关函数
    auto client = std::make_shared<Exer03Client>();
    bool flag = client->connect_server();
    if(!flag)
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务连接失败！");
        return 1;
    }
    // 发送请求并处理响应
    auto future = client->send_goal(goal_x, goal_y, goal_theta);
    // 判断响应结果状态
    if(rclcpp::spin_until_future_complete(client, future) == rclcpp::FutureReturnCode::SUCCESS)
    {
        RCLCPP_INFO(client->get_logger(),"两只乌龟距离%.2f米",future.get()->distance);
    }else{
        RCLCPP_ERROR(client->get_logger(),"服务响应失败！");
    }

    rclcpp::shutdown();
    return 0;
}

3-4 编写launch文件

运行服务端

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    # 1.turtlesim_node
    t = Node(package="turtlesim", executable="turtlesim_node")
    server = Node(package="cpp07_exercise", executable="exer02_server")
    return LaunchDescription([t, server])

运行客户端 

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
def generate_launch_description():
    x = 6
    y = 9
    theta = 0.0
    name = "t2"
    # 1.在目标点生成一只新乌龟
    # ros2 service call /spawn turtlesim/srv/Spawn "{"x": 5.0,"y": 3.0,"theta": 3.14,"name": "t2"}"
    spawn = ExecuteProcess(
        cmd = ["ros2 service call /spawn turtlesim/srv/Spawn \"{'x': '"+str(x)+"','y': '"+str(y)+"','theta': '"+str(theta)+"','name': '"+ name +"'}\""],
        output = "both",
        shell = True
    )
    # 2.使用客户端发送目标点坐标
    client = Node(package="cpp07_exercise", executable="exer03_client",
                  arguments=[str(x),str(y),str(theta)])
                  # 相当于运行 ros2 run cpp07_exercise exer03_client 6 9 0.0 --ros-args
    return LaunchDescription([spawn, client])

3-5 编辑配置文件

接口文件配置

src/base_interfaces_demo/CMakeLists.txt 下

src/base_interfaces_demo/package.xml 下

我们自己的包文件配置

 src/cpp07_exercise/CMakeLists.txt 下

 

 

 src/cpp07_exercise/package.xml 下已自动配置完成

3-6 编译 执行

接下来就可以正常执行了！