ROS2 with C++ - Lifecycle Nodes

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

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

 

managed nodes는 현재 C++에서만 사용 가능합니다.

 

 

1. Managed Nodes

ROS2에는 다음과 같이 2가지 종류의 node가 있습니다.

- Node()를 상속받은 node

- LifecycleNode()를 상속받은 node

 

managed nodes가 필요한 이유는?

- 몇몇 node를 협동하게 해야할 때 필요합니다. 하나의 로봇 어플리케이션에 포함된 모든 node은 동작하기 전에 특정한 순서를 따라서 시작하게 됩니다. 예를 들어, 우리는 map server가 지도 데이터를 주기 전에 localization system이 켜지는 것을 원하지는 않을 것입니다.

- 예를 들어, 특정한 소프트웨어를 잠깐 정지하거나 그 소프트웨어를 껐다가 키지 않고 그냥 다시 실행(re-launch)하고 싶을 수도 있습니다. 예를 들어, 뭔가 주변 환경이나 조건 등이 바뀌어서 내비게이션 시스템을 재시작하는 경우가 있을 수 있습니다.

 

근데 Managed nodes는 아직 완벽하게 구현되지는 않았습니다. 그래도 Nave2나 Moveit2 같이 많은 ROS2 프로젝트에서 사용하고 있습니다.

 

managed node란?

managed node에는 몇 가지 상태가 있습니다. 각 node는 현재 상태에 따라 다른 일을 하게됩니다. 가능한 상태는 다음과 같습니다.

- Unconfigured

- Inactive

- Active

- Finaized

 

처음 managed node가 만들어지면, Unconfigured 상태로 시작하게 됩니다. node를 어떤 상태로부터 다른 상태로 바꾸기 위해서는 누군가가 직접 바꿔줘야 합니다. 이는 보통 node들을 관리하는 외부 node에 의해서 처리될 수도 있고, 커맨드 라인에서도 가능합니다.

 

 

1. 간단한 managed node 예제

다음 명령으로 패키지를 만듭니다

$ ros2 pkg create --build-type ament_cmake managed_scan --dependencies rclcpp

 

managed_scan.cpp

#include <chrono>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <utility>

#include "lifecycle_msgs/msg/transition.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/publisher.hpp"

#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"

#include "rcutils/logging_macros.h"

#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class ManagedScan : public rclcpp_lifecycle::LifecycleNode
{
public:

  explicit ManagedScan(const std::string & node_name, bool intra_process_comms = false)
  : rclcpp_lifecycle::LifecycleNode(node_name,
      rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms))
  {}

  void
  publish()
  {
    static size_t count = 0;
    auto msg = std::make_unique<std_msgs::msg::String>();
    msg->data = "Lifecycle HelloWorld #" + std::to_string(++count);

    if (!pub_->is_activated()) {
      RCLCPP_INFO(
        get_logger(), "Lifecycle publisher is currently inactive. Messages are not published.");
    } else {
      RCLCPP_INFO(
        get_logger(), "Lifecycle publisher is active. Publishing: [%s]", msg->data.c_str());
    }

    pub_->publish(std::move(msg));
  }

  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_configure(const rclcpp_lifecycle::State &)
  {
    pub_ = this->create_publisher<std_msgs::msg::String>("managed_scan", 10);
    timer_ = this->create_wall_timer(
      1s, std::bind(&ManagedScan::publish, this));

    RCLCPP_INFO(get_logger(), "on_configure() is called.");

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }


  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_activate(const rclcpp_lifecycle::State &)
  {
    pub_->on_activate();

    RCUTILS_LOG_INFO_NAMED(get_name(), "on_activate() is called.");

    std::this_thread::sleep_for(2s);

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_deactivate(const rclcpp_lifecycle::State &)
  {
    pub_->on_deactivate();

    RCUTILS_LOG_INFO_NAMED(get_name(), "on_deactivate() is called.");

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_cleanup(const rclcpp_lifecycle::State &)
  {

    timer_.reset();
    pub_.reset();

    RCUTILS_LOG_INFO_NAMED(get_name(), "on cleanup is called.");

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_shutdown(const rclcpp_lifecycle::State & state)
  {

    timer_.reset();
    pub_.reset();

    RCUTILS_LOG_INFO_NAMED(
      get_name(),
      "on shutdown is called from state %s.",
      state.label().c_str());

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }
    
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_error(const rclcpp_lifecycle::State &)
  {

    RCUTILS_LOG_INFO_NAMED(get_name(), "something went wrong!");

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
  }

private:
  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> pub_;

  std::shared_ptr<rclcpp::TimerBase> timer_;
};


int main(int argc, char * argv[])
{
  // force flush of the stdout buffer.
  // this ensures a correct sync of all prints
  // even when executed simultaneously within the launch file.
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);

  rclcpp::init(argc, argv);

  rclcpp::executors::SingleThreadedExecutor exe;

  std::shared_ptr<ManagedScan> lc_node =
    std::make_shared<ManagedScan>("managed_scan_node");

  exe.add_node(lc_node->get_node_base_interface());

  exe.spin();

  rclcpp::shutdown();

  return 0;
}

 

start_managed_scan.launch.py

from launch import LaunchDescription
from launch_ros.actions import LifecycleNode


def generate_launch_description():
    return LaunchDescription([
        LifecycleNode(package='managed_scan', executable='scan_publisher',
                      name='managed_scan_node', namespace='', output='screen')
    ])

 

빌드 후, 실행해봅니다.

$ ros2 launch managed_scan start_managed_scan.launch.py
$ ros2 topic list | grep /managed_scan

우리는 publisher에다가 "managed_scan" 이라는 이름의 topic을 만들라고 했는데, 다른 것이 생겼습니다.

이 topic은 <node_name>/transition_event 형식으로 자동으로 만들어집니다. 또한, 이 topic은 해당 node가 언제 transition되는지를 확인하기 위해 사용됩니다. 우리는 이 topic을 확인해서 해당 node의 상태가 바뀌는 순간을 알 수 있습니다. 

 

그런데 정작 우리가 필요한 topic인 /managed_scan은 아직 publish되지 않고 있습니다. 이는 현재 node의 상태가 topic을 만들 수 있는 상태가 아니기 때문입니다. 아까 작성했던 코드를 보면,

  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_configure(const rclcpp_lifecycle::State &)
  {
    pub_ = this->create_publisher<std_msgs::msg::String>("managed_scan", 10);
    timer_ = this->create_wall_timer(
      1s, std::bind(&ManagedScan::publish, this));

    RCLCPP_INFO(get_logger(), "on_configure() is called.");

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

 

함수 이름에서 알 수 있듯이, on_configure상태가 되어야 publisher를 만들게 됩니다. 즉, managed node가 configured 상태로 바뀌어야 합니다. 지금 이 함수 내부에는 해당 node가 실행되기 전에 필요한 모든 것들을 다 넣어둬야 합니다. 즉, 해당 node의 생성자 처럼 생각하면 되겠습니다.

 

여기서 중요하게 봐야할 것이 있는데, publisher의 타입이 이전의 예제들과 다르게 LifecyclePublisher로 선언되었다는 것입니다.

std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> pub_;

 

이 자료형은 해당 publisher가 적절한 상태에서 activation 또는 deactivation상태로 변할 수 있게 해줍니다. 현재 ROS2 humble에서는 LifecycleSubscriber는 아직 없습니다.

 

현재 node 상태를 아는 방법

서비스를 보내서 확인할 수 있습니다.

$ ros2 service list | grep managed_scan
$ ros2 service call /managed_scan_node/get_state lifecycle_msgs/srv/GetState {}

 

command-line-interface(CLI)에서 managed nodes 다루기

 

사실, service를 보내지 않고 node의 이름만 알아도 확인할 수 있습니다.

$ ros2 lifecycle get /managed_scan_node

 

 

다음 명령들로 해당 node의 상태를 바꿀 수 있습니다. 둘 중 아무거나 사용해도 됩니다.

$ ros2 service call /managed_scan_node/change_state lifecycle_msgs/ChangeState "{transition: {id: 2}}"
# ros2 lifecycle set <node_name> <name_of_the_new_state>
$ ros2 lifecycle set /managed_scan_node configure

 

 

다음 명령으로 상태를 바꾸는 순간의 반응을 확인해봅니다.

$ ros2 topic echo /managed_scan_node/transition_event
$ ros2 lifecycle set /managed_scan_node configure

unconfigured에서 configuring로 바뀌고, configuring에서 inactive로 바뀐 결과를 보여줍니다. 즉, 현재는 inactive 상태입니다.

 

 

다음 명령으로 해당 node에 대한 lifecycle을 확인할 수 있습니다. 디버깅할때 유용합니다.

$ ros2 topic echo /managed_scan

 

 

현재는 inactive 상태이므로, topic은 생기긴 했지만 아직 데이터는 나오고 있지 않습니다. 

$ ros2 topic echo /managed_scan

 

아래 명령으로 activate 상태로 바꾸면 이제 데이터가 나오는 것을 확인할 수 있습니다.

$ ros2 lifecycle set /managed_scan_node activate

 

node를 멈추기 위해서는 상태를 finalized로 바꿔야 합니다. node가 기존에 어떤 상태였든지 간에, 그냥 바로 finalized로 바꾸는 것이 가능합니다.

 

아래 명령으로 node를 finalized로 바꿀 수 있습니다.

$ ros2 lifecycle set /managed_scan_node shutdown

 

그러나 아래 명령으로 확인해보면, node가 아직 죽지는 않은 것으로 나옵니다.

$ ros2 node list | grep managed_scan

 

하지만 여기서 중요하게 알아둬야 할 점은, 이미 finalized된 node는 다시 살릴 수 없습니다.

 

 

다음 명령으로 node를 일시정지 할 수 있습니다.

$ ros2 lifecycle set /managed_scan_node deactivate

activate를 보내면 다시 활성화 됩니다.

 

 

Life Cycle Manager

life cycle manager는 node를 관리하기 위한 프로그램입니다. 모든 managed node는 외부에서 상태를 바꿀 수 있도록 하는 service를 제공합니다.

 

다음 코드는 managed_scan_node를 activate 상태로 바꾸는 예시입니다.

lifecycle_manager.cpp

#include <chrono>
#include <memory>
#include <string>
#include <thread>

//#include "lifecycle_msgs/msg/state.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"

#include "rclcpp/rclcpp.hpp"

#include "rcutils/logging_macros.h"

using namespace std::chrono_literals;


static constexpr char const * change_state_topic = "/managed_scan_node/change_state";

template<typename FutureT, typename WaitTimeT>
std::future_status
wait_for_result(
  FutureT & future,
  WaitTimeT time_to_wait)
{
  auto end = std::chrono::steady_clock::now() + time_to_wait;
  std::chrono::milliseconds wait_period(100);
  std::future_status status = std::future_status::timeout;
  do {
    auto now = std::chrono::steady_clock::now();
    auto time_left = end - now;
    if (time_left <= std::chrono::seconds(0)) {break;}
    status = future.wait_for((time_left < wait_period) ? time_left : wait_period);
  } while (rclcpp::ok() && status != std::future_status::ready);
  return status;
}

class MyLifecycleServiceClient : public rclcpp::Node
{
private:
  std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_state_;

public:
  explicit MyLifecycleServiceClient(const std::string & node_name)
  : Node(node_name)
  {}

  void
  init()
  {
    client_change_state_ = this->create_client<lifecycle_msgs::srv::ChangeState>(
      change_state_topic);
  }


  bool
  change_state(std::uint8_t transition, std::chrono::seconds time_out = 3s)
  {
    auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
    request->transition.id = transition;

    if (!client_change_state_->wait_for_service(time_out)) {
      RCLCPP_ERROR(
        get_logger(),
        "Service %s is not available.",
        client_change_state_->get_service_name());
      return false;
    }

    auto future_result = client_change_state_->async_send_request(request);

    auto future_status = wait_for_result(future_result, time_out);

    if (future_status != std::future_status::ready) {
      RCLCPP_ERROR(
        get_logger(), "Server time out while getting current state for node managed_scan_node");
      return false;
    }

    if (future_result.get()->success) {
      RCLCPP_INFO(
        get_logger(), "Transition %d successfully triggered.", static_cast<int>(transition));
      return true;
    } else {
      RCLCPP_WARN(
        get_logger(), "Failed to trigger transition %u", static_cast<unsigned int>(transition));
      return false;
    }
  }

};

void
sv_callback(std::shared_ptr<MyLifecycleServiceClient> lc_client)
{
  rclcpp::WallRate time_between_state_changes(0.1);  // 10s

  if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)) 
      return;

  time_between_state_changes.sleep();

  if (!rclcpp::ok())
      return;
    
  if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE))
      return;
}

int main(int argc, char ** argv)
{
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);

  rclcpp::init(argc, argv);

  auto manager_client_node = std::make_shared<MyLifecycleServiceClient>("manager_client");
  manager_client_node->init();

  rclcpp::executors::SingleThreadedExecutor exe;
  exe.add_node(manager_client_node);

  std::shared_future<void> script = std::async(
    std::launch::async,
    std::bind(sv_callback, manager_client_node));
  exe.spin_until_future_complete(script);

  rclcpp::shutdown();

  return 0;
}

 

기존에 만들었던 launch 파일을 다음과 같이 수정합니다

start_managed_scan.launch.py

from launch import LaunchDescription
from launch_ros.actions import LifecycleNode, Node


def generate_launch_description():
    return LaunchDescription([
        LifecycleNode(package='managed_scan', executable='scan_publisher',
                      name='managed_scan_node', namespace='', output='screen'),
        Node(package='managed_scan', name='manager_client_node', 
             executable='lifecycle_manager', output='screen')
    ])

 

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(managed_scan)

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(lifecycle_msgs REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(std_msgs REQUIRED)

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()

add_executable(scan_publisher src/managed_scan.cpp)
ament_target_dependencies(scan_publisher 
    "lifecycle_msgs"
    "rclcpp_lifecycle"
    "std_msgs"
    "rclcpp"
    )

add_executable(lifecycle_manager src/lifecycle_manager.cpp)
ament_target_dependencies(lifecycle_manager
    "lifecycle_msgs"
    "rclcpp_lifecycle"
    "std_msgs"
  )

install(TARGETS
   scan_publisher
   lifecycle_manager
   DESTINATION lib/${PROJECT_NAME}
 )

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

 

빌드 후, 실행하면 3초 뒤에 node가 activate 되는 것을 확인할 수 있습니다.