はじめに

２月はじめ頃から先週まで約２ヶ月間に渡り、ソフトウェア構成の構想と、それに基づいたコードの整理をおこなってきました。

odome.hatenablog.com

それが完了したので、その結果をここにまとめていきます。

なぜソフトウェア構成とコードの整理が必要だったのか

　１番の理由は、メンテナンス性や可読性、拡張性が皆無になってしまっていたことが挙げられます。元々機能をnodeごとに分ける構想は合ったのですが、右往左往しながら機能を継ぎ足していった結果、「動くけど自分にしか理解できないコード」が生まれてしまいました。これではOSSである価値はほとんどなく、今後の開発にも良くないのは明らかです。なのでこの２ヶ月間、拡張性とメンテナンス性を考慮したソフトウェア構成の構想とコードの整理に取り組みました。
　OSSにする以上、ソフトウェアの拡張性やメンテナンス性、コードの可読性は重要視する必要のある要素だとも思いますしね。

今回からの変更点

　移行にあたり、同時に色々と変更点があるので、以下にまとめます。

Simulator: Webots R2022b -> Webots R2023a に変更
OS: Ubuntu 20.04 -> Xubuntu 22.04 に変更（Ubuntuの派生なので、Ubuntu 22.04とほぼ同じ。問題無いはず）
ROS 2 Distribution: ROS 2 Foxy -> ROS 2 Humble に変更

　これに加えて、Humbleに移行したことでwebots_ros2もHumble対応版に移行する必要があります。webots_ros2はfoxyとhumbleで多少仕様が変わってて、Webots関連の型宣言周りが変更されています。なので、Foxyで開発したものをHumbleで動かすことはできないはずです（私はそうでした）。詳しくは私のRepositoryのCommit logを見てもらえれば良いと思います。

ソフトウェア構成の構想と決定

　ソフトウェアの構成を考える上では、以下のことを意識しました。

シンプルさ：シンプルであることで、構成を理解しやすくなる。
node同士の疎な関係：疎であることで、nodeの切り離しが容易になる。
基本的な歩行パターン生成器の構成に準拠：歩行パターンジェネレータ、歩行安定化制御系、ロボット、この３要素で構成する。

　１つの完成形として、使いたい歩行パターン生成法や歩行安定化制御系に応じて、nodeの付け替えだけでそれに対応できるようにしたいと考えています。
　あと、制御ソフトウェアであることから周期とリアルタイム性も重要な要素としてあります。しかし今回はシミュレータ上であることと私の知識不足が著しいことから、決めるのは後回しということにしています。

　以上のことを踏まえて、以下のソフトウェア構成を考案しました。

<ここにソフトウェア構成図。既存のやつのnode名を調整したやつ>
<ここにソフトウェア構成についての説明>

ソフトウェア構成に基づくコードの整理

　以上のソフトウェア構成に基づいて、プログラム全体とコードを整理しました。以下、ソースコードを公開しているGitHubです。

github.com

　また、ディレクトリ・ファイル構造は以下のようになっています。（ソフトウェア構成内に無い無関係なものは除外しています）

.
├── kinematics
│   ├── CMakeLists.txt
│   ├── include
│   │   └── kinematics
│   │       ├── FK.hpp
│   │       └── IK.hpp
│   ├── package.xml
│   └── src
│       ├── FK.cpp
│       ├── FK_main.cpp
│       ├── IK.cpp
│       └── IK_main.cpp
├── msgs_package
│   ├── CMakeLists.txt
│   ├── msg
│   │   └── ToWalkingStabilizationControllerMessage.msg
│   ├── package.xml
│   └── srv
│       ├── ToKinematicsMessage.srv
│       └── ToWebotsRobotHandlerMessage.srv
├── walking_pattern_generator
│   ├── CMakeLists.txt
│   ├── include
│   │   └── walking_pattern_generator
│   │       └── WalkingPatternGenerator.hpp
│   ├── package.xml
│   └── src
│       ├── WalkingPatternGenerator.cpp
│       └── WalkingPatternGenerator_main.cpp
├── walking_stabilization_controller
│   ├── CMakeLists.txt
│   ├── include
│   │   └── walking_stabilization_controller
│   │       └── WalkingStabilizationController.hpp
│   ├── package.xml
│   └── src
│       ├── WalkingStabilizationController.cpp
│       └── WalkingStabilizationController_main.cpp
└── webots_robot_handler
    ├── CMakeLists.txt
    ├── include
    │   └── webots_robot_handler
    │       └── WebotsRobotHandler.hpp
    ├── launch
    │   ├── __pycache__
    │   │   └── robot_launch.cpython-38.pyc
    │   ├── robot_launch.py
    │   └── start_launch.py
    ├── package.xml
    ├── resource
    │   ├── webots_robot_handler
    │   └── webots_robotis_op2_description.urdf
    ├── src
    │   └── WebotsRobotHander.cpp
    ├── webots_robot_handler.xml
    └── worlds
        └── webots_simple_world.wbt

　見てわかるように、コードはC++で記述し、Launchの記述にはPython3を使用しています。

　今のところ通信はプロセス間でやる想定で組んでいますが、いずれプロセス内通信を部分的に適用しようと考えています。
　また、Life Cycle の適用も考えています。Parameterの取得やinit動作があることを考慮すると、採用すればよりキレイな構成になると見込んでいます。まだ構成は白紙ですが、今年中には取り組みたい課題です。

起動方法（Ubuntu系）

　現段階での起動方法（Ubuntu系）をここに示します。あくまで現段階なので、今後変わる可能性が大きいです。
　他の必要Packageは、ROS 2 Humble、WebotsR2023a、webots_ros2(humble)です。それぞれのインストール方法は、リンク先をご参照ください。
1. まず、ROS 2のWorkSpaceを作成します。既存のものがあればそれでもOKです。

mkdir -p ~/ros2_ws/src/
cd ~/ros2_ws/src/

次に、GitHubからRepositoryをCloneしてきます。現在のDefault Branchは開発用のものになっているので、必要に応じてBranchを変更してください。基本的にdevelが最新で、一段落しているVersionがmainという感じです。

git clone https://github.com/open-rdc/ROS2_Walking_Pattern_Generator.git

最後に、buildとsetupをした後にLaunchファイルから起動すればOKです。

cd ~/ros2_ws/
colcon build --symlink-isntall
. install/setup.bash
ros2 launch webots_robot_handler start_launch.py

これで、以下のようにWebotsが起動して、ロボットが動くはずです。（開発途中なので、この通り動かなかったらごめんなさい）

最後に

　こんな感じで、開発環境の移行と、ソフトウェア構成に則ってコードを整理しました。現在の進行状況として、運動学とRobotHandlerはほぼ完了しているので、WalkingPatternGeneratorとWalkingStabilizationControllerに取り組んでいく予定です。その２つが完成すれば、目標達成となるでしょう。