5.ROS2服务通信
文章目录
- 5.ROS2服务通信
- 5.1ROS2中的服务工具
- 5.2 rclcpp实现服务通信
- 5.2.1创建功能包和节点
- 5.2.2服务端实现
- 5.2.3客户端实现
- Reference
服务通信也是ROS2中基本的通信方式,服务分为客户端和服务端,由客户端向服务端发送请求,然后服务端对客户端的请求进行处理,最后将处理后的结果返回给客户端,所以服务-客户端模型,可以称为请求-响应模型。
服务通信有一些需要注意的事项:
- 同一个服务(名称相同)有且只能由一个节点来进行提供
- 同一个服务可以被多个客户端调用。
服务通信的运行示例,如图所示:
5.1ROS2中的服务工具
安装ROS2的时候提供了一些样例安装程序,其中就提供了服务通信的样例,我们可以直接调用
ros2 service -h
来查看相关的CLI工具。
5.2 rclcpp实现服务通信
5.2.1创建功能包和节点
这里我的工作空间名称为colcon_test03_ws
ros2 pkg create example_service_rclcpp --build-type ament_cmake --dependencies rclcpp --license Apache-2.0
touch example_service_rclcpp/src/service_server_01.cpp
touch example_service_rclcpp/src/service_client_01.cpp
然后分别编写如下代码:
service_server_01.cpp
#include "rclcpp/rclcpp.hpp"class ServiceServer01: public rclcpp::Node{
public:ServiceServer01(std::string name) : Node(name){RCLCPP_INFO(this->get_logger(), "Node: %s has been launched", name.c_str());}
private:};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;
}
service_client_01.cpp
#include "rclcpp/rclcpp.hpp"class ServiceClient01: public rclcpp::Node{
public:ServiceClient01(std::string name) : Node(name){RCLCPP_INFO(this->get_logger(), "Node: %s has been launched", name.c_str());}
};int main(int argc, char** argv){rclcpp::init(argc, argv);auto node = std::make_shared<ServiceClient01>("service_client_01");rclcpp::spin(node);rclcpp::shutdown();return 0;
}
然后修改配置文件CMakeLists.txt
add_executable(service_server_01 src/service_server_01.cpp)
ament_target_dependencies(service_server_01 rclcpp)add_executable(service_client_01 src/service_client_01.cpp)
ament_target_dependencies(service_client_01 rclcpp)install(TARGETSservice_server_01service_client_01DESTINATION lib/${PROJECT_NAME}
)
接着进行编译
colcon build --packages-select example_service_rclcpp
如果编译没问题就可以进行下一步了。
5.2.2服务端实现
(1)导入消息接口interface
这里使用的服务文件是内置的,并不是我们自己自定义的,在后续的文章中会介绍如何自定义自己的.msg
和.srv
文件。
我们使用ROS2自带的example_interfaces
接口,先查看一下这个接口的信息
ros2 interface show example_interfaces/srv/AddTwoInts
结果如下:
int64 a
int64 b
---
int64 sum
可以看出该服务的接口接收两个int
的数字a
和b
,相加之后返回一个int
类型的sum
。
ament_cmake
类型的功能包导入消息接口分为三步:
- 在
CMakeListts.txt
中导入,具体是先find_packages
再ament_target_dependencies
- 在
packages.xml
中导入,具体是添加到depend
标签并将消息接口写入- 在代码中导入,Cpp中是
#include "消息功能包/xxx/xxx.hpp"
具体操作如下:
CMakeLists.txt
find_package(example_interfaces REQUIRED)add_executable(service_server_01 src/service_server_01.cpp)
ament_target_dependencies(service_server_01 rclcpp example_interfaces)add_executable(service_client_01 src/service_client_01.cpp)
ament_target_dependencies(service_client_01 rclcpp example_interfaces)
packages.xml
<depend>example_interfaces</depend>
代码
#include "example_interfaces/srv/add_two_ints.hpp"
(2)编写服务端代码
相关API文档可以参考:https://docs.ros2.org/latest/api/rclcpp/
我们需要设置的参数就三个,分别是:
ServiceT
,是我们指定的消息数据接口,这里是example_interfaces::srv::AddTwoInts
service_name
,是我们的服务名称callback
,是回调函数,使用成员函数作为回调函数,std::bind
进行转换
编写服务端代码
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"class ServiceServer01: public rclcpp::Node{
public:ServiceServer01(std::string name) : Node(name){RCLCPP_INFO(this->get_logger(), "Node: %s has been launched", 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(), "Recieve 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;
}
5.2.3客户端实现
(1)相关API
相关API文档可以参考:https://docs.ros2.org/latest/api/rclcpp/
create_client
一共有两个参数需要我们进行指定:
ServiceT
,是我们指定的消息数据接口,这里是example_interfaces::srv::AddTwoInts
service_name
,对应我们请求的服务名称
async_send_request
用于异步发送消息
request
,是用于请求的消息,这里存放两个数a,bCallBack
,回调函数,异步接受服务器的返回的函数
wait_for_service
这个函数在等待服务上线之后返回true
(2)编写客户端程序
#include "rclcpp/rclcpp.hpp"
#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(), "Node: %s has been launched", 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(), "Calculate %d + %d", a, b);// 等待服务上线while (!client_->wait_for_service(std::chrono::seconds(1))){if (!rclcpp::ok()){RCLCPP_ERROR(this->get_logger(), "Waiting for service to be interrupted");return;}RCLCPP_INFO(this->get_logger(), "Waiting for service");}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(), "Result: %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;
}
这里需要讲解的就是这个回调函数
void result_callback_(rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture result_future)
函数的参数是客户端AddTwoInts
类型的SharedFuture
对象,这个对象的定义如下:
这个对象是利用c+11
的新特性std::shared_future
创建的SharedResponse
类模板,类模板std::shared_future
提供访问异步操作结果的机制,类似于std::future
,允许多个线程共享同一个状态,详细可以查看std::shared_future
的API
可以使用其get()
方法来获得结果。
最后编译然后运行
cd colcon_test_ws
colcon build --packages-select example_service_rclcpp
source install/setup.bash
ros2 run example_service_rclcpp service_client_01ros2 run example_service_rclcpp service_server_01
最后的运行结果如下:
Reference
[1]d2lros2
[2]ROS2 Tutorial Official