[ROS2] (16)-C++로 서비스와 클라이언트 만들기

서비스와 클라이언트 만들기

서비스에서 서비스를 request 하는 것을 client 노드라 하고, respond 하는 것을 service 또는, server 노드라고 한다. 요청과 응답의 구조는 .srv 파일에 의해 결정된다. 이번 포스팅에서는 클라이언트가 2개의 정수를 넘겨주면 서버에서 2개 정수의 합을 응답하는 예제를 만들어 볼 것이다.

패키지 만들기

먼저, 서비스와 클라이언트 노드를 포함할 패키지를 생성한다.

$ cd dev_ws/src
$ ros2 pkg create --build-type ament_cmake cpp_srvcli --dependencies rclcpp example_interfaces

여기서 --dependencies argument는 dependency 정보를 자동으로 package.xml 파일과 CMakeLists.txt 파일에 추가해주는 역할을 한다. example_interaces는 요청과 응답의 구조를 정의한 .srv 파일을 포함하는 패키지이다. 내 PC 기준으로는 opt/ros/foxy/share/example_interfaces/srv/AddTwoInts.srv 위치에 있고, 열어서 내용을 확인해보면 아래와 같다.

int64 a
int64 b
---
int64 sum

요청할 때 정수 a,b 를 넘겨주면 응답으로 정수 sum 을 돌려주는 것이다.

이제 필요한 경우, package.xml 파일의 description, maintainer 또는 license 정보 등을 입력해준다.

서비스 노드 작성하기

다음으로 dev_ws/src/cpp_srvcli/src 폴더에 add_two_ints_server.cpp 파일을 새로 만들고, 아래 내용을 붙여 넣는다.

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

#include <memory>

void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
          std::shared_ptr<example_interfaces::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<example_interfaces::srv::AddTwoInts>::SharedPtr service =
    node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");

  rclcpp::spin(node);
  rclcpp::shutdown();
}

코드의 상단은 필요한 dependency를 포함시키는 것이고, add 함수는 2개의 정수를 받아서 합을 response 해주는 함수이다.

main 함수의 내용을 자세히 살펴보자.

rclcpp::init(argc, argv)

ROS2 C++ client 라이브러리를 초기화하고 있다.

std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");

add_two_ints_server 라는 이름의 노드를 생성하고 있다.

rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =
node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);

add_two_ints 라는 이름의 서비스를 만들고 자동으로 네트워크에 advertise 하고 있다.

RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");

준비가 완료되면 메시지를 출력한다.

rclcpp::spin(node);

노드를 spin 시켜 서비스를 사용 가능하게 한다.

server라는 이름의 실행파일을 만들기 위해 아래 내용을 CMakeLists.txt에 추가해준다.

add_executable(server src/add_two_ints_server.cpp)
ament_target(server rclcpp example_interfaces)

ros2 run 명령어가 실행파일을 찾을 수 있게 아래 내용도 추가해준다.

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

클라이언트 노드 만들기

dev_ws/src/cpp_srvcli/src 폴더에 add_two_ints_client.cpp 파일을 새로 만들고, 아래 내용을 붙여넣는다.

#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/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<example_interfaces::srv::AddTwoInts>::SharedPtr client =
    node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");

  auto request = std::make_shared<example_interfaces::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;
}

코드를 자세히 살펴보자.

auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
request->a = atoll(argv[1]);
request->b = atoll(argv[2]);

요청을 만드는 부분이다. 구조는 .srv 파일에 정의되어 있다.

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...");
}

while 루프는 서비스 노드를 네트워크에서 1초 동안 찾도록 한다. 못 찾으면 계속 반복한다. 클라이언트가 취소되면(예: ctrl + c) 에러 메시지를 출력한다.

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");
}

다음으로 서버에 요청을 보내고, 응답이 오거나, 실패할 때까지 노드를 spin 한다.

서비스 노드에서 했던 것처럼 CMakeLists.txt 파일에 실행파일을 만드는 부분과 install 하는 부분을 추가한다.

add_executable(client src/add_two_ints_client.cpp)
ament_target_dependencies(client rclcpp example_interfaces)

install(TARGETS server client DESTINATION lib/${PROJECT_NAME})

패키지 빌드 및 실행

먼저, dependency를 설치한다.

$ cd ~/dev_ws
$ rosdep install -i --from-path src --rosdistro foxy -y

다음으로 새로 만든 패키지만 빌드한다.

$ cd ~/dev_ws
$ colcon build --packages-select cpp_srvcli

터미널을 새로 열고, 설정파일을 source 한 뒤, 서버 노드를 실행한다.

$ cd ~/dev_ws
$ . install/setup.bash
$ ros2 run cpp_srvcli server

새로운 터미널을 열고, 마찬가지로 클라이언트 노드를 실행한다. 이 때 클라이언트 노드 뒤로 숫자 2개를 추가해서 실행해준다.

$ cd ~/dev_ws
$ . install/setup.bash
$ ros2 run cpp_srvcli client 2 3

터미널 창에 응답으로 5를 출력해주는 것을 확인할 수 있다.