当节点使用服务进行通信时,发送数据请求的节点称为客户端节点,响应请求的节点称为服务节点。请求和响应的结构由.srv文件确定。
这里使用的例子是一个简单的整数加法系统;一个节点请求两个整数之和,而另一个节点以结果响应。
ros2 pkg create --build-type ament_cmake cpp_srvcli --dependencies rclcpp
在当前的功能包的目录下进行创建,创建对应的srv文件,并在次文件的下进行服务文件的建立
int64 a
int64 b
---
int64 sum
建立了 对应的文件之后,需要针对建立的文件进行编译处理,修改对应的xml文件和CMakeLists.txt文件
打开package.xml,并添加以下行:
rosidl_default_generators rosidl_default_runtime rosidl_interface_packages
注意,在构建时,我们需要rosidl_default_generators,而在运行时,我们只需要rosidl_default_runtime。
打开CMakeLists.txt并添加以下行:
查找从msg/srv文件生成消息代码的包:
find_package(rosidl_default_generators REQUIRED)
声明要生成的消息列表:
set(msg_files"msg/AddressBook.msg"
)
通过手动添加.msg文件,我们可以确保CMake知道在您添加其他.msg文件后何时需要重新配置项目。
rosidl_generate_interfaces(${PROJECT_NAME}${msg_files}
)
还要确保导出消息运行时依赖项:
ament_export_dependencies(rosidl_default_runtime)
#include "rclcpp/rclcpp.hpp"
#include "cpp_srvcli/srv/add_two_ints.hpp"#include
//服务端处理函数
void add(const std::shared_ptr request,std::shared_ptr response)
{response->sum = request->a + request->b;RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",request->a, request->b);RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}int main(int argc, char **argv)
{rclcpp::init(argc, argv);std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_server");rclcpp::Service::SharedPtr service =node->create_service("add_two_ints", &add);RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");rclcpp::spin(node);rclcpp::shutdown();
}
#include "rclcpp/rclcpp.hpp"#include "cpp_srvcli/srv/add_two_ints.hpp"
#include
#include
#include using namespace std::chrono_literals;int main(int argc, char **argv)
{rclcpp::init(argc, argv);if (argc != 3) {RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y");return 1;}std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_client");rclcpp::Client::SharedPtr client =node->create_client("add_two_ints");auto request = std::make_shared();request->a = atoll(argv[1]);request->b = atoll(argv[2]);while (!client->wait_for_service(1s)) {if (!rclcpp::ok()) {RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");return 0;}RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");}auto result = client->async_send_request(request);// Wait for the result.if (rclcpp::spin_until_future_complete(node, result) ==rclcpp::FutureReturnCode::SUCCESS){RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);} else {RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints");}rclcpp::shutdown();return 0;
}
代码解读:解析wait_for_service函数等待一个服务准备好,让客户端地址停止一段时间
/// Wait for a service to be ready./*** \param timeout maximum time to wait* \return `true` if the service is ready and the timeout is not over, `false` otherwise*/templateboolwait_for_service(std::chrono::duration timeout = std::chrono::duration(-1)){return wait_for_service_nanoseconds(std::chrono::duration_cast(timeout));}
同步发送对应的请求auto result = client->async_send_request(request);
/// Handle a server response/*** \param[in] request_header used to check if the secuence number is valid* \param[in] response message with the server response*/
handle_response(std::shared_ptr request_header,std::shared_ptr response) override{std::unique_lock lock(pending_requests_mutex_);auto typed_response = std::static_pointer_cast(response);int64_t sequence_number = request_header->sequence_number;// TODO(esteve) this should throw instead since it is not expected to happen in the first placeif (this->pending_requests_.count(sequence_number) == 0) {RCUTILS_LOG_ERROR_NAMED("rclcpp","Received invalid sequence number. Ignoring...");return;}auto tuple = this->pending_requests_[sequence_number];auto call_promise = std::get<0>(tuple);auto callback = std::get<1>(tuple);auto future = std::get<2>(tuple);this->pending_requests_.erase(sequence_number);// Unlock here to allow the service to be called recursively from one of its callbacks.lock.unlock();call_promise->set_value(typed_response);callback(future);}SharedFutureasync_send_request(SharedRequest request){return async_send_request(request, [](SharedFuture) {});}
rclcpp::spin_until_future_complete(node, result):得到对应的结果
spin_until_future_complete(std::shared_ptr node_ptr,const std::shared_future & future,std::chrono::duration timeout = std::chrono::duration(-1))
{return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);
}
add_executable(server src/add_two_ints_server.cpp)
ament_target_dependencies(serverrclcpp )
rosidl_target_interfaces(server${PROJECT_NAME} "rosidl_typesupport_cpp")add_executable(client src/add_two_ints_client.cpp)
ament_target_dependencies(clientrclcpp )
rosidl_target_interfaces(client${PROJECT_NAME} "rosidl_typesupport_cpp")install(TARGETS client serverDESTINATION lib/${PROJECT_NAME}
)
关键,由于本功能包内使用了本包内定义的数据类型,包含了定义的头文件,需要针对其编译的CMakeLists.txt添加
rosidl_target_interfaces(client
# ${PROJECT_NAME} "rosidl_typesupport_cpp")
保证正确引入对应的头文件,否则就会出现错误。
fatal error: cpp_srvcli/srv/add_two_ints.hpp: 没有那个文件或目录11 | #include "cpp_srvcli/srv/add_two_ints.hpp"| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [CMakeFiles/client.dir/build.make:76:CMakeFiles/client.dir/src/add_two_ints_client.cpp.o] 错误 1
make[1]: *** [CMakeFiles/Makefile2:696:CMakeFiles/client.dir/all] 错误 2
make: *** [Makefile:146:all] 错误 2
---
Failed <<< cpp_srvcli [12.5s, exited with code 2]Summary: 1 package finished [14.4s]1 package failed: cpp_srvcli1 package had stderr output: cpp_srvcli