ROS2 action 만들기

action 을 만들고 

action 서버-클라이언트 에서 사용해본다.

 

~/ros2_dev_ws/src$ ros2 pkg create action_tutorials_interfaces

 

.action 파일의 구조는 이런식이다. ---  (- 세개) 로 구분한다.

 

# Request

---

# Result

---

# Feedback

 

request 메세지는 액션 클라이언트에서 --->> 액션서버로 goal을 보낸다 .

result 메세지는 액션 서버에서 --->> 액션 클라이언트로 보내지는데 goal이완 료되면 보내진다.

feedback 메세지는 액션서버에서 --->>  액션 클라이언트로 정기적으로 보내진다. goal의 업데이트 상태를.

 

액션 인스턴스를 goal 이라고 부른다.

 

일단 만들어본다 .

 

action 디렉토리를 만든다.

~/ros2_dev_ws/src/action_tutorials_interfaces$ mkdir action

 

피보나치로 샘플을 만들거라서 Fibonacci.action 파일을 만들고 내용을 이렇게 채운다.

~/ros2_dev_ws/src/action_tutorials_interfaces/action$ cat Fibonacci.action 
int32 order
---
int32[] sequence
---
int32[] partial_sequence

 

빌드해본다.

 

CMakeLists.txt에 이걸 추가한다.

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
    "action/Fibonacci.action"
    )

 

package.xml 도 바꾼다. 이거 세줄 추가

  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <depend>action_msgs</depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

 

빌드한다.

~/ros2_dev_ws$ colcon build --packages-select action_tutorials_interfaces
Starting >>> action_tutorials_interfaces
Finished <<< action_tutorials_interfaces [3.94s]                     

Summary: 1 package finished [4.02s]

 

만든거 확인해보기.

$ ros2 action info action_tutorials_interfaces/action/Fibonacci
Action: action_tutorials_interfaces/action/Fibonacci
Action clients: 0
Action servers: 0

 

새로만든 action interface 로 server-client 만들어보기

 

패키지를 만든다.

위에서 만든 action_tutorials_interfaces 를 dependency 걸어준다.

~/ros2_dev_ws/src$ ros2 pkg create --dependencies  \

        action_tutorials_interfaces \

        rclcpp \

        rclcpp_action \

        rclcpp_components \

        -- action_tutorials_cpp

 

요 내용들을 코드에서 잘 살펴본다.

 

템플릿 액션 타입 이름 : Fibonacci

ROS2 액션에 추가한다. this

액션 이름은 : fibonacci

goal을 핸들링 할 콜백 함수 : handle_goal

cancel 을 핸들링할 콜백 함수 : handle_cancel

goal accept할 콜백함수 : handle_accept

 

~/ros2_dev_ws/src/action_tutorials_cpp/src$ cat fibonacci_action_server.cpp

#include <functional>
#include <memory>
#include <thread>

#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node //Node를 상속 받는다.
{
public:
  using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

  explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("fibonacci_action_server", options) //이름을 지정한다.
  {
    using namespace std::placeholders;

	//액션 서버 인스턴스를 만든다.
    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
      this,
      "fibonacci",
      std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
      std::bind(&FibonacciActionServer::handle_cancel, this, _1),
      std::bind(&FibonacciActionServer::handle_accepted, this, _1));
  }

private:
  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

  //goal을 핸들링 할 콜백 함수 : handle_goal
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Fibonacci::Goal> goal)
  {
    RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
    (void)uuid;
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
  //cancel 을 핸들링할 콜백 함수 : handle_cancel
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }
  //goal accept할 콜백함수 : handle_accept
  void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    using namespace std::placeholders;
    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
    std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
  }

  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    rclcpp::Rate loop_rate(1);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    auto & sequence = feedback->partial_sequence;
    sequence.push_back(0);
    sequence.push_back(1);
    auto result = std::make_shared<Fibonacci::Result>();

    //피보나치 계산.
    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
      // Check if there is a cancel request
      if (goal_handle->is_canceling()) {  //중간에 취소 가능하다.
        result->sequence = sequence;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal canceled");
        return;
      }
      // Update sequence
      sequence.push_back(sequence[i] + sequence[i - 1]);
      // Publish feedback
      goal_handle->publish_feedback(feedback);  //피드백으로 돌려준다.
      RCLCPP_INFO(this->get_logger(), "Publish feedback");

      loop_rate.sleep();
    }

    // Check if goal is done
    if (rclcpp::ok()) {
      result->sequence = sequence;
      goal_handle->succeed(result); //완료되면 보내준다.
      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
    }
  }
};  // class FibonacciActionServer

}  // namespace action_tutorials_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)

 

CMakeLists.txt 에는 이것을 추가한다.

 

add_library(action_server SHARED
  src/fibonacci_action_server.cpp)
target_include_directories(action_server PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_definitions(action_server
  PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_server
  "action_tutorials_interfaces"
  "rclcpp"
  "rclcpp_action"
  "rclcpp_components")
rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)
install(TARGETS
  action_server
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

 

$ colcon build --packages-select action_tutorials_cpp
Starting >>> action_tutorials_cpp
Finished <<< action_tutorials_cpp [2.22s]                     

Summary: 1 package finished [2.30s]

 

이제는 client 를 만든다.

 

~/ros2_dev_ws/src/action_tutorials_cpp/src$ cat fibonacci_action_client.cpp

#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>

#include "action_tutorials_interfaces/action/fibonacci.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

namespace action_tutorials_cpp
{
class FibonacciActionClient : public rclcpp::Node //Node 상속
{
public:
  using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

  explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
  : Node("fibonacci_action_client", options)  // 이름은 fibonacci_action_client로 설정.
  {
	//새 액션 클라이언트 인스턴스 생성.
    this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
      this,
      "fibonacci");

	//500ms 마다 send_goal 을 보낸다.
    this->timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&FibonacciActionClient::send_goal, this));
  }

  void send_goal()
  {
    using namespace std::placeholders;

    this->timer_->cancel(); //타이머를 끈다.

	//액션 서버가 뜨기를 기다린다.
    if (!this->client_ptr_->wait_for_action_server()) {
      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
      rclcpp::shutdown();
    }

    //goal을 만든다.
    auto goal_msg = Fibonacci::Goal();
    goal_msg.order = 10;

    RCLCPP_INFO(this->get_logger(), "Sending goal");

	//response, feedback, result callback들을 등록한다.

    auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
    send_goal_options.goal_response_callback =
      std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
      std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
      std::bind(&FibonacciActionClient::result_callback, this, _1);

	//goal_msg 를 send_goal_options 랑 같이 보낸다.
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;

  void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
  {
    auto goal_handle = future.get();
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
    }
  }

  void feedback_callback(
    GoalHandleFibonacci::SharedPtr,
    const std::shared_ptr<const Fibonacci::Feedback> feedback)
  {
    std::stringstream ss;
    ss << "Next number in sequence received: ";
    for (auto number : feedback->partial_sequence) {
      ss << number << " ";
    }
    RCLCPP_INFO(this->get_logger(), ss.str().c_str());
  }

  void result_callback(const GoalHandleFibonacci::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
        return;
    }
    std::stringstream ss;
    ss << "Result received: ";
    for (auto number : result.result->sequence) {
      ss << number << " ";
    }
    RCLCPP_INFO(this->get_logger(), ss.str().c_str());
    rclcpp::shutdown();
  }
};  // class FibonacciActionClient

}  // namespace action_tutorials_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)

CMakeLists.txt 수정.

이거 추가.

add_library(action_client SHARED
  src/fibonacci_action_client.cpp)
target_include_directories(action_client PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_definitions(action_client
  PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_client
  "action_tutorials_interfaces"
  "rclcpp"
  "rclcpp_action"
  "rclcpp_components")
rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client)
install(TARGETS
  action_client
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

$ colcon build --packages-select action_tutorials_cpp
Starting >>> action_tutorials_cpp
Finished <<< action_tutorials_cpp [3.27s]                     

Summary: 1 package finished [3.36s]

 

 

실행해본다.

$ ros2 run action_tutorials_cpp fibonacci_action_server 
[INFO] [1610544966.152762248] [fibonacci_action_server]: Received goal request with order 10
[INFO] [1610544966.209666531] [fibonacci_action_server]: Executing goal
[INFO] [1610544966.209851401] [fibonacci_action_server]: Publish feedback
[INFO] [1610544967.209933326] [fibonacci_action_server]: Publish feedback
[INFO] [1610544968.209967688] [fibonacci_action_server]: Publish feedback
[INFO] [1610544969.209902938] [fibonacci_action_server]: Publish feedback
[INFO] [1610544970.209905798] [fibonacci_action_server]: Publish feedback
[INFO] [1610544971.209905224] [fibonacci_action_server]: Publish feedback
[INFO] [1610544972.209944275] [fibonacci_action_server]: Publish feedback
[INFO] [1610544973.209951445] [fibonacci_action_server]: Publish feedback
[INFO] [1610544974.209912009] [fibonacci_action_server]: Publish feedback
[INFO] [1610544975.210549244] [fibonacci_action_server]: Goal succeeded


$ ros2 run action_tutorials_cpp fibonacci_action_client 
[INFO] [1610544966.151084054] [fibonacci_action_client]: Sending goal
[INFO] [1610544966.209747609] [fibonacci_action_client]: Goal accepted by server, waiting for result
[INFO] [1610544966.210068637] [fibonacci_action_client]: Next number in sequence received: 0 1 1 
[INFO] [1610544967.210357407] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 
[INFO] [1610544968.210365351] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 
[INFO] [1610544969.210282595] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 
[INFO] [1610544970.210300445] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 
[INFO] [1610544971.210289341] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 
[INFO] [1610544972.210337676] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 
[INFO] [1610544973.210346620] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 34 
[INFO] [1610544974.210316499] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 34 55 
[INFO] [1610544975.211311037] [fibonacci_action_client]: Result received: 0 1 1 2 3 5 8 13 21 34 55