Choreonoid_ros利用時のコントローラ起動までの待機について

Japanese (日本語)
1

大学での研究にてChoreonoid2.2.0を使わせて頂いており、Choreonoid_ros利用時のシミュレーションの待機についてお伺いしたく投稿いたします。
現在Choreonoid_rosを利用してROS上で自作のコントローラを用いたヒューマノイドロボットのシミュレーションを行うため、『ROSとの連携』を参考にして下記の環境でシミュレーションを行っています。
<<使用環境>>
・Ubuntu20.04
・ROS noetic
Choreonoid_rosは現状私が確認できているシミュレーションでは、Choreonoidのシミュレーション開始後にros_controllersのコントローラがloadされるため数 msほどコントローラが入らない状態で動作いたします。そのためコントローラの起動が間に合わず指令値が送られないためヒューマノイドロボットがコントローラの起動前に重力によって崩れ落ちる現象が起こっております．現状下記のようにWorldROSItem.cppにて４ ms分getchar()関数を用いてコントローラ起動まで時間が経過するよう変更を加えています。

#define gc std::cout << std::endl << "Please hit any key to continue !!" << std::endl; getchar()

void WorldROSItem::Impl::onSimulationStarted()
{
    clockPublishingInterval = 1.0 / maxClockPublishingRate;
    clockPublishingInterval = 1.0 / 1000;
    nextClockPublishingTime = 0.0;

    currentSimulatorItem->addMidDynamicsFunction(
        [&](){ 
            if( a < 4){
              gc;
              a++;
            }//add
            onSimulationStep(); });
}

下記に示すGazebo Classicにあるpausedオプションのようなシミュレーションを初期状態で一時停止させる機能などはChoreonoid_rosにございますでしょうか？
<arg name="paused" value="true"/>
ご回答いただけますと幸いです。よろしくお願いいたします。

2

choreonoid_ros 開発者の一人の大西です．ご返事遅くなりすみませんでした．

本件，以下の二つの方法が考えられますので，お試しいただけますでしょうか？

  1. choreonoid_ros の ROSControlItem.cpp のstart関数でsleepを行う．
  2. 上記と同じ関数で，sleepの代わりに初期状態から制御入力を計算して適用する（下記参照，打ち間違いがあるかもですが……）．
bool ROSControlItem::start()
{
    int now_sec = static_cast<int>(io->currentTime());
    double now_nsec = (io->currentTime() - now_sec) * 1e9;
    ros::Time now(now_sec, static_cast<int>(now_nsec));

    ros::Duration period(io->timeStep());

    robotHWSim->read(now, period);
    controllerManager->update(now, period, false);
}

ご参考までにお伺いしたいことがあります．

  • ロボットの関節はトルク制御でしょうか？
  • どのくらいの重さ・身長のロボットを使われていますか？

よろしくお願いします．

3

ご返答頂きありがとうございます。
ご提案いただいた1つ目のROSControlItem.cppのstart関数にsleepを入れる方式で制御前にコントローラを止めることができました．sleepさせることにより0.000000 sにてコントローラが起動し，次の制御に行くまでに間に合わせることができました．
ご提案いただいた２つ目の先に制御指令値を入れる方法に関しましてはreturn true;でstart関数から出ることで記載いただいたプログラムを使用いたしました．しかしコントローラの起動前に制御指令値を入れることはできたもののコントローラの起動前に次の制御が回ったためロボットが崩れ落ちる現象については変化はありませんでした．
これから使用させていただくにあたりstart関数の中にsleepを入れる方法で実行したいと考えております．

使用しているヒューマノイドロボットは各関節をトルク制御するもので，42 ㎏,137.5 cmのロボットになります．

お答えいただきありがとうございました。