2017年8月14日月曜日

ROSのPublish、Subscribeの遅れの対策

ROSで基本的なPub、Subをしていた時にどうもうまくいかないところがあったのでそれの対策をメモします。重要なところだけを抜き出すと下の様にmsg_pubとmsg_subを作って通信をさせてその遅延を測りました(本当は途中にデータの変換ノードがいくつかあったが)。本来ならいくらかの遅延が発生するだけなのですが、実際には送信受信するデータのずれや遅れが発生しました。
Publish側では、例えばdata0をpublish()しても実際はパブリッシュされず、次のループでdata1をpublish()すると実際はdata0がパブリッシュされるという現象です。
Subscribe側では、受信処理が行われるタイミングが受信してから1秒遅れるという不具合です。$rostopic echo /msg_outで表示されるタイミングよりも遅れて表示が起きます。
ネットを探したりと1時間ほど格闘していたら答えが分かりました。

Publisher側の解決策としては
ros::Publisher msg_pub = n.advertise<std_msgs_Float64>("/msg_out", 1000);

ros::Publisher msg_pub = n.advertise<std_msgs_Float64>("/msg_out", 1000, true);
に変更しました。
どうやらpublishをするとソケット通信をするプロセスが裏で立ち上がって、それが通信をするみたいです。しかし、元のスレッドが短い場合はタイミングの問題なのか1つ分をデータを内部でバッファしてしまうみたいです。ros::Publisherの3つ目のオプションであるlatchをtrueにすることでそれを防げます。

Subscliber側の解決策としては
ros::Rate loop_rate(1);

ros::Rate loop_rate(100);
に変更しました。
これはなかなか分かりにくい問題で、普通は受信処理はトピックが来たら即実行されると思っていたのですが、実はmain関数のfor文でloop_rate.sleep()している間は受信処理が起きず、スリープが解除されたときにトピックが来たら受信処理をするという仕様の様です。そのためにfor文中で何の処理をしていなくてもloop_rateはある程度細くしなければなりません。

==========msg_pub.c==========
#include "ros/ros.h"
#include "std_msgs/Float64.h" 

int main(int argc, char **argv)
{
    ros::init(argc, argv, "msg_pub");
    ros::NodeHandle n;
    ros::Publisher msg_pub = n.advertise<std_msgs_Float64>("/msg_out", 1000);
    ros::Rate loop_rate(1);
    while (ros::ok()){
        std_msgs::Float64 msg=1.0;
        msg_pub.publish(msg);
        //時間を計測
        ros::spinOnce();
        loop_rate.sleep();
    }
}
==========#####==========

==========msg_sub.c==========
#include "ros/ros.h"
#include "std_msgs/Float64.h" 

void msg_callback(const std_msgs::Float64& msg){
        //時間を計測
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "msg_sub");
    ros::NodeHandle n;
    ros::Subscriber msg_sub = n.subscribe("/msg_out", 10, msg_callback);
    ros::Rate loop_rate(1);
    while (ros::ok()){
        ros::spinOnce();
        loop_rate.sleep();
    }
     return 0;
}
==========#####==========

0 件のコメント:

コメントを投稿