Service

导言

我们都知道在计算机网络中，会分出服务端和客户端，当然在我们的 ROS2 网络中也有这样的分离机制，只不过这里的客户端和服务端的内容会和纯粹计算机网络的内容有所差异，我们边学边体会

为什么有服务

在我们的 Topic 通讯模式中，我们很容易发现 Topic 只是建立了单向边，这也就导致如果我们需要数据双向传输（通常是需要建立反馈机制的时候），就非常不方便，很容易把自己累死（bushi

所以 ROS2 就推出了这样一套 Request 和 Response 结合的服务通讯机制，构建了 请求-响应 模型，用来满足这方面的需求

服务通信简介

在我们 ROS2 的服务通讯机制中，分出了服务端(Service)和客户端(Client)，这两者之间共同建立一套 请求-响应 机制的通讯路线

而在我们这一套机制当中，我们需要注意一些事项：

• 同一个服务（名称相同）有且只能有一个节点来提供
• 同一个服务可以被多个客户端调用

这也就和 Topic 不一样了，服务只有1-1和1-n的模型

服务是需要接口的，就像常规软件开发过程中使用的 API
这个接口会定义一些参数，用来保证多端口通讯的时候可以互相明白对面在说什么

接下来我们同样在样例中进行更细节的讲解

C++

首先我们需要创建功能包：

cd ros2_learning
mkdir -p Service/service_ws/src
cd Service/service_ws/src
ros2 pkg create service_demo_rclcpp --build-type ament_cmake --dependencies rclcpp
touch service_demo_rclcpp/src/service_server_01.cpp
touch service_demo_rclcpp/src/service_client_01.cpp

创建完成后的文件包你应该已经清楚是什么样子的了，这里就不再过多赘述了

这里基础的节点结构相信前面几篇文章看过的同学都很清晰了，所以这里就不赘述了，我们直接来到配置文件

CMakeList.txt

add_executable(service_client_01 src/service_client_01.cpp)
ament_target_dependencies(service_client_01 rclcpp)

add_executable(service_server_01 src/service_server_01.cpp)
ament_target_dependencies(service_server_01 rclcpp)

install(TARGETS
    service_server_01
    DESTINATION lib/${PROJECT_NAME}
)

install(TARGETS
    service_client_01
    DESTINATION lib/${PROJECT_NAME}
)

完成上面的步骤，再加上你自己编写的两个节点模板，就可以进行最基础的运行调试了

接下来讲解服务端的实现

服务端实现

接口导入

在此之前，我们先聊聊 Service 的内在机制：
服务本身的数据传输格式对比 Topic 就比较复杂了，毕竟有一个request和response，由于数据比较复杂，我们就需要做专门的接口来进行数据传输，放置错乱，我会分别展示自定义服务接口和系统自带的接口（当然，Topic 也是可以定义接口的）

我们首先拿一个系统自带接口来看：

ros2 interface show example_interfaces/srv/AddTwoInts

从名字就可以看得出来，这是一个将两个int类型相加的函数，另外，example_interfaces是 ROS2 自带的接口

这里我们通过上方的指令应该可以看到如下内容：

int64 a
int64 b
---
int64 sum

简单解释一下上面的内容，中间有一条分割线，上方是 request 的内容，下方是 response 的内容
在代码中我们会使用智能指针进行读取，封装非常清晰

这里我们添加了一个新的接口，那么我们就需要在配置文件和代码头文件中进行修改：

CMakeLists.txt

find_package(example_interfaces REQUIRED)

add_executable(service_client_01 src/service_client_01.cpp)
ament_target_dependencies(service_client_01 rclcpp example_interfaces)

add_executable(service_server_01 src/service_server_01.cpp)
ament_target_dependencies(service_server_01 rclcpp example_interfaces)

packages.xml

<depend>example_interfaces</depend>

Server

#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"

class ServiceServer01 : public rclcpp::Node {
public:
    ServiceServer01(std::string name) : Node(name) {
        RCLCPP_INFO(this->get_logger(),"节点已启动：%s",name.c_str());
        add_ints_server_ = this->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints_srv",std::bind(&ServiceServer01::handle_add_two_ints,this,std::placeholders::_1,std::placeholders::_2));
    }
private:
    rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr add_ints_server_;

    void handle_add_two_ints(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response) {
        RCLCPP_INFO(this->get_logger(),"收到a: %ld b: %ld",request->a,request->b);
        response->sum = request->a + request->b;
    };
};

int main(int argc,char **argv) {
    rclcpp::init(argc,argv);
    auto node = std::make_shared<ServiceServer01>("service_server_01");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

这里对上面的代码段做出详细的解释：

• 我们通过 rclcpp 内置的create_service()函数以example_interfaces包（包的三个文件内容如下）中的AddTwoInts.srv文件作为服务模式的消息接口文件进行传输，设置了接受request后的唯一回调函数：handle_add_two_ints()，后面的参数不需要过多解释了
  
  （上面的清朝老文档就是当时配置的一些可以直接使用的服务类型的接口文件）

*在我们的中断回调函数中，根据数据传输的模式和 Topic文章 中所讲解的基本是一致的，这里传入智能指针，就自然读取了request传入的两个量：a,b（这里是int64类型的）后面跟着的参数是responce，这里值得注意的是不要在response加const，因为通常来说，回调函数是需要修改response进行返回的

客户端实现

异步发送

在实现客户端的内容之前，我们先简单了解一下异步发送和同步发送的区别：

• 在我们的双向传输中通常会有异步和同步的两种概念，这里两种概念在通讯过程中的最明显的体现在于：异步不需要等待另一方做出任何反应，而同步必须双方同时做出某个动作或者一方等待另一方做出某个动作，在等待的过程中，发送进程处于阻塞状态

在我们的 ROS2 机器人系统中，异步发送是很常见的一种信息传递模式，因为具有很高的时效性，能够适应需要快速反应的场景

接下来我们来看看异步发送的函数：

async_send_request(SharedRequest request,CallbackT && cb)

这里也就说明我们的异步发送函数只需要传入request的消息和回调函数就可以了

这里的回调函数是在客户端收到来自服务端的response之后会调用的中断，这也体现了 Service 通讯模式的双向特性

等待连接

如果你打网络游戏，那你肯定也会遇到游戏匹配的时候出现需要等待别人上线的情况

我们的客户端也是这样，如果不等待服务端上线就直接开始运行，那我们的数据会在客户端打开的一瞬间飞走，没有服务端接受的话，马上就会淹没在网络的洪流里，再也找不见了

这也就提示我们，在发送数据的时候，需要等待服务端上线，才能正常运行客户端，而我们的 ROS2 已经为我们配置好了专门用于等待服务端上线的函数：

wait_for_service(std::chrono::duration<RepT,RatioT> timeout = std::chrono::duration<RepT,RatioT>(-1))

这个函数只有一个参数，却被封装得又臭又长，事实上，根据我们的变量名timeout我们可以很快推测出这个数值就是用来设置超时时间的

我们另外有一个rclcpp::ok()的函数，专门用来表示节点的状态，如果节点在当前进程中运行时被按下Ctrl+C，或者出现等待过久或无法处理的情况，我们的rclcpp::ok()就会返回false，可以帮助节点进程优雅退出（官方用的就是“优雅”一词）

在这里，当我们调用了 Client 的wait_for_service'方法，系统就会自动按下定时器，当定时器到达设置的时钟周期后，rclcpp::ok()的值就会返回false，这个时候在while`里面检测到节点出问题后，就可以优雅退出了

这里设置的等待时间是 1s （相信精通八国语言的你一定看得懂）

接下来我们就可以上代码了

Client:

#include "example_interfaces/srv/add_two_ints.hpp"

class ServiceClient01 : public rclcpp::Node {
public:
    ServiceClient01(std::string name) : Node(name) {
        RCLCPP_INFO(this->get_logger(),"节点已启动: %s.",name.c_str());
        client_ = this->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints_srv");
    }
    void send_request(int a,int b) {
        RCLCPP_INFO(this->get_logger(),"计算%d+%d",a,b);
  
        while(!client_->wait_for_service(std::chrono:::seconds(1))) {
            if(!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(),"等待服务的过程中被打断...");
                return;
            }
            RCLCPP_INFO(this->get_logger(),"等待服务端上线中");
        }
        auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
        request->a = a;
        request->b = b;

        client_->async_send_request(request,std::bind(&ServiceClient01::result_callback_,this,std::placeholders::_1));
    }
private:
    rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_;
    void result_callback_(rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture result_future) {
        auto response = result_future.get();
        RCLCPP_INFO(this->get_logger(),"计算结果：%ld",response->sum);
    }
};

int main(int argc,char **argv) {
    rclcpp::init(argc,argv);
    auto node = std::make_shared<ServiceClient01>("service_client_01");
    node->send_request(5,6);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

下面讲解上面的代码内容：

• 首先我们使用了create_client()创建了名为add_two_ints_srv的服务类型接口的客户端（再次强调，尖括号中才是接口类型的定义文件，圆括号中只有客户端名称）

• 在我们定义的send_request函数中，我们等待服务端上线后，构造了请求的内容，并且使用异步发送将数据传入服务端

• 这里值得注意的是可以看到我们的异步发送的回调函数里面有一个SharedFuture类型的指针，这里简单说明一下这个指针的作用，简单来说，Future就是设置了一个期待未来赋予值的指针，这里异步发送出去了以后，程序等待一个数据传入，这个时候传入的数据是异步的，那就需要使用Future类型的共享指针，防止数据丢失

• 另外，这里需要注意的是Future的.get()的意思是阻塞进程直到获得传入的值，这样就能实现安全地收取 Response 的值

这样我们就可以开始编译测试了，只要打开两个节点，我们的数据就可以在这里正常传输了

自定义接口类型的服务

我们前面已经简单介绍过接口文件的含义，但在这里，我们还是需要重新回顾并拓展一下接口到底是怎么用的

首先，我们的 ROS2 内部已经提前内置了一种通讯方式：DDS 数据通过底层进入到接口文件后，就会被另一个同样连接到该接口文件的节点接收

我们的 ROS2 中也提前定义了很多接口消息类型，比如我们之前已经使用过的std_msgs/msg/String就是一种很典型的类型，在我们的 Topic 章节中已经列出过更具体的消息类型，可以取前面参考

当然还有其他的类型，比如机器人使用激光雷达的专属接口sensor_msgs/msg/LaserScan，几乎每个厂家都会把自己的雷达数据转化成这个格式，就像机械中使用的标准间，我们在机器人的系统层级中也有一套工业化的标准，提升了灵活性和可迭代性以及复用率

当然，不要忘了我们可以通过ros2 interface package [接口包名称]来查看某个接口包内部的所有消息接口类型

比如ros2 interface package sensor_msgs：

sensor_msgs/msg/JointState  #机器人关节数据
sensor_msgs/msg/Temperature #温度数据
sensor_msgs/msg/Imu #加速度传感器
sensor_msgs/msg/Image #图像
sensor_msgs/msg/LaserScan #雷达数据
......

这些接口提供了相当基础的内容，当然，很多时候无法满足我们的拓展需求，这里我们就需要自定义接口了

接口文件的内容讲解：

ROS2 提供了四种通讯方式：Topics、Service、Action、Parameters
这四种通讯方式我们后面还会再讲到，这里先说明：除了参数之外，另外三种都支持自定义接口，接口类型也被分为话题接口、服务接口、动作接口三种

我们来看看这三种接口定义的样式，再给出讲解：

Topic 接口格式：xxx.msg

int64 num

Service 接口格式：xxx.srv

int64 a
int64 b
---
int64 sum

Action 接口格式：'xxx.action'

int32 order
---
int32[] sequence
---
int32[] partial_sequence

这里我们可以发现我们的接口类型之下还有一些基础的类型，这里再次更完整地列出：

bool
byte
char
float32,float64
int8,uint8
int16,uint16
int32,uint32
int64,uint64
string

当然，除此之外，我们还可以使用嵌套的结构，把其他已经存在的包结构混合传入接口中，就像struct一样：

uint32 id
string image_name
sensor_msgs/Image

接口如何生成代码

这里我们写的一个简单的接口自然是不方便我们的程序直接调用和读取的，更没有设置传输协议和模式

事实上，我们的 ROS2 在编译的时候会自动读取接口文件，并进行转换

IDL 的这一次转换生成了 Python 的包和 C++ 的头文件，这样我们就能用头文件导入使用一个消息模块了

注意规范：

值得注意的是，我们的接口文件在生成可以被程序调用的代码包的时候，ROS2 中的 IDL 工具会把我们的接口包的文件名读取出来，然后把我们的驼峰命名法转化成蛇命名法，比如：MyMessageText.srv在 C++ 中转化为可调用的my_message_test.hpp，这样就生成了我们的一个头文件，就可以使用我们的消息接口了

实战

我们在 Topic 已经定义过一个机器人运动的通讯功能，没有做得比较完善肯定是比较失望的，所以我们在这里使用我们已经学过的服务和话题来设计一个可以控制的机器人

我们考虑机器人会需要两个节点：

• 机器人本身，需要检测自己实际上走了多少距离，全过程中实时反馈给操作手自己在什么位置，并且处于什么状态（移动状态）

• 控制节点，通过服务控制机器人移动，并在此过程中实时获取机器人的位置和状态

定义接口

定义服务接口MoveRobot.srv

float32 distance
---
float32 pose

这个服务接口提供了一个distance的 request ，和一个pose的 response ，符合我们的基础设计要求，即传给机器人需要移动的距离，然后让机器人实时返回位置

定义话题接口RobotStatus.msg

uint32 STATUS_MOVING = 1
uint32 STATUS_STOP = 2
uint32  status
float32 pose

这里我们检测机器人当前的状态，由于我们这里编写的是一个简单的一维上运动的机器人类型，所以这里就不需要使用更多维度的封装了

如果我们需要编写现实中（三维）运动的机器人，可能需要像下面一样封装：
（这一段代码不用加入到我们的文件中，先看看，作为学习）

uint32 STATUS_MOVING = 1
uint32 STATUS_STOP = 2
uint32  status
geometry_msgs/Pose pose

其中，我们的geometry_msgs是我们在 Topic 中已经提到的一种消息类型，包含了机器人几何基础的内容，这里展示其中/Pose pose的内容：

geometry_msgs/Pose:
  geometry_msgs/Point position
    float64 x
    float64 y  
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

这里为什么角度会多一个元素呢？这个留到后面我们的机器人运动学中再具体介绍
当然，我们这里还是使用我们一维的接口包就可以了

构建结构

这里我们先创建功能包和节点：
在我们上面创建的 /Service 中另开一个工作空间：

mkdir robot_demo_ws
cd robot_demo_ws
ros2 pkg create robot_demo_rclcpp --build-type ament --dependencies rclcpp
cd /src/robot_demo_rclcpp/src
touch robot_node_01.cpp
touch robot_control_01.cpp

我们就完成了一个最基础的工作空间的创建

接下来我们来创建我们的接口包：

cd /robot_demo_ws/src
ros2 pkg create my_interfaces --build-type ament_cmake --dependencies rosidl_default_generators

在这里面我们再创建几个文件夹(/msg，/srv)，就会变成下面的结构：

.
└── src/
    ├── my_interfaces_rclcpp/     # 接口包
    │   ├── msg/
    │   ├── srv/
    │   ├── CMakeLists.txt
    │   └── package.xml
    └── robot_demo_rclcpp/   # 你的节点包
        ├── src/
        ├── CMakeLists.txt
        └── package.xml

我们在这两个新加入的文件夹里写入我们的接口文件即可
在msg/中写入文件名为robot_status.msg的文件
在srv/中写入文件名为move_robot.srv的文件
（这两个文件在编译之后就会生成对应的.hpp类型文件，就不需要再次手动编写了）

接下来我们要求这个接口包能够完整使用，这里也要修改一下/my_interfaces_rclcpp中的配置文件：
CMakeList:

find_package(rosidl_default_generators REQUIRED)

# 添加下面的内容
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/RobotStatus.msg"
  "srv/MoveRobot.srv"
)

解释一下添加的内容：
英语还不错的同学应该可以读出来，着表示使用 ROS 的 IDL 工具生成消息接口包，其中我们的环境变量${PROJRCT_NAME}默认配置为当前的这个项目，下面则是添加了两个路径连接单个接口文件，用来生成接口包

packages.xml:

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rosidl_default_generators</depend>
  
  <member_of_group>rosidl_interface_packages</member_of_group> #添加这一行

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

这里的<member_of_goup>事实上是指定了这个包是消息接口包
这里我们还没有完成对于消息接口的创建，还需要进行一次编译才行：

cd robot_demo_ws
colcon builde --packages-select my_interfaces_rclcpp

这里我们是后来加入的接口包，所以需要在/robot_demo_rclcpp文件下重新配置我们的文件：
CMakeList:

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(my_interfaces_rclcpp REQUIRED)

add_executable(robot_node_01 src/robot_node_01.cpp)
target_include_directories(robot_node_01 PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(robot_node_01 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  robot_node_01
  "rclcpp"
  "my_interfaces_rclcpp"
)

install(TARGETS robot_node_01
  DESTINATION lib/${PROJECT_NAME})


add_executable(robot_control_01 src/robot_control_01.cpp)
target_include_directories(robot_control_01 PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(robot_control_01 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  robot_control_01
  "rclcpp"
  "my_interfaces_rclcpp"
)

install(TARGETS robot_control_01
  DESTINATION lib/${PROJECT_NAME})

这里解释几个前面没有出现过的代码段含义：

target_include_directories(robot_control_01 PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

这句话的意思是给我们的节点添加需要的头文件路径，下面解释具体参数：

• robot_control_01 就是我们的几点名称，这个参数不需要过多解释
• PUBLIC 表示可见，也就是其他链接这个节点的节点也会自动使用这个头文件路径
• 后面两个环境变量设置了构建和安装的时候需要包含的路径为/include

target_compile_features(robot_control_01 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

这句话对编写代码的语言版本提出了要求，要求为：C99 和 C++17 的版本风格

接下来我们就可以开始构建我们的节点了

构建节点：

我们同样使用先编写代码再给出解释的方式带各位读懂代码内容

robot_node_01.cpp

#include "my_interfaces_rclcpp/msg/robot_status.hpp"
#include "my_interfaces_rclcpp/srv/move_robot.hpp"
#include "rclcpp/rclcpp.hpp"

class Robot {
public:
    Robot() = default;
    ~Robot() = default;
    float move_distance(float distance) {
        status_ = my_interfaces_rclcpp::msg::RobotStatus::STATUS_MOVING;
        target_pose_ += distance;
        while(fabs(target_pos_ - current_pose_)>0.01) {
            float step = distance/fabs(distance)*fabs(target_pose_ - current_pose_)*0.1;
            current_pose_ += step;
            std::this->this_thread::sleep_for(std::chrono::milliseconds(500));
        }
        status_ = my_interfaces_rclcpp::msg::RobotStatus::STATUS_STOP;
        return current_pose_;
    }
    float get_current_pose() {return current_pose_;}
    int get_status() {return status_;}

private:
    float curren_pose_ = 0.0;
    float target_pose_ = 0.0;
    int status_ =my_interfaces_rclcpp::msg::RobotStatus::STATUS_STOP;
};

class RobotDemo : public rclcpp::Node {
public:
    RobotDemo(std::string name) : Node(name) {
        RCLCPP_INFO(this->get_logger(),"节点已启动：%s.",name.c_str());
        using namespace std::placeholder;
        move_robot_server_ = this->create_service<my_interfaces_rclcpp::srv::MoveRobot>("move_robot",std::bind(&RobotDemo::handle_move_robot,this,_1,_2));
        robot_status_publisher_ = this->create_publisher<my_interfaces_rclcpp::msg::RobotStatus>("robot_status",10);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(500),std::bind(&RobotDemo::timer_callback,this));
    }
private:
    Robot robot;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Service<my_interfaces_rclcpp::srv::MoveRobot>::SharedPtr move_robot_server_;
    rclcpp::Publisher<my_interfaces_rclcpp::msg::RobotStatus>::SharedPtr robot_status_publisher_;
    void handle_move_robot(const std::shared_ptr<my_interfaces_rclcpp::srv::Request> request,std::shared_ptr<my_interfaces_rclcpp::srv::Response> response) {
        RCLCPP_INFO(this->get_logger(),"收到请求移动距离：%f，当前位置：%f",request->distance,robot.get_current_pose());
        robot.move_distance(request->distance);
        response->pose = robot.get_current_pose();
    }
};

int main(int argv,char **argv) {
    rclcpp::init(argc,argv);
    auto node = std::make_shared<RobotDemo>("");
}

这里我们简单定义了一个机器人的类型，里面封装了一些简单的方法和参数
接下来我们通过在RobotDemo实例化Robot类型，成功创建了一个实例
这里都是以前讲解过的代码结构，可以自己看看了

robot_control_01.cpp

#include "rclcpp/rclcpp.hpp"
#include "my_interfaces_rclcpp/srv/move_robot.hpp"
#include "my_interfaces_rclcpp/msg/robot_status.hpp"

class RobotControl : public rclcpp::Node {
public:
    RobotControl(std::string name) : Node(name) {
        RCLCPP_INFO(this->gte_logger(),"节点已启动：%s.",name.c_srt());
        client_ = this->create_lient<examle_ros2_interfaces::srv::MoveRobot>("move_robot");
        using namespace std::placeholders;
        robot_status_subscribe_ = this->create_subscription<my_interfaces_rclcpp::msg::RobotStatus>("robot_status",10,std::bind(&RobotControl::robot_status_callback_,this,_1));
    }
    
    void move_robot(float distance) {
        RCLCPP_INFO(this->get_logger(),"请求让机器人移动%f",distance);
        while(!client_ -> wait_for_service(std::chrono::seconds(1))) {
            if(!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(),"等待服务端上线中...");
                return;
            }
            RCLCPP_INFO(this->get_logger(),"等待服务端上线中");
        }
        auto request = std::make_shared<my_interfaces_rclcpp::srv::MoveRobot::Request>(); 
        request->distance = distance;
        using namespace std::placeholder;
        client_->async_send_request(request,std::bind(&RobotControl::result_callback_,this,_1))
    }
  
private:
    rclcpp::Client<my_interfaces_rclcpp::srv::MoveRobot>::SharedPtr client_;
    rclcpp::Subscription<my_interfaces_rclcpp::msg::RobotStatus>::SharedPtr robot_status_subscribe_;
    void result_callback_(rclcpp::Client<my_interfaces_rclcpp::srv::MoveRobot>::SharedFuture result_future) {
        auto response = result_future.get();
        RCLCPP_INFO(this->get_logger(),"收到移动结果：%f",response->pose);
    }
    void robot_status_callback_(const my_interfaces_rclcpp::msg::RobotStatus::SharedPtr msg) {
        RCLCPP_INFO(this->get_logger(),"收到状态数据位置：%f 状态：%d",msg->pose,msg->status);
    }
};

int main(int argc,char **argv) {
    rclcpp::init(argc,argv);
    auto node = std::make_shared<RobotControl>("robot_control_01");
    node->move_robot(5.0);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

我们这里创建了一个可以控制机器人地专属控制节点，在我们的主函数中调用了move_robot()的方法，相当于通过我们的通讯方式发送消息给机器人节点，让他向前行走5.0m

接下来我们就可以编译运行测试了！

存在的一些问题

这个时候我们会发现在机器人移动的过程中，我们没有办法收到来自机器人端的实时位置信息的Topic发布，这是因为机器人的移动造成了主线程的阻塞与休眠，只有机器人完成移动后才会退出，这也就导致了我们的发布数据的定时器回调无法正常进行

这里再详细讲解一下这边线程出现阻塞的原因：我们知道的 ROS2 节点真正运行的过程其实是单线程的，大概的内容应该如下：

// 伪代码：rclcpp::spin() 的内部逻辑
while (rclcpp::ok()) {
    // 检查所有待处理的任务
    if (有服务请求) { 执行服务回调(); }
    else if (有定时器到期) { 执行定时器回调(); }
    else if (有订阅消息) { 执行订阅回调(); }
    else { 等待新事件; }
}

我们可以看到的是，当一个回调被阻塞的时候，整个循环就会进入阻塞状态，自然整个节点就是卡住无法发送消息的，所以节点就收不到反馈了

这也就意味着，如果我们需要完成这一部分的全部功能，如果单单使用 Topic 和 Service 是无法满足我们对于多线程调用的需求的，我们后面在讲述 Action 的时候会发现 Action 很便于解决这类问题

那么我们接下来学习一下 Python 中的 Service

Python

在 C++ 中已经介绍了很多底层的原理，这里就不多赘述了

DDS简介

这里需要说明的是，ROS2 中的通讯不会受语言不通的影响，C++ 编写的节点和 Python 编写的节点是可以正常通讯的

这里继续介绍 ROS2 中的通讯机制，ROS2 使用的是统一接口定义语言（IDL），这可以保证不同语言中获取的数据类型是一致的，并且不会出现错乱
更底层地说，ROS2 使用了一种中间件（DDS）来实现跨语言和平台的通讯，这也就保证了多节点通讯地数据结构和内容正常，其中 DDS 是一种工业级标准，这是相当规范和稳定的

创建功能包

这里我们先进行功能包的创建：
我们回到ros2_learning，创建Service_py文件（因为实在是懒得管理文件名是否冲突了，所以干脆再单开一个包）

mkdir -p Service_py/service_ws/src
ros2 pkg create service_demo_rclpy --build-type ament_python --dependencies rclpy example_interfaces

值得注意的是，这里我们还没有先创建接口文件，但是提前在--dependencies里面包含了my_interfaces_rclpy的依赖，这样也是可以操作的，相当于直接在我们的配置文件里面添加了一部分依赖的路径，不过后面就必须要自己添加这些文件，否则可能就会出现找不到依赖路径的问题

然后我们创建两个py文件：

cd service_demo_rclpy/service_demo_rclpy/
touch service_client_01.py
touch service_server_01.py

这样我们就创建好了我们的两个节点代码，接下来我们修改启动文件：

    entry_points={
        'console_scripts': [
            "service_client_01 = service_demo_rclpy.service_client_01:main",
            "service_server_01 = service_demo_rclpy.service_server_01:main"
        ],
    },

上面这两句相必各位已经清楚了，那我们这里就不多赘述了

服务端实现

我们先简单浏览一下创建服务端的函数文档

我们看到我们的参数内容，包含我们的类型、名称、回调函数和服务质量（后面那个是回调函数组合，后面我们在 Action 中会细节讲解这个参数的含义，当然如果你文档看得多的话自然就明白了）

那么我们接下来就可以快乐地完成我们的代码了

service_server_01

import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts

class ServiceServer01(Node):
    def __init__(self,name):
        super().__init__(name);
        self.get_logger().info("节点已启动：%s!" % name)
        self.add_ints_server_ = self.create_service(AddTwoInts,"add_two_ints_serv",self.handle_add_ints)
  
    def handle_add_two_ints(self,request,response):
        self.get_logger().info(f"收到请求，计算 {request.a} + {request.b}")
        response.sum =request.a+request.b
        return response

def main(args=None):
    rclpy.init(args=args)
    node = ServiceServer01("service_server_01")
    rclpy.spin(node)
    rclpy.shutdown()

需要注意的是，Python 的文件父子关系一般是通过.连接的，然后 ROS2 的驼峰转蛇形的机制仍然是存在的，所以我们在书写的时候，相对于 C++，只需要注意一些语法上的不同就可以了，另外还是一样的

service_client_01

import rclpy
from rclpy.node import Node 
from example_interfaces.srv import AddTwoInts

class ServiceClient01(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("节点已启动：%s!" % name)
        self.client_ = self.create_client(AddTwoInts,"add_two_ints_srv")

    def result_callback_(self,result_future):
        response = result_future.result()
        self.get_logger().info(f"收到返回结果：{response.sum}")

    def send_request(self,a,b)
        while rclpy.ok() and self.client_.wait_for_service(1)==False:
            self.get_logger().info(f"等待服务端上线")
        request = AddTwoInts.Request()
        request.a = a
        request.b = b
        self.client_.call_async(request).add_done_callback(self.result_callback_)

def main(args=None):
    rclcpp.init(args,args)
    node = ServiceClient01("service_client_01")
    node.send_request(3,4)
    rclpy.spin(node)
    rclpy.shutdown();

这一段代码使用了比较清晰的结构，下面我们简单讲解一下这段代码的唯一一个长难句：

• self.client_.call_async(request).add_done_callback(self.result_callback_)这句话的意思就是使用异步发送传输request请求，并且设置在做完了以后的回调函数为result_callback_，这里不需要很多封装或转换的语法，这是比 C++ 更优秀的地方

当然为什么我不喜欢使用 Python 呢？其实这两者就像两种语言，C++ 更严谨且清晰，也就意味着它的底层逻辑都可以在代码里看得到；而 Python 更简单，代码量通常很短，所以它的封装程度会比较高，很多底层逻辑就不能在代码中体现

而我更喜欢严谨且运行速度更快的 C++ ，自然也对这一套语言的语法更为熟悉，所以使用 C++ 更多

这里写好了就可以自己编译运行调试了

自定义接口类型的服务

我们闲言少叙，接下来我们使用自定义接口来写Python的内容

关于自定义接口的基础知识，这里就不多说了，相信各位的能力应该能够看一遍上面的内容就全部理解了

接下来我们同样使用我们机器人的例子使用 Python 给各位展示一下代码的书写

我们先来创建文件结构：

cd ros2_learning/Service/
mkdir -p service_py/src
cd service_py/src
ros2 pkg create robot_demo_rclpy --build-type ament_python --dependencies rclpy my_interfaces_rclpy
cd src/robot_demo_rclpy/robot_demo_rclpy/
touch robot_node_01.py
touch robor_control_01.py

然后我们设置配置文件：

entry_points={
        'console_scripts': [
            'robot_node_01 = robot_demo_rclpy.robot_node_01:main',
            'robot_control_01 = robot_demo_rclpt.robot_control_01:main'
        ],
    },

这里的接口文件写法和之前 C++ 中的是一样的，这里可以直接把整个包/my_interfaces粘贴到这个工作空间下并编译生成接口

接下来我们同样先进行节点的编写，然后再给出具体的解释

robot_node_01.py

import rclpy
from rclpy.node import Node
from my_interfaces_rclpy.msg import RobotStatus
import math
from time import sleep
from my_interfaces_rclpy.srv import MoveRobot

class Robot():
    def __init__(self) -> None:
        self.current_pos_ = 0.0
        self.target_pos_ = 0.0
        self.status_ = RobotStatus.STATUS_STOP

    def get_status(self):
        return self.status_

    def get_current_pose(self):
        return self.current_pose_

    def move_distance(self,distance):
        self.status_ = RobotStatus.STATUS_MOVING
        self.target_pos_ += distance

        while math.fabs(self.target_pose_ - self.current_pose) > 0.01:
            step = distance/math.fabs(diatance)*math.fabs(self.target_pose_ - self.current_pose_) * 0.1
            self.current_pose_ += step
            print(f"移动了：{step} 当前位置：{self.current_pose_}")
            sleep(0.5)
        self.status_ = Robot.STATUS_STOP
        return self.current_pose_

class RobotDemo(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("节点已启动：%s!" % name)
        self.move_robot_server_ = self.create_service(MoveRobot,"move_robot",self.handle_move_robot)
        self.robot_status_publisher_ = self.create_publisher(RobotStatus,"robot_status",10)
        self.publisher_timer_ = self.create_timer(0.5,self.publisher_timer_callback)

    def publisher_timer_callback(self):
        msg = RobotStatus()
        msg.status = self.robot.get_status()
        msg.pose = self.robot.get_current_pose()
        self.robot_status_publisher_.publish(msg)
        self.get_logger().info(f'发布了当前的状态：{msg.status} 位置：{msg.pose}')
    
    def handle_move_robot(self,request,response):
        self.robot.move_distance(request.distance)
        response.pose = self.robot.get_current_pose()
        return response

def main(args=None):
    rclpy.init(args=args)
    node = RobotDemo("robot_node_01")
    rclpy.spin(node)
    rclpy.shutdown()

这里很多的代码内容我们已经见过了
这里可以看到，Python 中应该有在成员函数中自己手动写上 self 参数的好习惯

另外需要简单解释一下某一行代码：

• msg = RobotStatus()这一句中的这个函数名是以我们创建的消息名称命名的，我们在下面创建的控制节点中也会看到一个类似于这样的消息，这里就相当于我们读取了这个文件里的所有数据（就像 C++ 中的指针一下子抓取所有数据一样）

robot_control_01.py

import rclpy
from rclpy.node import Node
from my_interfaces_rclpy.msg import RobotStatus
from my_interfaces_rclpy.srv import MoveRobot

class RobotControl(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("节点已启动:%s!" % name)
        self.client_ = self.create_client(MoveRobot,"move_robot")
        self.robot_status_subscribe_ = self.create_subscription(RobotStatus,"robot_status",self.robot_status_callback,10)

    def robot_status_callback(self,msg):
        self.get_logger().info(f"收到状态数据位置{msg.pose} 状态：{msg.status}")

    def move_result_callback_(self,result_future):
        response = result_future.result()
        self.get_logger().info(f"等待服务端上线....")
        request = MoveRobot.Request()
        request.distance = distrance
        self.get_logger().info(f"请求服务让机器人移动{distance}")
        self.client_.call_async(request).add_done_callback(self.move_result_callback_)

def main(args=None) {
    rclpy.init(args=args)
    node = RobotControl("robot_control_01")
    node.move_robot(5.0)
    rclpy.spin(node)
    rclpy.shutdown()
}

同上，这里应该只有一段代码时需要我们进行调试的
*request = MoveRobot.Request()这里由于是 Servcice 的通讯方式，所以是分出 request 和 response 的，这里就可以直接使用首字母大写来进行读取数据

然后我们就可以开始进行调试了
（不要忘了source install/setup.bash哦）

后面我们会在 Action 中讲解更为快捷且通用的一种描述行为的方法，这种方法是相当美观的