data_collection_serverで低Hzの画像トピックを保存するとブレてしまう

[Kanazawanaoaki]

https://github.com/Kanazawanaoaki/jsk_demos/blob/ebc8fa648b795b90e06ab4366509387dc40445c2/jsk_2020_04_pr2_curry/launch/data_collection_prosilica.launch#L20-L35 でdata_collection_serverを使って低Hzの画像トピックを保存するとサービスを呼んだタイミングよりも大分前の画像が保存されて，ブレてしまいました．

https://gist.github.com/Kanazawanaoaki/f0ba8039274772a4d44877fa8e53c97b

(send *ri* :angle-vector (send *pr2* :angle-vector))
(send *ri* :wait-interpolation)  
(setq res (ros::service-call "after_stow_data_collection/save_request" req t))

のように動作をさせてwait-interpolationを入れた後に画像を保存しても，動作中のぶれている画像が保存されました．

原因としては，data_collection_serverではサービスコールが呼ばれた時点でサブスクライブしている最新のトピックを保存するようになっているので，そもそもrossetmasterをしている遠隔PCから実行しているために遅延が生じていて，さらに低Hzのトピックだとサービスコールを呼んだ時点よりもかなり前のトピックが保存されることが考えられます．

解決策としては，

• サービスコールを呼ぶ前に sleepを入れる
• data_collection_serverでサービスコールを受け取った時点よりもtimestampが後のトピックを保存するようにする．（オプションを作る）

が考えられます．

(send *ri* :angle-vector (send *pr2* :angle-vector))
(send *ri* :wait-interpolation)

(unix:sleep 1)

(setq res (ros::service-call "after_stow_data_collection/save_request" req t))

のようにしてぶれていない画像が保存できることは確認できました．

[knorth55]

https://github.com/jsk-ros-pkg/jsk_common/blob/master/jsk_data/node_scripts/data_collection_server.py#L291-L296

ここでself.msgsのtimestampとservice callのtimestampを比較して，self.msgsのtimestampが更新されるまで待つようにすると解決できそうですね．

[knorth55]

@Kanazawanaoaki このPRを試してもらえますか？ https://github.com/jsk-ros-pkg/jsk_common/pull/1669

[Kanazawanaoaki]

@knorth55 そのままでは動かなかったのでhttps://github.com/knorth55/jsk_common/pull/2 のようにとりあえずエラーを起きないようにしました．
実機で試してみても画像がブレずに上手くいっていそうです．