[WIP] [hrpsys_ros_bridge] Start joint trajectory at assigned timing

[pazeshun]

Depends on #1049
Fixes #1048

[pazeshun]

Execution flow:

1. Check time stamp of joint trajectory in its callback
2. When time stamp is in future, just remember time stamp and trajectory in member variables without executing trajectory. Otherwise, just execute onJointTrajectory function as before
3. Check whether remembered time stamp is in present or past in execution loop (onExecute)
4. When remembered time stamp is in present or past, execute trajectory by calling onJointTrajectory from execution loop

[pazeshun]

Currently, feedback from joint trajectory starts directly after getting goal, and stops when trajectory is completed.
time_from_start of feedback (e.g. /fullbody_controller/follow_joint_trajectory_action/feedback/feedback/error/time_from_start) is calculated based on trajectory start time.
So when trajectory is in future, time_from_start starts from minus value.

[pazeshun]

I want to follow common specification of feedback, but specification is not uniformed...

• PR2
  • Feedback starts directly after getting goal
  • time_from_start of feedback always starts from zero. So time_from_start of feedback may be different from time_from_start of trajectory point currently executed. (I feel this is strange)
• Baxter
  • Feedback starts when trajectory starts
  • time_from_start of feedback starts from zero
• Fetch
  • Feedback starts directly after getting goal
  • time_from_start of feedback is always zero (I feel this is strange)
• joint_trajectory_controller in ros_controllers
  • In indigo, feedback never comes
  • In kinetic, feedback seems to start directly after getting goal, but time_from_start seems zero (joint_trajectory_controller does not publish feedback via actionlib ros-controls/ros_controllers#173, Write feedback for the RealtimeServerGoalHandle to publish -- rebased ros-controls/ros_controllers#219)

＊ In all cases, joint trajectory is started on its time stamp, which is implemented in this PR.

[pazeshun]

NOTE: After this PR, time synchronization between PC bringing up ros bridge and PC publishing trajectory goal becomes very important.

[mmurooka]

NOTE: After this PR, time synchronization between PC bringing up ros bridge and PC publishing trajectory goal becomes very important.

これってノートPCから毎日違うロボットを動かそうとするときに，ロボットごとに時計が違ったら困りそうですが，その場合の正しい対処は何でしょうか．

[pazeshun]

これってノートPCから毎日違うロボットを動かそうとするときに，ロボットごとに時計が違ったら困りそうですが，その場合の正しい対処は何でしょうか．

違うロボットを使う場合は、そのロボットの時計に合わせるのがROSのルールになっています。
https://answers.ros.org/question/11570/ros-time-across-machines/
ntp/chronyのサーバをrosmasterと同じにすることで対応できます。
なので厳密には、ロボットを変えるたびにPCの設定を書き換えることになります。

[mmurooka]

なるほど，ちょっと面倒そうだけれど，”ROSのルール”と言われたら仕方ないのか．

[furushchev]

横から失礼します．
参考までに，PR2ではchronyを使って，nictの時計に合わせたPCを1つ用意して，そのPCに対して各ロボットやラップトップなどをすべて合わせるようにしています．
(ラップトップはロボットに合わせている場合もあり)

[pazeshun]

なるほど，ちょっと面倒そうだけれど，”ROSのルール”と言われたら仕方ないのか．

ただ、実際はあまり時間ずれしてなくて、そのまま使えちゃうことの方が多いようにも思います。
現在、時刻が大きくずれていても:angle-vectorが実行できてしまうのは、hrpsys_ros_bridgeを使っているロボットだけかと思います。

参考までに，PR2ではchronyを使って，nictの時計に合わせたPCを1つ用意して，そのPCに対して各ロボットやラップトップなどをすべて合わせるようにしています．
(ラップトップはロボットに合わせている場合もあり)

今までちゃんと考えたことなかったですが、JSKのロボットを全て一つのPCに合わせるようにすれば、そのPCにラップトップを合わせるようにすればよいだけになるので、楽になってよさそうですね。

[YoheiKakiuchi]

http://wiki.ros.org/joint_trajectory_controller では、 stamp に 0 を入れると、受け取り次第動き出すようになっている。
これは、これまでのRosBridgeの動作と同じ動作をする。
時間同期が必要なかったら 0 を入れるという運用もありかもしれない。

[pazeshun]

I found that current implementation doesn't support multiple trajectory waiting.
When new trajectory comes, waiting trajectory should be cancelled, so current implementation may be correct.
However, I didn't come up with that situation and didn't test.
Back to WIP.