ROS2 with C++ - Topic

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

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

 

 

1. Topic Publisher

ROS1, ROS2 Python과 겹치는 내용들은 모두 생략하고, 바로 코드로 들어가겠습니다.

 

 

1. 패키지를 만들어봅니다. cpp 패키지로 만들면 ROS1에서 봤던 것 같은 디렉토리 및 파일들이 생깁니다.

$ ros2 pkg create topic_publisher_pkg --build-type ament_cmake --dependencies rclcpp std_msgs

 

 

2. src 디렉토리에 cpp 파일을 만듭니다.

simple_topic_publisher.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("simple_publisher");
  auto publisher = node->create_publisher<std_msgs::msg::Int32>("counter", 10);
  auto message = std::make_shared<std_msgs::msg::Int32>();
  message->data = 0;
  rclcpp::WallRate loop_rate(2);

  while (rclcpp::ok()) {
    
    publisher->publish(*message);
    message->data++;
    rclcpp::spin_some(node);
    loop_rate.sleep();
  }
  rclcpp::shutdown();
  return 0;
}

 

ROS1과는 좀 다른 방식으로 코드가 짜여있습니다. make_shared가 도대체 뭔가... 싶을텐데, 스마트 포인터라는 것입니다. 아마 C++ 입문~초급 수준의 책에는 해당 내용이 없을거에요... ROS2에서 cpp로 코드를 짜려면 ROS1보다는 cpp에 대해 좀 더 깊게 알아야 할 내용들이 꽤나 있습니다.

 

3. CMakeLists.txt파일에 아래 내용을 추가합니다.

add_executable(simple_publisher_node src/simple_topic_publisher.cpp)
ament_target_dependencies(simple_publisher_node rclcpp std_msgs)

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

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

 

최종적으로, 다음과 같습니다.

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(topic_publisher_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_msgs REQUIRED)

add_executable(simple_publisher_node src/simple_topic_publisher.cpp)
ament_target_dependencies(simple_publisher_node rclcpp std_msgs)

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

 

 

4. launch 파일을 만듭니다.

from launch import LaunchDescription
from launch_ros.actions import Node

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

 

5. 빌드 후, 실행하고 ros2 topic echo 명령으로 확인해봅니다.

$ ros2 launch topic_publisher_pkg simple_topic_publisher.launch.py

 

 

 

2. Node Composition

이전 예제에서는 정말 간단한, old-school 스타일의 프로그래밍 방법을 사용했는데, 이는 ROS1에서 사용했던 방법과 비슷합니다. ROS2에서는 ROS1과 다르게 "Composition"이라는 개념을 사용하는데, 이로 인해 단일 프로세스에서 여러 개의 노드를 실행할 수 있게 됩니다.

 

node composition을 위해서는 코드를 객체 지향적으로 짜야합니다. 즉, 이전의 코드로는 node composition이 불가능합니다. 이번에는 클래스를 사용하여 node composition하는 예제를 진행해보겠습니다.

 

simple_topic_publisher_composable.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include <chrono>

using namespace std::chrono_literals;

class SimplePublisher : public rclcpp::Node
{
public:
  SimplePublisher()
  : Node("simple_publisher"), count_(0)
  {
    publisher_ = this->create_publisher<std_msgs::msg::Int32>("counter", 10);
    timer_ = this->create_wall_timer(
      500ms, std::bind(&SimplePublisher::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto message = std_msgs::msg::Int32();
    message.data = count_;
    count_++;
    publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr publisher_;
  size_t count_;
};

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

Timer Callback을 사용해서 0.5초마다 count를 1씩 늘리고, 이 값을 publish합니다.

 

 

다음 예제로는 geometry_msgs/msg/Twist 타입의 메시지를 보내서 시뮬레이션 상의 turtlebot을 움직이도록 해보겠습니다.

 

1. move_robot.cpp

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <chrono>

using namespace std::chrono_literals;

class MoveRobot : public rclcpp::Node
{
public:
  MoveRobot()
  : Node("move_robot")
  {
    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
    timer_ = this->create_wall_timer(500ms, std::bind(&MoveRobot::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto message = geometry_msgs::msg::Twist();
    message.linear.x = 0.2;
    message.angular.z = 0.2;
    publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;

};

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

 

2. move_robot.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

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

 

3. CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(topic_publisher_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_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)

add_executable(simple_publisher_node src/simple_topic_publisher.cpp)
ament_target_dependencies(simple_publisher_node rclcpp std_msgs)

add_executable(move_robot_node src/move_robot.cpp)
ament_target_dependencies(move_robot_node rclcpp geometry_msgs)

install(TARGETS
   simple_publisher_node
   move_robot_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()

 

4. 빌드 후, 실행

 

 

3. Topic Subscriber

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

$ ros2 pkg create topic_subscriber_pkg --build-type ament_cmake --dependencies rclcpp std_msgs

 

1. simple_topic_subscriber.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
using std::placeholders::_1;

class SimpleSubscriber : public rclcpp::Node
{
public:
  SimpleSubscriber()
  : Node("simple_subscriber")
  {
    subscription_ = this->create_subscription<std_msgs::msg::Int32>(
      "counter", 10, std::bind(&SimpleSubscriber::topic_callback, this, _1));
  }

private:
  void topic_callback(const std_msgs::msg::Int32::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "I heard: '%d'", msg->data);
  }
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_;
};

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

 

2. simple_topic_subscriber.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

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

 

3. CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(topic_subscriber_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_msgs REQUIRED)

add_executable(simple_subscriber_node src/simple_topic_subscriber.cpp)
ament_target_dependencies(simple_subscriber_node rclcpp std_msgs)

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

 

4. 빌드 후, 실행

$ ros2 launch topic_subscriber_pkg simple_tpoic_subscriber.launch.py

 

 

 

 

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

단, 아래 예시와 같이 CMakeLists.txt에서 의존성 부분을 추가해주세요.