ROS2 with C++ - Executors and Callbacks

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

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

 

 

1. 간단한 Executor와 Callback 함수 구현하기

 

1. 아래 명령으로 패키지를 만듭니다

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

 

2. executor_example_1.cpp

#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>

class OdomSubscriber : public rclcpp::Node {
public:
  OdomSubscriber(std::string odom_topic_name) : Node("simple_subscriber") {
    subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
        odom_topic_name, 10,
        std::bind(&OdomSubscriber::topic_callback, this,
                  std::placeholders::_1));
  }

private:
  void topic_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(),
                "Odometry = ['%f','%f','%f']",
                msg->pose.pose.position.x,
                msg->pose.pose.position.y,
                msg->pose.pose.position.z);
  }
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
};

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

  std::shared_ptr<OdomSubscriber> node = std::make_shared<OdomSubscriber>("odom");

  RCLCPP_INFO(node->get_logger(), "Odom Subscriber has been started.");

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

line 32에서 SingleThreadedExecutor를 만들었습니다. 자세한 내용은 ROS2 with python 포스트를 참조하세요

 

3. CMakeLists.txt에 아래 코드를 추가해줍니다.

find_package(nav_msgs REQUIRED)

add_executable(executor_example_1_node src/executor_example_1.cpp)
ament_target_dependencies(executor_example_1_node rclcpp nav_msgs)

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

 

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(executors_exercises_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(nav_msgs REQUIRED)

add_executable(executor_example_1_node src/executor_example_1.cpp)
ament_target_dependencies(executor_example_1_node rclcpp nav_msgs)

install(TARGETS
   executor_example_1_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. package.xml에 아래 코드를 추가해줍니다.

  <depend>nav_msgs</depend>

 

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>executors_exercises_pkg</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="justin@todo.todo">justin</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>nav_msgs</depend>
  
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

 

5. launch 파일을 만듭니다.

executor_example_1.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

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

 

6. 빌드 후, 실행해봅니다.

$ ros2 launch executors_exercises_pkg executor_example_1.launch.py

 

 

2. Single Threaded, Multi Threaded Executors와 Callbacks 구현해보기

 

먼저, Single Threaded와 2개의 Callback을 테스트해보겠습니다.

 

1. cpp 파일 만들기

executor_example_2.cpp

#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <unistd.h>

using namespace std::chrono_literals;

class OdomSubscriber : public rclcpp::Node {
public:
  OdomSubscriber(std::string odom_topic_name) : Node("odom_subscriber") {
    subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
        odom_topic_name, 10,
        std::bind(&OdomSubscriber::topic_callback, this, std::placeholders::_1));
  }

private:
  void topic_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Odometry=['%f','%f','%f']",
                msg->pose.pose.position.x, msg->pose.pose.position.y,
                msg->pose.pose.position.z);
  }
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
};

class SlowTimer : public rclcpp::Node {
public:
  SlowTimer(float sleep_timer) : Node("slow_timer_subscriber") {
    this->wait_time = sleep_timer;
    timer_ = this->create_wall_timer(500ms, std::bind(&SlowTimer::timer_callback, this));
  }

private:
  void timer_callback() {
    sleep(this->wait_time);
    RCLCPP_INFO(this->get_logger(), "TICK");
  }
  rclcpp::TimerBase::SharedPtr timer_;
  float wait_time;
};

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

  std::shared_ptr<OdomSubscriber> odom_subs_node = std::make_shared<OdomSubscriber>("odom");

  float sleep_time = 3.0;
  std::shared_ptr<SlowTimer> slow_timer_node = std::make_shared<SlowTimer>(sleep_time);

  RCLCPP_INFO(odom_subs_node->get_logger(), "odom_subs_node INFO...");
  RCLCPP_INFO(slow_timer_node->get_logger(), "slow_timer_node INFO...");

  rclcpp::executors::SingleThreadedExecutor executor;
    
  executor.add_node(odom_subs_node);
  executor.add_node(slow_timer_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

 

2. CMakeLists.txt 수정하기

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(executors_exercises_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(nav_msgs REQUIRED)

add_executable(executor_example_1_node src/executor_example_1.cpp)
ament_target_dependencies(executor_example_1_node rclcpp nav_msgs)

add_executable(executor_example_2_node src/executor_example_2.cpp)
ament_target_dependencies(executor_example_2_node rclcpp nav_msgs)

install(TARGETS
   executor_example_1_node
   executor_example_2_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()

 

3. launch 파일 만들기

executor_example_2.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

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

 

4. 컴파일 후, 실행

$ ros2 launch executors_exercises_pkg executor_example_2.launch.py

 

Timer Callback에서의 sleep동안 odometry callback이 들어오지 않는 것을 확인할 수 있습니다

 

 

3. Multi-Threaded Executor와 2개의 node 구현해보기

1. cpp 파일 만들기

executor_example_3.cpp

#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <unistd.h>

using namespace std::chrono_literals;

class OdomSubscriber : public rclcpp::Node {
public:
  OdomSubscriber(std::string odom_topic_name) : Node("odom_subscriber") {

    subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
        odom_topic_name, 10,
        std::bind(&OdomSubscriber::topic_callback, this,
                  std::placeholders::_1));
  }

private:
  void topic_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Odometry=['%f','%f','%f']",
                msg->pose.pose.position.x, msg->pose.pose.position.y,
                msg->pose.pose.position.z);
  }
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
};

class SlowTimer : public rclcpp::Node {
public:
  SlowTimer(float sleep_timer) : Node("slow_timer_subscriber") {
    this->wait_time = sleep_timer;
    timer_ = this->create_wall_timer(
        500ms, std::bind(&SlowTimer::timer_callback, this));
  }

private:
  void timer_callback() {
    sleep(this->wait_time);
    RCLCPP_INFO(this->get_logger(), "TICK");
  }
  rclcpp::TimerBase::SharedPtr timer_;
  float wait_time;
};

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

  std::shared_ptr<OdomSubscriber> odom_subs_node = std::make_shared<OdomSubscriber>("odom");

  float sleep_time = 3.0;
  std::shared_ptr<SlowTimer> slow_timer_node =
      std::make_shared<SlowTimer>(sleep_time);

  RCLCPP_INFO(odom_subs_node->get_logger(), "odom_subs_node INFO...");
  RCLCPP_INFO(slow_timer_node->get_logger(), "slow_timer_node INFO...");

  rclcpp::executors::MultiThreadedExecutor executor;

  executor.add_node(odom_subs_node);
  executor.add_node(slow_timer_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

 

2. launch 파일 만들고, CMakeLists.txt 수정하고, 빌드하고, 실행해보기

이전과 계속 같은 방식이므로, 코드는 생략하고 결과만 보겠습니다.

중간 중간 Timer Callback이 들어오는 것을 확인할 수 있습니다.

 

 

4. 2개의 Executors와 2개의 Node 구현해보기

1. cpp 파일 만들기

executor_example_4.cpp

#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <unistd.h>

using namespace std::chrono_literals;

class OdomSubscriber : public rclcpp::Node {
public:
  OdomSubscriber(std::string odom_topic_name) : Node("odom_subscriber") {

    subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
        odom_topic_name, 10,
        std::bind(&OdomSubscriber::topic_callback, this,
                  std::placeholders::_1));
  }

private:
  void topic_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Odometry=['%f','%f','%f']",
                msg->pose.pose.position.x, msg->pose.pose.position.y,
                msg->pose.pose.position.z);
  }
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
};

class SlowTimer : public rclcpp::Node {
public:
  SlowTimer(float sleep_timer) : Node("slow_timer_subscriber") {
    this->wait_time = sleep_timer;
    timer_ = this->create_wall_timer(
        500ms, std::bind(&SlowTimer::timer_callback, this));
  }

private:
  void timer_callback() {
    sleep(this->wait_time);
    RCLCPP_INFO(this->get_logger(), "TICK");
  }
  rclcpp::TimerBase::SharedPtr timer_;
  float wait_time;
};

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

  std::shared_ptr<OdomSubscriber> odom_subs_node =
      std::make_shared<OdomSubscriber>("odom");

  float sleep_time = 3.0;
  std::shared_ptr<SlowTimer> slow_timer_node =
      std::make_shared<SlowTimer>(sleep_time);

  RCLCPP_INFO(odom_subs_node->get_logger(), "odom_subs_node INFO...");
  RCLCPP_INFO(slow_timer_node->get_logger(), "slow_timer_node INFO...");

  rclcpp::executors::StaticSingleThreadedExecutor executor;
  rclcpp::executors::StaticSingleThreadedExecutor executor2;
    
  executor.add_node(odom_subs_node);
  executor2.add_node(slow_timer_node);
  executor.spin();
  executor2.spin();

  rclcpp::shutdown();
  return 0;
}

 

 

2. launch 파일 만들고, CMakeLists.txt 수정하고, 빌드하고, 실행해보기

이전과 계속 같은 방식이므로, 코드는 생략하고 결과만 보겠습니다.

 

실행해보면, 아무리 기다려도 Timer Callback이 들어오지 않습니다. 그 이유는 바이너리 파일 1개에는 1개의 executor만 존재할 수 있습니다. 따라서 다수 개의 SingleThreadedExecutors를 사용하고 싶다면, 다른 바이너리 파일을 실행해야 합니다.

 

SingleThreadedExecutors는 MultiThreadedExecutors보다 CPU점유가 낮으므로, 필요에 따라 선택하시면 되겠습니다.

 

 

5. Callback Group

먼저, Callback group없이 그냥 MultiThreadedExecutor를 사용하면 어떻게 되는지 확인해보겠습니다.

 

1. cpp 파일을 만듭니다.

executor_example_5.cpp

#include <rclcpp/rclcpp.hpp>
#include <unistd.h>

using namespace std::chrono_literals;

class TwoTimer : public rclcpp::Node {
public:
  TwoTimer(float sleep_timer1, float sleep_timer2)
      : Node("slow_timer_subscriber") {

    this->wait_time1 = sleep_timer1;
    this->wait_time2 = sleep_timer2;

    timer1_ = this->create_wall_timer(
        500ms, std::bind(&TwoTimer::timer1_callback, this));
    timer2_ = this->create_wall_timer(
        500ms, std::bind(&TwoTimer::timer2_callback, this));
  }

private:
  void timer1_callback() {
    sleep(this->wait_time1);
    RCLCPP_INFO(this->get_logger(), "TIMER 1 CALLBACK");
  }

  void timer2_callback() {
    sleep(this->wait_time2);
    RCLCPP_INFO(this->get_logger(), "TIMER 2 CALLBACK");
  }

  rclcpp::TimerBase::SharedPtr timer1_;
  rclcpp::TimerBase::SharedPtr timer2_;
  float wait_time1;
  float wait_time2;
};

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

  float sleep_time1 = 1.0;
  float sleep_time2 = 3.0;
  std::shared_ptr<TwoTimer> two_timer_node =
      std::make_shared<TwoTimer>(sleep_time1, sleep_time2);

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(two_timer_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

 

2. launch 파일 만들고, CMakeLists.txt 수정하고, 빌드하고, 실행해보기

이전과 계속 같은 방식이므로, 코드는 생략하고 결과만 보겠습니다.

결과를 보면 Timer Callback이 1개만 들어오고 있는데, 이는 Multi-Threaded Excutor가 노드 당 1개의 Thread만 만들었기 때문입니다.

 

 

6. Reentrant Callback Groups

기본적인 내용은 python에서의 내용과 같습니다.

 

1. cpp 코드 작성

executor_example_6.cpp

#include <rclcpp/rclcpp.hpp>
#include <unistd.h>

using namespace std::chrono_literals;

class TwoTimer : public rclcpp::Node {
public:
  TwoTimer(float sleep_timer1, float sleep_timer2)
      : Node("slow_timer_subscriber") {
          
    callback_group_ =
        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);

    this->wait_time1 = sleep_timer1;
    this->wait_time2 = sleep_timer2;

    timer1_ = this->create_wall_timer(
        500ms, std::bind(&TwoTimer::timer1_callback, this), callback_group_);
    timer2_ = this->create_wall_timer(
        500ms, std::bind(&TwoTimer::timer2_callback, this), callback_group_);
  }

private:
  void timer1_callback() {
    RCLCPP_INFO(this->get_logger(), "Timer 1 Callback Start");
    sleep(this->wait_time1);
    RCLCPP_INFO(this->get_logger(), "Timer 1 Callback End");
  }

  void timer2_callback() {
    RCLCPP_INFO(this->get_logger(), "Timer 2 Callback Start");
    sleep(this->wait_time2);
    RCLCPP_INFO(this->get_logger(), "Timer 2 Callback End");
  }

  rclcpp::CallbackGroup::SharedPtr callback_group_;
  rclcpp::TimerBase::SharedPtr timer1_;
  rclcpp::TimerBase::SharedPtr timer2_;
  float wait_time1;
  float wait_time2;
};

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

  float sleep_time1 = 1.0;
  float sleep_time2 = 3.0;
  std::shared_ptr<TwoTimer> two_timer_node =
      std::make_shared<TwoTimer>(sleep_time1, sleep_time2);

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(two_timer_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

 

2. launch 파일 만들고, CMakeLists.txt 수정하고, 빌드하고, 실행해보기

이전과 계속 같은 방식이므로, 코드는 생략하고 결과만 보겠습니다.

 

7. 하나의 MutuallyExclusive Callback group에 2개의 콜백 넣기

1. 코드 작성

executor_example_7.cpp

#include <rclcpp/rclcpp.hpp>
#include <unistd.h>

using namespace std::chrono_literals;

class TwoTimer : public rclcpp::Node {
public:
  TwoTimer(float sleep_timer1, float sleep_timer2)
      : Node("slow_timer_subscriber") {

    callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

    this->wait_time1 = sleep_timer1;
    this->wait_time2 = sleep_timer2;

    timer1_ = this->create_wall_timer(
        500ms, std::bind(&TwoTimer::timer1_callback, this), callback_group_);
    timer2_ = this->create_wall_timer(
        500ms, std::bind(&TwoTimer::timer2_callback, this), callback_group_);
  }

private:
  void timer1_callback() {
    RCLCPP_INFO(this->get_logger(), "Timer 1 Callback Start");
    sleep(this->wait_time1);
    RCLCPP_INFO(this->get_logger(), "Timer 1 Callback End");
  }

  void timer2_callback() {
    RCLCPP_INFO(this->get_logger(), "Timer 2 Callback Start");
    sleep(this->wait_time2);
    RCLCPP_INFO(this->get_logger(), "Timer 2 Callback End");
  }

  rclcpp::CallbackGroup::SharedPtr callback_group_;
  rclcpp::TimerBase::SharedPtr timer1_;
  rclcpp::TimerBase::SharedPtr timer2_;
  float wait_time1;
  float wait_time2;
};

int main(int argc, char *argv[]) {

  rclcpp::init(argc, argv);

  float sleep_time1 = 1.0;
  float sleep_time2 = 3.0;
  std::shared_ptr<TwoTimer> two_timer_node =
      std::make_shared<TwoTimer>(sleep_time1, sleep_time2);

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(two_timer_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

 

2. 결과 확인

 

 

8. 2개의 MutuallyExclusive Callback Groups 사용해보기

1. 코드 작성

executor_example_8.cpp

#include <rclcpp/rclcpp.hpp>
#include <unistd.h>

using namespace std::chrono_literals;

class TwoTimer : public rclcpp::Node {
public:
  TwoTimer(float sleep_timer1, float sleep_timer2)
      : Node("slow_timer_subscriber") {

    callback_group_1 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_2 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

    this->wait_time1 = sleep_timer1;
    this->wait_time2 = sleep_timer2;

    timer1_ = this->create_wall_timer(
        500ms, std::bind(&TwoTimer::timer1_callback, this), callback_group_1);
    timer2_ = this->create_wall_timer(
        500ms, std::bind(&TwoTimer::timer2_callback, this), callback_group_2);
  }

private:
  void timer1_callback() {
    RCLCPP_INFO(this->get_logger(), "Timer 1 Callback Start");
    sleep(this->wait_time1);
    RCLCPP_INFO(this->get_logger(), "Timer 1 Callback End");
  }

  void timer2_callback() {
    RCLCPP_INFO(this->get_logger(), "Timer 2 Callback Start");
    sleep(this->wait_time2);
    RCLCPP_INFO(this->get_logger(), "Timer 2 Callback End");
  }

  rclcpp::CallbackGroup::SharedPtr callback_group_1;
  rclcpp::CallbackGroup::SharedPtr callback_group_2;
  rclcpp::TimerBase::SharedPtr timer1_;
  rclcpp::TimerBase::SharedPtr timer2_;
  float wait_time1;
  float wait_time2;
};

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

  float sleep_time1 = 1.0;
  float sleep_time2 = 3.0;
  std::shared_ptr<TwoTimer> two_timer_node =
      std::make_shared<TwoTimer>(sleep_time1, sleep_time2);

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(two_timer_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

 

 

2. 결과 확인