ROS学习（2）（ service and client）

最新推荐文章于 2023-05-22 11:01:24 发布

蓝天巨人

最新推荐文章于 2023-05-22 11:01:24 发布

阅读量993

点赞数

分类专栏： ROS 文章标签：学习

本文链接：https://blog.csdn.net/qq_44632658/article/details/129657085

版权

ROS 专栏收录该内容

10 篇文章 1 订阅

订阅专栏

当节点使用服务进行通信时，发送数据请求的节点称为客户端节点，响应请求的节点称为服务节点。请求和响应的结构由.srv文件确定。
这里使用的例子是一个简单的整数加法系统;一个节点请求两个整数之和，而另一个节点以结果响应。

创建对应服务功能包

ros2 pkg create --build-type ament_cmake cpp_srvcli --dependencies rclcpp

创建对应的消息

在当前的功能包的目录下进行创建，创建对应的srv文件，并在次文件的下进行服务文件的建立

int64 a
int64 b
---
int64 sum

建立了对应的文件之后，需要针对建立的文件进行编译处理，修改对应的xml文件和CMakeLists.txt文件

修改xml文件

打开package.xml，并添加以下行：

<buildtool_depend>rosidl_default_generators</buildtool_depend>

<exec_depend>rosidl_default_runtime</exec_depend>

<member_of_group>rosidl_interface_packages</member_of_group>

注意，在构建时，我们需要rosidl_default_generators，而在运行时，我们只需要rosidl_default_runtime。
打开CMakeLists.txt并添加以下行：

修改cmakeList文件

查找从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 <memory>
//服务端处理函数
void add(const std::shared_ptr<cpp_srvcli::srv::AddTwoInts::Request> request,
          std::shared_ptr<cpp_srvcli::srv::AddTwoInts::Response>      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<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");

  rclcpp::Service<cpp_srvcli::srv::AddTwoInts>::SharedPtr service =
    node->create_service<cpp_srvcli::srv::AddTwoInts>("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 <chrono>
#include <cstdlib>
#include <memory>

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<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_client");
  rclcpp::Client<cpp_srvcli::srv::AddTwoInts>::SharedPtr client =
    node->create_client<cpp_srvcli::srv::AddTwoInts>("add_two_ints");

  auto request = std::make_shared<cpp_srvcli::srv::AddTwoInts::Request>();
  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
   */
  template<typename RepT = int64_t, typename RatioT = std::milli>
  bool
  wait_for_service(
    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
  {
    return wait_for_service_nanoseconds(
      std::chrono::duration_cast<std::chrono::nanoseconds>(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<rmw_request_id_t> request_header,
    std::shared_ptr<void> response) override
  {
    std::unique_lock<std::mutex> lock(pending_requests_mutex_);
    auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(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 place
    if (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);
  }

  SharedFuture
  async_send_request(SharedRequest request)
  {
    return async_send_request(request, [](SharedFuture) {});
  }

rclcpp::spin_until_future_complete(node, result)：得到对应的结果

spin_until_future_complete(
  std::shared_ptr<NodeT> node_ptr,
  const std::shared_future<FutureT> & future,
  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
  return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);
}

配置CMakeLists.txt文件

add_executable(server src/add_two_ints_server.cpp)
ament_target_dependencies(server
  rclcpp )
rosidl_target_interfaces(server
  ${PROJECT_NAME} "rosidl_typesupport_cpp")


add_executable(client src/add_two_ints_client.cpp)
ament_target_dependencies(client
  rclcpp )
rosidl_target_interfaces(client
  ${PROJECT_NAME} "rosidl_typesupport_cpp")

install(TARGETS client server
  DESTINATION 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_srvcli
  1 package had stderr output: cpp_srvcli