ロボット開発の最前線！「ROS2」で変わる未来のシステム構築

// my_node.hpp -------------------------------------------------------------

// ROS2のライブラリのインクルード
#include "rclcpp/rclcpp.hpp“

// メッセージ型のインクルード
// ROS2標準のものに加え自作のカスタムメッセージ型が使用可能
#include “std_msgs/msg/float32.hpp" // ROS2標準のメッセージ型
#include “my_msgs/msg/route.hpp" // カスタムメッセージ型

// Nodeクラスを継承して独自のノードを定義
class MyNode : public rclcpp::Node
{
public:
MyNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
private:
// サブスクライバーの定義
rclcpp::Subscription<my_msgs::msg::Route>::SharedPtr _sub;
// パブリッシャーの定義
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr _pub;
// コールバック関数の定義
void _callback(const my_msgs::msg::Route::SharedPtr msg);
};

// my_node.cpp -------------------------------------------------------------

#include “my_node.hpp“

// コンストラクタでパブリッシャーとサブスクライバーを設定
// 数行の記述により通信の設定が完了
MyNode::MyNode(const rclcpp::NodeOptions& options):
rclcpp::Node(“my_node", options)
{
// サブスクライバーの設定
_sub = this->create_subscription<my_msgs::msg::Route>(
“target_route",
rclcpp::QoS(10),
std::bind(&myNode::_callback, this, std::placeholders::_1)
);
// パブリッシャーの設定
_pub = this->create_publisher<std_msgs::msg::Float32>(
“target_velocity”,
rclcpp::QoS(10),
);
}

// コールバック関数：ノードの処理の本体となる部分
void MyNode::_callback(const my_msgs::msg::Route::SharedPtr msg)
{
// 走行制御であれば目標速度の計算など、メインの処理を記述
float target_velocity = calculate_target_velocity(msg);
// 計算結果をメッセージに変換
auto pub_msg = std_msgs::msg::Float32();
pub_msg.data = target_velocity;
// メッセージのパブリッシュ
_pub->publish(pub_msg);
}

// main.cpp -------------------------------------------------------------

// main関数：ROS2の初期化とノードの実行
int main(int argc, char * argv[]) {
// ROS2の初期化
rclcpp::init(argc, argv);
// ノードの実行
rclcpp::spin(std::make_shared<MyNode>());
// ROS2の終了処理
rclcpp::shutdown();
return 0;
}