hotch-potch, Note to self

いろいろ作業記録

ROS2・PlanSys2 チュートリアルチャレンジ（４）

ROS

１．はじめに

回数 シリーズ「ROS2・PlanSys2 チュートリアルチャレンジ」・概要
1 PlanSys2のインストールと基本操作
2 プランニング
2.1 プランの調製
3 rvizで視覚化
4 （今回）Behavior Trees
番外編 PDDL入門

前回記事 に引き続き、 ROS2のPlanSys2の試行と、それに関連する気づきをまとめました。

ROS2 Planning System — ROS2 Planning System 2 1.0.0 documentation

今回も、ドキュメントを翻訳しながら、 チュートリアル４番目を進めていきます。

■注意■：このチュートリアルが書かれたのは2020年ごろで、2024年現在のhumble版とは、サンプルプログラムの内容が変わっていました。そのため、チュートリアルの手順通りにサンプルプログラムを実行しても、期待通りの動作はしない模様です。

２．Implement actions as Behavior Trees

４つ目のチュートリアルを進めていきます。

Implement actions as Behavior Trees — ROS2 Planning System 2 1.0.0 documentation

ros2_planning_system_examples/plansys2_bt_example at humble · PlanSys2/ros2_planning_system_examples · GitHub

概要

Behavior Trees (BT) は、アクションを実装する方法です。 （Behavior Treesとは）

アクションを実装する代わりに、動作ツリーの構造を含む XML ファイルを指定し、ツリー ノードを実装します。 これにより、アクションの実装を容易に変更できるうえ、さまざまなBehavior Trees で再利用し易くなります。

PlanSys2 では、Davide Facontiさんの書かれたコード BehaviorTree.CPP パッケージを使用します。（こちらにもチュートリアルがありますので、発展的な活用の参考になります）

このチュートリアルでは、自動車工場での組立作業を例としています。 この工場のロボットは、自動車を組み立てるために、自動車の部品を保管エリアから組立エリアまで搬送する必要があります。

https://plansys2.github.io/_images/carfactory.png

このチュートリアルの PDDL ドメインはこのファイルにあります。

（０）Requisites

前回までのチュートリアルチャレンジで、ソースコードのクローンとビルドまで終わっていますので、その続きから始めます。

# 環境変数読み込み
$ source /opt/ros/humble/setup.bash

# 作業ディレクトリに移動
$ cd ~/gitwork/ps2_tuto2

（１）Running the example

新しいターミナルを開いて PlanSys2 を実行します。

$ source ./tcommon.sh 
$ ros2 launch plansys2_bt_example plansys2_bt_example_launch.py

この時のperformerの状態は下記のとおりです。

オプションは 2 つあります。 シミュレーションで実際の Nav2 パッケージを使用するか、 Nav2 の偽ノードを使用することができます。

$ cd ~/gitwork/ps2_tuto2/
$ source ./tcommon.sh 

#①実際のNav2およびシミュレーション用
$ ros2 launch nav2_bringup tb3_simulation_launch.py
# (rvizが立ち上がります)

# ②シミュレータのNav2ノードの場合
$ ros2 run plansys2_bt_example nav2_sim_node
[INFO] [1715234741.521595104] [navigate_to_pose_server]: Ready.
# （このままターミナルで待機状態になります）

PlanSys2 ターミナル シェルで次のコマンドをコピーして貼り付け、システムの知識を初期化し、目標を設定します。

ターミナルに１行ずつコピペは大変なので、内容をいったんcommands.listに書き出し、sourceで読ませます。

$ cd ~/gitwork/ps2_tuto2/
$ ./tpterm.sh 
[INFO] [1715235414.192234079] [terminal]: No problem file specified.
ROS2 Planning System console. Type "quit" to finish
> source commands.list

この時のKnowledgeの状態は下記のとおりです。

プランを実行します。

> get plan
plan: 
0:      (move r2d2 wheels_zone recharge_zone)   [5]
5.001:  (recharge r2d2 recharge_zone)   [5]
10.002: (move r2d2 recharge_zone assembly_zone) [5]
15.003: (move r2d2 assembly_zone body_car_zone) [5]
20.004: (transport r2d2 body_car_1 body_car_zone assembly_zone) [5]
25.005: (move r2d2 assembly_zone steering_wheels_zone)  [5]
30.006: (transport r2d2 steering_wheel_1 steering_wheels_zone assembly_zone)    [5]
35.007: (move r2d2 assembly_zone wheels_zone)   [5]
40.008: (transport r2d2 wheel_1 wheels_zone assembly_zone)      [5]
45.009: (assemble r2d2 assembly_zone wheel_1 body_car_1 steering_wheel_1 car_1) [5]

> run
[(move r2d2 wheels_zone recharge_zone) 0%]

ここではこれ以上動かず、ターミナル１の状況

…
[bt_action_node-4] [INFO] [1715236445.809274028] [move_3_bb_node]: "move" BtActionNode initialized
[bt_action_node-4] [ERROR] [1715236445.809662110] [move_3]: Caught exception in callback for transition 13
[bt_action_node-4] [ERROR] [1715236445.809697808] [move_3]: Original error: Statically typed parameter 'waypoints' must be initialized.
[bt_action_node-4] [WARN] [1715236445.809721544] [move_3]: Error occurred while doing error handling.

（２）Using Behavior Trees

PDDL ドメインには、assemble, move, transportという 3 つのアクションがあります。

前のチュートリアルで行ったように、BT を使用せずにアセンブル アクションを実装します。 他の 2 つの PDDL アクション、moveとtransportを BT を使用して実装します。

このため、各アクションには XML ファイルにエンコードされた BT が含まれます。 この XML ファイルには、制御構造 (シーケンス、フォールバックなど) とアクションを実行するノードが含まれています。

このチュートリアルでは、PDDL アクションの BT に含めることができる 4 つの BT ノードが実装されています。

  • Move
  • ApproachObject
  • OpenGripper
  • CloseGripper

PlanSys2 には、あらゆる BT が実行できる汎用の実行可能ファイルがあります。 この実行可能ファイルは、plansys2_bt_actionsパッケージの bt_action_node です。

これを使用するには、BT と PDDL アクション名を含む XML ファイルをパラメーターとして指定して、launchファイルに追加する必要があります。

（書き方の例）

･･･
    move_1_cmd = Node(
        package='plansys2_bt_actions',
        executable='bt_action_node',
        name='move_1',
        namespace=namespace,
        output='screen',
        parameters=[
          example_dir + '/config/params.yaml',
          {
            'action_name': 'move',
            'publisher_port': 1668,
            'server_port': 1669,
            'bt_xml_file': example_dir + '/behavior_trees_xml/move.xml'
          }
        ])
･･･

params.yaml には、BT で使用される BT ノードが含まれています。 各 BT をプラグインとして実装するため、使用する各カスタム ノードを指定する必要があります。 特定の BT ノードに必要なパラメータを含めることもできます。 この場合、各部屋の座標はパラメータを使用して BT ノードのmoveに指定されます。

（書き方の例）

move_1:
  ros__parameters:
    plugins:
      - plansys2_move_bt_node
    waypoints: ["wheels_zone", "steering_wheels_zone", "body_car_zone", "assembly_zone", "recharge_zone"]
    waypoint_coords:
      wheels_zone: [0.0, -2.0, 0.0]
      steering_wheels_zone: [1.8, 0.0, 0.0]
      body_car_zone: [0.0, 2.0, 0.0]
      assembly_zone: [-2.0, -0.4, 0.0]
      recharge_zone: [-2.0, -0.4, 0.0]
･･･

2.1 PDDL Action move

moveアクションのBTです。 シーケンスは必須ではありませんが、利用可能なコードと一貫性があるようにチュートリアルで管理されています。 1 つの BT ノードのMoveのみで構成されます。 (PDDL アクションのmoveと混同しないでください)

<root BTCPP_format="4" main_tree_to_execute = "MainTree" >
    <BehaviorTree ID="MainTree">
       <Sequence name="root_sequence">
           <Move    name="move" goal="{arg2}" />
       </Sequence>
    </BehaviorTree>
</root>

PlanSys2において、PDDLアクションの引数は、Blackboard内のarg0, arg1, arg2...などの識別子を通じてXMLでアクセス可能です。（ここでいうBlackboardは、異なるモジュール間で情報を共有するためのデータ構造やメモリスペースのことを指します）

PDDL アクション(move r2d2 corridor kitchen)を実行する場合、arg2 は次に決められた行き先である kitchenとなります。

2.2 PDDL Action transport

`transport`アクションのBTです。これは、BT ノードの Move の再利用を含め、一連の BT ノードとして実装されます。

<root BTCPP_format="4" main_tree_to_execute = "MainTree" >
    <BehaviorTree ID="MainTree">
       <Sequence name="root_sequence">
           <OpenGripper    name="open_gripper"/>
           <ApproachObject name="approach_object"/>
           <CloseGripper   name="close_gripper"/>
           <Move    name="move" goal="{arg3}"/>
           <OpenGripper    name="open_gripper"/>
       </Sequence>
    </BehaviorTree>
</root>

2.3 BT Nodes

このチュートリアルでは 4 つの BT ノードを実装しました。 ApproachObject、OpenGripper、および CloseGripper にも同様の実装があり、実行時にターミナルにメッセージのみが表示されます。

ros2_planning_system_examples/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/ApproachObject.hpp at rolling · PlanSys2/ros2_planning_system_examples · GitHub

namespace plansys2_bt_example
{

class ApproachObject : public BT::ActionNodeBase
{
public:
  explicit ApproachObject(
    const std::string & xml_tag_name,
    const BT::NodeConfiguration & conf);

  void halt();
  BT::NodeStatus tick();

  static BT::PortsList providedPorts()
  {
    return BT::PortsList({});
  }

private:
  int counter_;
};

}  // namespace plansys2_bt_example

ros2_planning_system_examples/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp at rolling · PlanSys2/ros2_planning_system_examples · GitHub

namespace plansys2_bt_example
{

ApproachObject::ApproachObject(
  const std::string & xml_tag_name,
  const BT::NodeConfiguration & conf)
: BT::ActionNodeBase(xml_tag_name, conf), counter_(0)
{
}

void
ApproachObject::halt()
{
  std::cout << "ApproachObject halt" << std::endl;
}

BT::NodeStatus
ApproachObject::tick()
{
  std::cout << "ApproachObject tick " << counter_ << std::endl;

  if (counter_++ < 5) {
    return BT::NodeStatus::RUNNING;
  } else {
    counter_ = 0;
    return BT::NodeStatus::SUCCESS;
  }
}

}  // namespace plansys2_bt_example

#include "behaviortree_cpp/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<plansys2_bt_example::ApproachObject>("ApproachObject");
}

BT ノードの Move の実装では、ノードが ROS2 アクションを呼び出す際の実装を簡素化するために、BtActionNode<> を継承するようにしています。

この場合、nav2_msgs::action::NavigateToPose。

namespace plansys2_bt_tests
{

class Move : public plansys2::BtActionNode<
    nav2_msgs::action::NavigateToPose>
{
public:
  explicit Move(
    const std::string & xml_tag_name,
    const std::string & action_name,
    const BT::NodeConfiguration & conf);

  BT::NodeStatus on_tick() override;
  BT::NodeStatus on_success() override;

  static BT::PortsList providedPorts()
  {
    return {
      BT::InputPort<std::string>("goal")
    };
  }

private:
  int goal_reached_;
  std::map<std::string, geometry_msgs::msg::Pose2D> waypoints_;
};

}  // namespace plansys2_bt_tests

BtActionNode<> は複雑さをすべて隠しており、 BT ノードがチェックされるたびに呼び出されるメソッド on_tick を実装することだけが必要です。

この BT ノードでは、入力ゴール パラメーターから宛先 ID を取得します。 前述のXMLのコードの行を思い出してください。

･･･
        <Move name="move" goal="${arg3}"/>
･･･

ros2_planning_system_examples/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp at rolling · PlanSys2/ros2_planning_system_examples · GitHub

･･･
BT::NodeStatus
Move::on_tick()
{
  if (status() == BT::NodeStatus::IDLE) {
    rclcpp_lifecycle::LifecycleNode::SharedPtr node;
    if (!config().blackboard->get("node", node)) {
      RCLCPP_ERROR(node_->get_logger(), "Failed to get 'node' from the blackboard");
    }

    std::string goal;
    getInput<std::string>("goal", goal);

    geometry_msgs::msg::Pose2D pose2nav;
    if (waypoints_.find(goal) != waypoints_.end()) {
      pose2nav = waypoints_[goal];
    } else {
      std::cerr << "No coordinate for waypoint [" << goal << "]" << std::endl;
    }

    geometry_msgs::msg::PoseStamped goal_pos;

    goal_pos.header.frame_id = "map";
    goal_pos.header.stamp = node->now();
    goal_pos.pose.position.x = pose2nav.x;
    goal_pos.pose.position.y = pose2nav.y;
    goal_pos.pose.position.z = 0;
    goal_pos.pose.orientation = tf2::toMsg(tf2::Quaternion({0.0, 0.0, 1.0}, pose2nav.theta));

    goal_.pose = goal_pos;
  }

  return BT::NodeStatus::RUNNING;
}
･･･

必要に応じて実装できるメソッドは他にも以下のようなものがあります。

  • on_success
  • on_aborted
  • on_canceled など