見出し画像

Self Driving and ROS 2 - Learn by Doing! Odometry & Control: カルマンフィルタ (セクション12-3/13)

Hafnium

  • ROS 2ライフサイクルノードは、ノードの状態と遷移を管理し、リソース管理とデバッグを改善するための強力なツールです。

  • C++でライフサイクルノードを作成し、コマンドラインインターフェース（CLI）を使用してノードの状態を取得・制御する方法を学びます。

  • ハードウェア制御インターフェースとライフサイクルノードの統合により、モーターのコマンドとフィードバックのシリアル通信を管理します。

今回のブログでは、Self Driving and ROS 2 - Learn by Doing! Odometry & Controlコースのセクション12の第3部（講義151〜154）を紹介します。これらの講義は、ROS 2のライフサイクルノードに焦点を当て、ROS 2ノードの状態と遷移を管理するための強力なメカニズムを提供します。ライフサイクルノードは、ノードの動作をより細かく制御できるため、ロボットコンポーネントの管理を改善し、さまざまな動作状態間のスムーズな遷移を確保します。

講義151: ROS 2 ライフサイクルノードの紹介

この講義では、ROS 2ライフサイクルノードの概念と、従来のノードに対する利点について説明します。ライフサイクルノードは、いくつかの状態と遷移を提供し、ノードの動作を細かく制御できます。

主な状態と遷移：

  1. 未構成状態（Unconfigured State）: ノードの初期状態で、アクティブな機能はありません。

    • 遷移: `configure`、`shutdown`

  2. 非アクティブ状態（Inactive State）: ノードは構成されていますが、主な機能を実行していません。

    • 遷移: `cleanup`、`shutdown`、`activate`

  3. アクティブ状態（Active State）: ノードは完全に動作しており、主なロジックを実行しています。

    • 遷移: `deactivate`、`shutdown`

  4. 終了状態（Finalized State）: ノードはシャットダウン中で、破壊の準備をしています。

    • 遷移: `destroy`

ライフサイクルノードを使用すると、リソース管理が向上し、デバッグが改善され、状態遷移中の動作がより予測可能になります。

講義152: C++でライフサイクルノードを作成する

この講義では、C++でシンプルなライフサイクルノードを作成する方法を紹介します。このノードは、トピックを購読し、その状態に応じてメッセージを処理します。

コード: C++でのシンプルなライフサイクルノード

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <std_msgs/msg/string.hpp>
#include <chrono>
#include <memory>
#include <string>
#include <thread>

using namespace std::chrono_literals;
using std::placeholders::_1;

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

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_configure(const rclcpp_lifecycle::State &) override {
    sub_ = create_subscription<std_msgs::msg::String>("chatter", 10, std::bind(&SimpleLifecycleNode::msgCallback, this, _1));
    RCLCPP_INFO(get_logger(), "Lifecycle node on_configure() called.");
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_activate(const rclcpp_lifecycle::State & state) override {
    LifecycleNode::on_activate(state);
    RCLCPP_INFO(get_logger(), "Lifecycle node on_activate() 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 & state) override {
    LifecycleNode::on_deactivate(state);
    RCLCPP_INFO(get_logger(), "Lifecycle node on_deactivate() called.");
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_cleanup(const rclcpp_lifecycle::State &) override {
    sub_.reset();
    RCLCPP_INFO(get_logger(), "Lifecycle node on_cleanup() called.");
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_shutdown(const rclcpp_lifecycle::State &) override {
    sub_.reset();
    RCLCPP_INFO(get_logger(), "Lifecycle node on_shutdown() called");
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

void msgCallback(const std_msgs::msg::String &msg) {
    auto state = get_current_state();
    if(state.label() == "active") {
      RCLCPP_INFO_STREAM(get_logger(), "Lifecycle node heard: " << msg.data.c_str());
    }
}

private:
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
};

int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor ste;
std::shared_ptr<SimpleLifecycleNode> simple_lifecycle_node = std::make_shared<SimpleLifecycleNode>("simple_lifecycle_node");
ste.add_node(simple_lifecycle_node->get_node_base_interface());
ste.spin();
rclcpp::shutdown();
return 0;
}

このスクリプトは、さまざまな状態間の遷移とメッセージの処理を行うライフサイクルノードを設定します。

CMakeLists.txtとpackage.xmlの更新

この新しいノードを統合するために、`CMakeLists.txt`と`package.xml`ファイルを更新し、依存関係とビルド手順を追加します。

講義153: ROS 2 ライフサイクルCLIの使用

この講義では、ROS 2のコマンドラインインターフェース（CLI）を使用してライフサイクルノードと対話する方法を示します。

CLIコマンド：

  • 現在の状態を取得：

ros2 lifecycle get /simple_lifecycle_node

  • 利用可能な遷移をリスト：

ros2 lifecycle list /simple_lifecycle_node

  • 遷移をトリガー：

ros2 lifecycle set /simple_lifecycle_node <transition_name_or_id>

例：

ros2 lifecycle set /simple_lifecycle_node configure
ros2 lifecycle set /simple_lifecycle_node activate
ros2 lifecycle set /simple_lifecycle_node deactivate
ros2 lifecycle set /simple_lifecycle_node shutdown

講義154: ROS 2制御インターフェースの実装

この講義では、ハードウェア制御インターフェースとライフサイクルノードを統合する方法に焦点を当てます。`BumperbotInterface`クラスが紹介され、ハードウェアとのやり取りを管理します。

コード: Bumperbotインターフェースヘッダー

#ifndef BUMPERBOT_INTERFACE_HPP
#define BUMPERBOT_INTERFACE_HPP

#include <rclcpp/rclcpp.hpp>
#include <hardware_interface/system_interface.hpp>
#include <libserial/SerialPort.h>
#include <rclcpp_lifecycle/state.hpp>
#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
#include <vector>
#include <string>

namespace bumperbot_firmware {

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

class BumperbotInterface : public hardware_interface::SystemInterface {
public:
BumperbotInterface();
virtual ~BumperbotInterface();

virtual CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;

virtual CallbackReturn on_init(const hardware_interface::HardwareInfo &hardware_info) override;
virtual std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
virtual std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
virtual hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override;
virtual hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;

private:
LibSerial::SerialPort arduino_;
std::string

 port_;
std::vector<double> velocity_commands_;
std::vector<double> position_states_;
std::vector<double> velocity_states_;
};

}  // namespace bumperbot_firmware

#endif  // BUMPERBOT_INTERFACE_HPP

このインターフェースは、Arduinoとの通信を管理し、モーターのコマンドとフィードバックをシリアルポートを通じて処理します。

結論

講義151から154では、ROS 2ライフサイクルノードの包括的な紹介とC++での実装を提供します。ライフサイクルノードをロボティクスプロジェクトに統合することで、制御と柔軟性が向上し、システムがスムーズかつ効率的に動作するようになります。自律走行ロボットの構築に向けて、引き続き役立つ情報を提供していきますので、ご期待ください！

「超本当にドラゴン」へ

この記事が気に入ったらサポートをしてみませんか？