Unity-Robotics-Hubで複数のROSマスターに接続して複数のロボットを動かす

# 概要
Unity-Robotics-HubのPick&Placeチュートリアルは、起動するROSマスターが1つだという前提で組まれていてROSの接続先IPアドレスを増やせないのですが(2022/6/09現在)、
これではロボットを複数に増やしてそれぞれ別のROSで制御したい！という時に困ります。

そこで、Pick&Placeチュートリアルのプログラムを利用しつつ複数のROSマスターに接続する方法を見つけたので共有したいと思います。

# 結論
あらかじめ手動でROS Connectionコンポーネントを追加した上で、
```
m_Ros = ROSConnection.GetOrCreateInstance();
```
を
```
m_Ros = GetComponent<ROSConnection>();
```
 に書き換える。


# 具体例
Pick&PlaceチュートリアルのPart3でロボットを2台に増やして個別のROSで個別にロボットを動かします。

### 図解
ROSコネクションのコンポーネントを複数生成して個別に接続先IPアドレス設定を割り当てます。
そのためのUIが用意されていないので適切にプログラムを書き換える必要があります。
今回はロボット2つ・ROSマスター2つの場合を紹介しますが、３つ以上の場合も数が増えるだけでやり方は同じはずです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589689/a3369aad-0301-8e2b-1535-97a1337d5778.png)


### プログラムを書き換えるところ
UnityのC#スクリプトTrajectoryPlanner.csのStart()関数の始めを以下のように一部編集します。
```C#:TrajectoryPlanner.cs

～～～～～省略～～～～～

    void Start()
    {
        // Get ROS connection static instance
        m_Ros = GetComponent<ROSConnection>();//書き換え。旧コード：m_Ros = ROSConnection.GetOrCreateInstance();
        m_Ros.RegisterRosService<MoverServiceRequest, MoverServiceResponse>(m_RosServiceName);//ここはそのまま

～～～～～省略～～～～～

```
本筋からは逸れるのですがPublishJoints()関数も編集します。
これによってロボット自体の位置を変えても正常に動いてくれます。
もともとロボットの位置を変えても良いように出来ていないのがおかしい気もしますが......

```C#:TrajectoryPlanner.cs

～～～～～省略～～～～～

    public void PublishJoints()
    {
        var request = new MoverServiceRequest();
        request.joints_input = CurrentJointConfig();

        // Pick Pose
        request.pick_pose = new PoseMsg
        {
            //「　- m_NiryoOne.transform.position　」を追加
            position = (m_Target.transform.position + m_PickPoseOffset - m_NiryoOne.transform.position).To<FLU>(),

            // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.
            orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To<FLU>()
        };

        // Place Pose
        request.place_pose = new PoseMsg
        {
            //「　- m_NiryoOne.transform.position　」を追加
            position = (m_TargetPlacement.transform.position + m_PickPoseOffset - m_NiryoOne.transform.position).To<FLU>(),
            orientation = m_PickOrientation.To<FLU>()
        };

        m_Ros.SendServiceMessage<MoverServiceResponse>(m_RosServiceName, request, TrajectoryResponse);
    }

～～～～～省略～～～～～

```

### Unityシーンの変更
ロボット、テーブル、ターゲット、ターゲットプレイスメントをコピペするなりで２組に増やします。
![スクリーンショット (50).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589689/a89e6e10-4bda-83a7-7592-e9e859411183.png)
Hierachyは以下のようになりました。publisherとボタンも２つに増やします。
![スクリーンショット (51).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589689/9e69dae7-7d33-7949-d6a6-244084a6d5fa.png)
publisherAにはInspectorタブからAdd Component→ROS Connection(検索して入力)でROS Connectionコンポーネントを手動で追加します。
また、先ほど少し書き換えたTrajectoryPlannerスクリプト（ここではMultipleROSTrajectoryPlannerという名前で保存)も同様に追加します。
もう片方のpublisherBも同様です。
![スクリーンショット (54).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589689/722a0c48-15cf-ae13-40ba-380bf83fb3f2.png)
PublisherAのROS ConnectionコンポーネントのROS IP Adressの欄に接続したいROSのIPアドレスを入力します。
PublisherBでは２つめの接続したいIPアドレスを入力しましょう。
![スクリーンショット (57).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589689/cf5fe877-5d0d-31d3-c5e2-6024c69e4c61.png)
以上で出来たシーンを実行すると、それぞれのpublishボタンで個別にロボットを動かすことが出来ます。
![スクリーンショット (59).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589689/55714abe-7689-7ed2-89fe-d3bd1e0a61b8.png)
1つのROSマスターで２つのロボットを動かすこともできなくはないですが、実機のロボットでは１台ごとにROSマスターが起動しているという想定が普通だと思うので、ロボットシミュレーションとしてはこの複数のマスターで動かす方法の方がよりリアルかと思われます。