LoginSignup
1
1

More than 1 year has passed since last update.

ROS2Advent Calendar 2022Day 17
@naga-karupi(karupi cat)

ROS2をROSっぽく書いてみる

Last updated at Posted at 2022-12-17

ROS2を書きなれていないすべての人類へ

ROSのようにROS2を書く記事を書くべきという神の啓示を得た(tekitou)

なぜこれを書いたのか

  • なんか後輩がROS2にめっちゃ苦戦してるのを見てかわいそうだと思ったから

これだけ

とりあえずpubsub通信だけ書いちゃおう

諸注意

吾輩の環境は
VMWare上で

項目 値
CPU Core i7-1165G7
ubuntu 22.04 LTS
ROS humble
c++ c++17
CMake 3.22.1

まぁどこかしらのバージョンをかえれば動くとは思うけどね

ROSとROS2の実装の比較

細かい解説はどこにでもあるので今回はなしでやります

さてさて、まずは標準的なROS2のsubpub通信から

取り合えず公式の書き方をそのまんま参考にして

コードだよ
publisher.cpp
#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

using namespace std::chrono_literals;

class pub : public rclcpp::Node {
    void timer_callback() {
      auto msg = std_msgs::msg::String();
      msg.data = "Hello, world! " + std::to_string(count_++);
      RCLCPP_INFO(this->get_logger(), "Pub: '%s'", msg.data.c_str());
      publisher_->publish(msg);
    }

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    size_t count_;

public:
    pub()
    : Node("pub"), count_(0) {
      publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
      timer_ = this->create_wall_timer(500ms, std::bind(&pub::timer_callback, this));
    }
};

int main(int argc, char * argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<pub>());
  rclcpp::shutdown();
  return 0;
}
subscriber.cpp
#include <memory>

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

using std::placeholders::_1;

class sub
  : public rclcpp::Node {
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const {
      RCLCPP_INFO(this->get_logger(), "sub: '%s'", msg->data.c_str());
    }

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;

  public:
    sub()
    : Node("sub") {
      subscription_ = this->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&sub::topic_callback, this, _1));
    }
};

int main(int argc, char * argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<sub>());
  rclcpp::shutdown();
  return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(pubsub CXX)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic -O2 -std=c++2a)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

add_executable(pub src/publisher.cpp)
add_executable(sub src/subscriver.cpp)
ament_target_dependencies(pub rclcpp std_msgs)
ament_target_dependencies(sub rclcpp std_msgs)

install(
  TARGETS pub sub
  DESTINATION lib/${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()

参考元: ROS 2 Documentationより

何と言うか私はc系はcとc++とc#しか触ったことないんだけどなんとなくc#に似てるって感じたけどc++っぽさも残ってるっちゃ残ってるって感じた
有識者曰くobjective c++に似てるらしい

どのみちこの書き方はc上がりの後輩には厳しそうなのでまだc/c++なROSっぽいの書き方にしてみよう

一応ROSのpubsub通信も貼っとくね

ROS
publisher.cpp
#include <ros/ros.h>
#include <std_msgs/String.h>

int main(int argc, char** argv) {
  ros::init(argc, argv, "pub");
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<std_msgs::String>("topic", 10);
  ros::Rate rate(10);

  while (ros::ok()) {
    std_msgs::String msg;
    msg.data = "hello world";
    ROS_INFO("pub: %s", msg.data.c_str());
    pub.publish(msg);

    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}
subscriber.cpp
#include <ros/ros.h>
#include <std_msgs/String.h>

void chatterCallback(const std_msgs::String& msg) {
  ROS_INFO("sub: %s", msg.data.c_str());
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "sub");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("topic", 10, chatterCallback);

  ros::spin();
  return 0;
}
CMakeLists.txt
project(pubsub)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(pub src/publisher.cpp)
add_executable(sub src/subscriber.cpp)

target_link_libraries(pub ${catkin_LIBRARIES})
target_link_libraries(sub ${catkin_LIBRARIES})

個人的にROSの方が書きやすい気がする

それはそうとして本題に入ろう

ROSライクなROS2

色々前書きをしても仕方ないので早速、

publisher.cpp
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("pub");
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub = node->create_publisher<std_msgs::msg::String>("topic", 10);
  rclcpp::Rate loop_rate(10);

  auto msg = std_msgs::msg::String();

  while (rclcpp::ok()) {
    msg.data = "hello world";
    RCLCPP_INFO(node->get_logger(), "pub: '%s'", msg.data.c_str());
    pub->publish(msg);
    rclcpp::spin_some(node);
   
    loop_rate.sleep();
  }
  return 0;
}
subscriber.cpp
#include <memory>

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

rclcpp::Node::SharedPtr node;

void sub_callback(const std_msgs::msg::String::SharedPtr msg){
  RCLCPP_INFO(node->get_logger(), "sub: '%s'", msg->data.c_str());
}

int main(int argc, char * argv[]) {
  rclcpp::init(argc, argv);
  node = rclcpp::Node::make_shared("sub");
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub = node->create_subscription<std_msgs::msg::String>("topic", 10, sub_callback);
  rclcpp::spin(node);
  node.reset();
  rclcpp::shutdown();
  return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(pubsub CXX)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic -O2 -std=c++17)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

add_executable(pub src/publisher.cpp)
add_executable(sub src/subscriber.cpp)
ament_target_dependencies(pub rclcpp std_msgs)
ament_target_dependencies(sub rclcpp std_msgs)

install(
  TARGETS pub sub
  DESTINATION lib/${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()

ｷﾓﾁｴｰ
私と同じような環境ならば問題なくコンパイルができ、動くだろう

注意点

subscribe.cppの15行目のnode.reset()を忘れると以下のような実行時エラーが出る

cannot publish data, at ./src/rmw_publish.cpp:62 during '__function__'
[ERROR] [1671301238.203238973] [sub.rclcpp]: Error in destruction of rcl subscription handle: Failed to delete datareader, at ./src/subscription.cpp:52, at ./src/rcl/subscription.c:183
cannot publish data, at ./src/rmw_publish.cpp:62 during '__function__'
Fail in delete datareader, at ./src/rmw_service.cpp:104 during '__function__'
[ros2run]: Segmentation fault

これが出ても問題なく次のコードも動くのだが、セグメンテーション違反はよろしくない。
何が起きているかというと、グローバル変数として宣言したshared_ptrの寿命が尽きていないからで、プログラムが終了してもメモリに残っているということになるらしい。

まとめ

あんまり人が勧めてない書き方するもんじゃないね

一応今回のコードの載ってるgithubのリポジトリも載せておきますね

1
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
1