ROS2 with C++ - Service

본 포스트는 ROS1에 대한 전반적인 내용 및 ROS2 Python에 대한 지식이 있다는 가정 하에 작성되었으며, The Construct Youtube 영상을 참고하였습니다.

또한, 시뮬레이션 환경은 turtlebot3 waffle_pi를 사용합니다 : https://github.com/ROBOTIS-GIT/turtlebot3_simulations

 

 

1. Service Client

기본적인 내용은 python과 같으니, 다 생략하고 바로 예제로 넘어갑니다.

 

1. 패키지 만들기

$ ros2 pkg create client_cpp_pkg --build-type ament_cmake --dependencies rclcpp std_srvs

 

2. cpp 코드 작성

service_client.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/empty.hpp"

#include <chrono>
#include <cstdlib>
#include <memory>

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("service_client");
  rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
    node->create_client<std_srvs::srv::Empty>("reset_world");

  auto request = std::make_shared<std_srvs::srv::Empty::Request>();

  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_future = client->async_send_request(request);
  if (rclcpp::spin_until_future_complete(node, result_future) ==
    rclcpp::FutureReturnCode::SUCCESS)
  {
    auto result = result_future.get();
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "The world has been reset");
  } else {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service /reset_world");
  }

  rclcpp::shutdown();
  return 0;
}

 

3. launch 파일 만들기

service_client.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='client_cpp_pkg',
            executable='service_client_node',
            output='screen'),
    ])

 

4. CMakeLists.txt 수정

cmake_minimum_required(VERSION 3.8)
project(client_cpp_pkg)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_srvs REQUIRED)

add_executable(service_client_node src/service_client.cpp)
ament_target_dependencies(service_client_node rclcpp std_srvs)

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

# Install launch files.
install(DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}/
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

 

5. 빌드 후, 실행

$ ros2 launch client_pkg service_client_launch_file.launch.py

 

launch 파일을 실행하면 시뮬레이션이 초기화되는 것을 볼 수 있습니다

 

 

2. Spinning Node에서 service call 하기

이전 예제의 line 29~31을 보면 다음과 같습니다.

그런데 node가 이미 디폴트로 spin 상태로 선언되어있을 수 있습니다. 이런 경우에 대한 예제를 보겠습니다.

 

 

1. cpp파일 생성

service_client_2.cpp

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "std_srvs/srv/empty.hpp"

#include <chrono>
#include <cstdlib>
#include <future>
#include <memory>

using namespace std::chrono_literals;

class ServiceClient : public rclcpp::Node {
private:
  rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client_;
  rclcpp::TimerBase::SharedPtr timer_;
  bool service_done_ = false;

  void timer_callback() {
    while (!client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(
            this->get_logger(),
            "Client interrupted while waiting for service. Terminating...");
        return;
      }
      RCLCPP_INFO(this->get_logger(),
                  "Service Unavailable. Waiting for Service...");
    }

    auto request = std::make_shared<std_srvs::srv::Empty::Request>();

    service_done_ = false;
    auto result_future = client_->async_send_request(
        request, std::bind(&ServiceClient::response_callback, this,
                           std::placeholders::_1));
  }

  void
  response_callback(rclcpp::Client<std_srvs::srv::Empty>::SharedFuture future) {
    auto status = future.wait_for(1s);
    if (status == std::future_status::ready) {
      RCLCPP_INFO(this->get_logger(), "Result: success");
      service_done_ = true;
    } else {
      RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
    }
  }

public:
  ServiceClient() : Node("service_client") {
    client_ = this->create_client<std_srvs::srv::Empty>("reset_world");
    timer_ = this->create_wall_timer(
        1s, std::bind(&ServiceClient::timer_callback, this));
  }

  bool is_service_done() const { return this->service_done_; }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);

  auto service_client = std::make_shared<ServiceClient>();
  while (!service_client->is_service_done()) {
    rclcpp::spin_some(service_client);
  }

  rclcpp::shutdown();
  return 0;
}

 

2. launch 파일 생성, CMakeLists.txt 파일 수정 후 실행해봅니다.

결과는 이전과 같습니다. 이전 코드와 달라진 부분은 아래와 같습니다.

 

 

3. Service Server 만들기

1. 패키지 생성 후, 코드 작성

$ ros2 pkg create server_cpp_pkg --build-type ament_cmake --dependencies rclcpp geometry_msgs std_srvs

 

service_server.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/set_bool.hpp"
#include "geometry_msgs/msg/twist.hpp"

#include <memory>

using SetBool = std_srvs::srv::SetBool;
using std::placeholders::_1;
using std::placeholders::_2;

class ServerNode : public rclcpp::Node
{
public:
  ServerNode()
  : Node("service_moving")
  {

    srv_ = create_service<SetBool>("moving_right", std::bind(&ServerNode::moving_callback, this, _1, _2));
    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);

  }

private:
  rclcpp::Service<SetBool>::SharedPtr srv_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;

  void moving_callback(
      const std::shared_ptr<SetBool::Request> request,
      const std::shared_ptr<SetBool::Response> response) 
    {

        auto message = geometry_msgs::msg::Twist();
    
        if (request->data == true)
        {   
            message.linear.x = 0.2;
            message.angular.z = -0.2;
            publisher_->publish(message);

            response->success = true;
            response->message = "Turning to the right!";
        }

        if (request->data == false)
        {
            message.linear.x = 0.0;
            message.angular.z = 0.0;
            publisher_->publish(message);

            response->success = false;
            response->message = "stop!"; 
        }    
                
    }
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ServerNode>());
  rclcpp::shutdown();
  return 0;
}

 

2. launch 파일 작성

service_server.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='server_cpp_pkg',
            executable='service_server_node',
            output='screen'),
    ])

 

3. CMakeLists.txt 수정

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(server_cpp_pkg)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_srvs REQUIRED)


add_executable(service_server_node src/service_server.cpp)
ament_target_dependencies(service_server_node rclcpp geometry_msgs std_srvs)

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

install(DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}/
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

 

4. 빌드 및 실행

$ ros2 service call /moving_right std_srvs/srv/SetBool data:\ true\
$ ros2 service call /moving_right std_srvs/srv/SetBool data:\ false\

 

 

 

 

Custom srvice만드는 법은 python과 동일하므로 생략하겠습니다.