Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ros::spin-onceの処理時間が長い #396

Closed
h-kamada opened this issue Nov 10, 2015 · 19 comments · Fixed by #398
Closed

ros::spin-onceの処理時間が長い #396

h-kamada opened this issue Nov 10, 2015 · 19 comments · Fixed by #398

Comments

@h-kamada
Copy link
Contributor

ros::spin-once の処理時間が時間の経過につれて長くなる

時間が経過とともに溜め込んでいるデータが増えるため、徐々にros::spin-onceにかかる時間が長くなる。
特に高負荷な処理を行っている際には、これが溜まると一度のros::spin-onceに2s ~ 4s程度の時間がかかるようになってしまうため、ros::spin-onceで読み込むデータを制限するなど、何らかの対処が必要。

PR2体内

冷蔵庫デモで立ち上げるlaunch(detect_cans_in_fridge_201202/launch/startup.launch)と、
jsk-ros-pkg/jsk_demos#1107 で用いるfridge_demo.launchを立ち上げた状態で
c1 load average 7.16    8.67     9.37
c2 load average 23.96     22.34    21.25

この状態でpr2-interfaceのinstanceを作成、一度ros::spin-onceした後10s後に再びros::spin-onceを行い(実際には(send *ri* :state)を呼んだ)、2度目のros::spin-onceにかかる時間を計測。(8回くらい)
以下に示すようにcontroller actionのデータが支配的。

何もsubscribeしないpr2-interface
0.000484 0.00066 0.000555 0.001829
0.00043 0.000524 0.000489 0.002214
--> 0.0008983s程度

通常のpr2-interface

Subscriptions:
 * /head_traj_controller/follow_joint_trajectory/result [control_msgs/FollowJointTrajectoryActionResult]
 * /l_arm_controller/follow_joint_trajectory/feedback [control_msgs/FollowJointTrajectoryActionFeedback]
 * /tf_static [unknown type]
 * /head_traj_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]
 * /tf [tf2_msgs/TFMessage]
 * /r_arm_controller/follow_joint_trajectory/result [control_msgs/FollowJointTrajectoryActionResult]
 * /l_gripper_controller/gripper_action/feedback [pr2_controllers_msgs/Pr2GripperCommandActionFeedback]
 * /r_gripper_controller/gripper_action/status [actionlib_msgs/GoalStatusArray]
 * /joint_states [sensor_msgs/JointState]
 * /base_controller/joint_trajectory_action/result [pr2_controllers_msgs/JointTrajectoryActionResult]
 * /pressure/r_gripper_motor [pr2_msgs/PressureState]
 * /r_arm_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]
 * /l_gripper_controller/gripper_action/status [actionlib_msgs/GoalStatusArray]
 * /move_base/feedback [move_base_msgs/MoveBaseActionFeedback]
 * /head_traj_controller/follow_joint_trajectory/status [actionlib_msgs/GoalStatusArray]
 * /r_arm_controller/follow_joint_trajectory/status [actionlib_msgs/GoalStatusArray]
 * /r_gripper_controller/gripper_action/result [pr2_controllers_msgs/Pr2GripperCommandActionResult]
 * /l_arm_controller/follow_joint_trajectory/status [actionlib_msgs/GoalStatusArray]
 * /move_base/status [actionlib_msgs/GoalStatusArray]
 * /r_gripper_controller/gripper_action/feedback [pr2_controllers_msgs/Pr2GripperCommandActionFeedback]
 * /move_base/result [move_base_msgs/MoveBaseActionResult]
 * /head_traj_controller/follow_joint_trajectory/feedback [control_msgs/FollowJointTrajectoryActionFeedback]
 * /torso_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]
 * /l_arm_controller/follow_joint_trajectory/result [control_msgs/FollowJointTrajectoryActionResult]
 * /l_arm_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]
 * /base_controller/joint_trajectory_action/feedback [pr2_controllers_msgs/JointTrajectoryActionFeedback]
 * /torso_controller/follow_joint_trajectory/result [control_msgs/FollowJointTrajectoryActionResult]
 * /base_controller/joint_trajectory_action/status [actionlib_msgs/GoalStatusArray]
 * /torso_controller/follow_joint_trajectory/feedback [control_msgs/FollowJointTrajectoryActionFeedback]
 * /torso_controller/follow_joint_trajectory/status [actionlib_msgs/GoalStatusArray]
 * /r_arm_controller/follow_joint_trajectory/feedback [control_msgs/FollowJointTrajectoryActionFeedback]
 * /pressure/l_gripper_motor [pr2_msgs/PressureState]
 * /base_odometry/odom [nav_msgs/Odometry]
 * /l_gripper_controller/gripper_action/result [pr2_controllers_msgs/Pr2GripperCommandActionResult]

計測時間
0.06082 0.061493 0.07795 0.117355 0.078446 0.103501 0.080939 0.121987
--> 0.087811s程度

rarm, larm, head, torsoのcontroller系のみsubscribeしたpr2-interface

Subscriptions:
* /head_traj_controller/follow_joint_trajectory/result [control_msgs/FollowJointTrajectoryActionResult]
 * /l_arm_controller/follow_joint_trajectory/feedback [control_msgs/FollowJointTrajectoryActionFeedback]
 * /head_traj_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]
 * /r_arm_controller/follow_joint_trajectory/result [control_msgs/FollowJointTrajectoryActionResult]
 * /r_arm_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]
 * /head_traj_controller/follow_joint_trajectory/status [actionlib_msgs/GoalStatusArray]
 * /r_arm_controller/follow_joint_trajectory/status [actionlib_msgs/GoalStatusArray]
 * /l_arm_controller/follow_joint_trajectory/status [actionlib_msgs/GoalStatusArray]
 * /head_traj_controller/follow_joint_trajectory/feedback [control_msgs/FollowJointTrajectoryActionFeedback]
 * /torso_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]
 * /l_arm_controller/follow_joint_trajectory/result [control_msgs/FollowJointTrajectoryActionResult]
 * /l_arm_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]
 * /torso_controller/follow_joint_trajectory/result [control_msgs/FollowJointTrajectoryActionResult]
 * /torso_controller/follow_joint_trajectory/feedback [control_msgs/FollowJointTrajectoryActionFeedback]
 * /torso_controller/follow_joint_trajectory/status [actionlib_msgs/GoalStatusArray]
 * /r_arm_controller/follow_joint_trajectory/feedback [control_msgs/FollowJointTrajectoryActionFeedback]

0.115325 0.053393 0.047287 0.04078 0.038668 0.090261 0.045294 0.054744
--> 0.060719s程度

rgripper, lgripper, movebaseaction, movebasetrajactionのcontrollerのみsubscribeするpr2-interface

Subscriptions:
 * /l_gripper_controller/gripper_action/feedback [pr2_controllers_msgs/Pr2GripperCommandActionFeedback]
 * /r_gripper_controller/gripper_action/status [actionlib_msgs/GoalStatusArray]
 * /base_controller/joint_trajectory_action/result [pr2_controllers_msgs/JointTrajectoryActionResult]
 * /l_gripper_controller/gripper_action/status [actionlib_msgs/GoalStatusArray]
 * /move_base/feedback [move_base_msgs/MoveBaseActionFeedback]
 * /r_gripper_controller/gripper_action/result [pr2_controllers_msgs/Pr2GripperCommandActionResult]
 * /move_base/status [actionlib_msgs/GoalStatusArray]
 * /r_gripper_controller/gripper_action/feedback [pr2_controllers_msgs/Pr2GripperCommandActionFeedback]
 * /move_base/result [move_base_msgs/MoveBaseActionResult]
 * /base_controller/joint_trajectory_action/feedback [pr2_controllers_msgs/JointTrajectoryActionFeedback]
 * /base_controller/joint_trajectory_action/status [actionlib_msgs/GoalStatusArray]
 * /l_gripper_controller/gripper_action/result [pr2_controllers_msgs/Pr2GripperCommandActionResult]

0.032307 0.020242 0.027439
--> 0.026663s程度

joint_states のみのpr2-interface   
0.004268 0.005096 0.011949 0.005918
0.011486 0.004679 0.013978 0.005114
--> 0.007811s程度

rl-gripper-pressureのみのpr2-interface

Subscriptions:
 * /pressure/r_gripper_motor [pr2_msgs/PressureState]
 * /pressure/l_gripper_motor [pr2_msgs/PressureState]

0.004494 0.001197 0.00128
--> 0.002324s程度

/base_odometry/odomのみのpr2-interface
0.001578  0.001852  0.002352 --> 0.001927s程度

tf のみのpr2-interface
0.000964 0.00047 0.000912 0.000416
--> 0.00069s程度

手元PC(参考程度)

roseus/test/talker.lからpub(rate 10)される情報をsubscribeし、同様に処理時間を計測
0.000145 0.00011 3.109300e-05 5.467700e-05
--> 8.519250e-05s程度

@k-okada
Copy link
Member

k-okada commented Nov 10, 2015

/move_base/feedback だけ,あるいは, /move_base/status をsubscribeするノードを自前でつくって

同じ条件で計測するとどうなるかな.

◉ Kei Okada

@k-okada
Copy link
Member

k-okada commented Nov 11, 2015

 * /l_gripper_controller/gripper_action/feedback [pr2_controllers_msgs/Pr2GripperCommandActionFeedback]
 * /r_gripper_controller/gripper_action/status [actionlib_msgs/GoalStatusArray]
 * /base_controller/joint_trajectory_action/result [pr2_controllers_msgs/JointTrajectoryActionResult]
 * /l_gripper_controller/gripper_action/status [actionlib_msgs/GoalStatusArray]
 * /move_base/feedback [move_base_msgs/MoveBaseActionFeedback]
 * /r_gripper_controller/gripper_action/result [pr2_controllers_msgs/Pr2GripperCommandActionResult]
 * /move_base/status [actionlib_msgs/GoalStatusArray]
 * /r_gripper_controller/gripper_action/feedback [pr2_controllers_msgs/Pr2GripperCommandActionFeedback]
 * /move_base/result [move_base_msgs/MoveBaseActionResult]
 * /base_controller/joint_trajectory_action/feedback [pr2_controllers_msgs/JointTrajectoryActionFeedback]
 * /base_controller/joint_trajectory_action/status [actionlib_msgs/GoalStatusArray]
 * /l_gripper_controller/gripper_action/result [pr2_controllers_msgs/Pr2GripperCommandActionResult]

rostopic hz, rostopic bw を調べてみて下れると嬉しいです.@h-kamada

@h-kamada
Copy link
Contributor Author

feedback, result は待機状態ではgoal無しで何も飛んでいないので、hz , bwともに得られません。
待機状態におけるそれぞれのstatusの情報は次のようになります。

h-kamada@pr1012:~$ rostopic hz /l_gripper_controller/gripper_action/status
subscribed to [/l_gripper_controller/gripper_action/status]
average rate: 5.527
        min: 0.105s max: 0.200s std dev: 0.03810s window: 6
^Caverage rate: 5.367
        min: 0.105s max: 0.200s std dev: 0.03331s window: 8
h-kamada@pr1012:~$ rostopic bw /l_gripper_controller/gripper_action/status
subscribed to [/l_gripper_controller/gripper_action/status]
average: 537.30B/s
        mean: 89.00B min: 89.00B max: 89.00B window: 6
average: 490.75B/s
        mean: 89.00B min: 89.00B max: 89.00B window: 11
^Caverage: 451.84B/s
        mean: 89.00B min: 89.00B max: 89.00B window: 12
h-kamada@pr1012:~$ rostopic bw /r_gripper_controller/gripper_action/status

subscribed to [/r_gripper_controller/gripper_action/status]
average: 604.46B/s
        mean: 100.00B min: 100.00B max: 100.00B window: 6
average: 551.76B/s
        mean: 100.00B min: 100.00B max: 100.00B window: 11
average: 534.29B/s
        mean: 100.00B min: 100.00B max: 100.00B window: 16
^C^Caverage: 434.72B/s
        mean: 100.00B min: 100.00B max: 100.00B window: 16
h-kamada@pr1012:~$ rostopic hz /r_gripper_controller/gripper_action/status
subscribed to [/r_gripper_controller/gripper_action/status]
average rate: 5.376
        min: 0.130s max: 0.201s std dev: 0.02803s window: 6
average rate: 5.181
        min: 0.130s max: 0.201s std dev: 0.02102s window: 11
^C^Caverage rate: 5.138
        min: 0.130s max: 0.201s std dev: 0.01867s window: 14
h-kamada@pr1012:~$ rostopic hz /move_base/status
subscribed to [/move_base/status]
average rate: 5.899
        min: 0.052s max: 0.202s std dev: 0.05864s window: 6
average rate: 5.415
        min: 0.052s max: 0.202s std dev: 0.04415s window: 11
^Caverage rate: 5.343
        min: 0.052s max: 0.202s std dev: 0.04068s window: 13
h-kamada@pr1012:~$ ^C
h-kamada@pr1012:~$ rostopic bw /move_base/status
subscribed to [/move_base/status]
average: 121.64B/s
        mean: 20.00B min: 20.00B max: 20.00B window: 6
average: 110.70B/s
        mean: 20.00B min: 20.00B max: 20.00B window: 11
average: 107.08B/s
        mean: 20.00B min: 20.00B max: 20.00B window: 16
average: 105.28B/s
        mean: 20.00B min: 20.00B max: 20.00B window: 21
^Caverage: 100.87B/s
        mean: 20.00B min: 20.00B max: 20.00B window: 22
h-kamada@pr1012:~$ rostopic hz /base_controller/joint_trajectory_action/status
subscribed to [/base_controller/joint_trajectory_action/status]
average rate: 5.298
        min: 0.144s max: 0.200s std dev: 0.02250s window: 6
average rate: 5.145
        min: 0.144s max: 0.200s std dev: 0.01687s window: 11
average rate: 5.096
        min: 0.144s max: 0.200s std dev: 0.01403s window: 16
^Cno new messages
h-kamada@pr1012:~$ ^C
h-kamada@pr1012:~$ ^C
h-kamada@pr1012:~$ rostopic bw /base_controller/joint_trajectory_action/status
subscribed to [/base_controller/joint_trajectory_action/status]
average: 120.59B/s
        mean: 20.00B min: 20.00B max: 20.00B window: 6
average: 110.21B/s
        mean: 20.00B min: 20.00B max: 20.00B window: 11
^Caverage: 103.22B/s
        mean: 20.00B min: 20.00B max: 20.00B window: 13

@k-okada
Copy link
Member

k-okada commented Nov 11, 2015

5hz で 120 or 550 B/s なものがそれぞれ2個づつある,という状況で正しい?

@h-kamada
Copy link
Contributor Author

全体では次のようになります。

/l_gripper_controller/gripper_action/status
/r_gripper_controller/gripper_action/status
5hz 500B/s

/move_base/status
/base_controller/joint_trajectory_action/status
5hz 120B/s

/head_traj_controller/follow_joint_trajectory/status
/l_arm_controller/follow_joint_trajectory/status
/r_arm_controller/follow_joint_trajectory/status
/torso_controller/follow_joint_trajectory/status
5hz 1.27KB/s

/torso_controller/state
100hz 15KB/s

/head_traj_controller/state
100hz 22KB/s

/tf
200hz 400KB/s

/joint_states
100hz 200KB/s

/pressure/r_gripper_motor
/pressure/l_gripper_motor
24hz 2.7KB/s

/r_arm_controller/state
/l_arm_controller/state
100hz 66KB/s

/base_odometry/odom
100hz 60KB/s

@k-okada
Copy link
Member

k-okada commented Nov 11, 2015

どれがどれぐらいきいているだろう?
100hz 60KB の3つをsubscribeしないと使える程度になったりする?

◉ Kei Okada

On Wed, Nov 11, 2015 at 1:55 PM, h-kamada [email protected] wrote:

全体では次のようになります。

/l_gripper_controller/gripper_action/status
/r_gripper_controller/gripper_action/status
5hz 500B/s

/move_base/status
/base_controller/joint_trajectory_action/status
5hz 120B/s

/head_traj_controller/follow_joint_trajectory/status
/l_arm_controller/follow_joint_trajectory/status
/r_arm_controller/follow_joint_trajectory/status
/torso_controller/follow_joint_trajectory/status
5hz 1.27KB/s

/torso_controller/state
100hz 15KB/s

/head_traj_controller/state
100hz 22KB/s

/tf
200hz 400KB/s

/joint_states
100hz 200KB/s

/pressure/r_gripper_motor
/pressure/l_gripper_motor
24hz 2.7KB/s

/r_arm_controller/state
/l_arm_controller/state
100hz 66KB/s

/base_odometry/odom
100hz 60KB/s


Reply to this email directly or view it on GitHub
#396 (comment)
.

@YoheiKakiuchi
Copy link
Member

spin-onceが遅くなる現象は、キューに多くのトピックがたまってそれを処理する時間がかかるのだと思います。
オプション無しでsubscribeしていれば、キューは1なので数ヘルツのデータだったら、1秒後と10秒後で
spin-onceの速度は変わらないと思います。

actionlib系はデータ取りこぼしが嫌なのでキューを無限大にしてあります。

これと同じ問題とおもっていて、
jsk-ros-pkg/jsk_pr2eus#119
joint_statesも取りこぼしが嫌でキューをどうにかしたいと思った記憶があったのですが
ソース上ではキューは1なように見えます。
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L146-L148

@k-okada
Copy link
Member

k-okada commented Nov 11, 2015

無限大ができたのか.
fb0bdcc
http://wiki.ros.org/roscpp/Overview/Publishers%20and%20Subscribers#Subscriber_Options

queue_size
This is the incoming message queue size roscpp will use for your callback. If messages are arriving too fast and you are unable to keep up, roscpp will start throwing away messages. A value of 0 here means an infinite queue, which can be dangerous.

とあるね.無限大はできないと@h-kamadaに説明していた気がする.嘘でした.

http://docs.ros.org/api/roscpp/html/classros_1_1NodeHandle.html#a317fe4c05919e0bf3fb5162ccb2f7c28
にこのことはないのはdangerousだから?

max 100hz, どうせtfだって20秒しかバッファがないから,2000というサイズはどうだろう.
また,goalは取りこぼすのは嫌だけど,feedbackはどうでもいいか.statusはどうなんだろう?
取りこぼしても問題ない?ある?feedbackは1, goal/statusは2000ぐらいか.

@YoheiKakiuchi
Copy link
Member

actionlibだと、以下の数字の0を変更するとキューのサイズが変わります。
https://github.com/jsk-ros-pkg/jsk_roseus/blob/master/roseus/euslisp/actionlib.l#L93-L107

この数字を変えてみて、変化があるか見てみてくれるかな?
@h-kamada

feedbackは1

それでいいと思います。

goal/statusは2000

もっと短くてもいいかもしれません。
trajectoryやgo-posはほとんど、wait-interpolationしていて、取りこぼさないように思います。

@h-kamada
Copy link
Contributor Author

statusのqueの個数について変化させながら、
(ros::spin-once "pr2_interface") にかかる時間を計測してみました。
result , feedbackは1で固定しました。

spin-once して40s後に再びspin-onceした際にかかる時間を5回計測し、平均します。
(40sだと、2000 , 1000, 500あたりが頭打ちになっている???)
statusのqueの個数: 2000 処理時間: 0.293438
statusのqueの個数: 1000 処理時間: 0.285796
statusのqueの個数: 500 処理時間: 0.290249
statusのqueの個数: 100 処理時間: 0.120743
statusのqueの個数: 50 処理時間: 0.062835
statusのqueの個数: 10 処理時間: 0.019033
statusのqueの個数: 1 処理時間: 0.008192

@h-kamada
Copy link
Contributor Author

300s後に再びspin-once する形では
que: 2000 処理時間: 1.79802
que: 1500 処理時間: 1.88189
que: 1000 処理時間: 1.27714
que: 500 処理時間: 0.703268
となります。

1000以上のque で設定してしまうと、全体で1s以上かかってしまうことがあります

@YoheiKakiuchi
Copy link
Member

すごくざっくりですが、キューは200くらいで良さそうな気がします。

result/goal はactionを使った場合にしかトピックがやりとりされないから、キューは大きくても小さくても
あまり時間には影響がなさそう。
feedbackは読み落としても問題ないだろう。
statusは200くらいあったら、まあ良さそうかな?

@k-okada
Copy link
Member

k-okada commented Nov 12, 2015

ところでstatus読み飛ばすと具体的に問題あるんだっけ?
切り替わった一瞬を捉える必要がないということは無いんだっけ?

◉ Kei Okada

2015/11/12 22:41、Yohei Kakiuchi [email protected] のメッセージ:

すごくざっくりですが、キューは200くらいで良さそうな気がします。

result/goal はactionを使った場合にしかトピックがやりとりされないから、キューは大きくても小さくても
あまり時間には影響がなさそう。
feedbackは読み落としても問題ないだろう。
statusは200くらいあったら、まあ良さそうかな?


Reply to this email directly or view it on GitHub.

@YoheiKakiuchi
Copy link
Member

ところでstatus読み飛ばすと具体的に問題あるんだっけ?
切り替わった一瞬を捉える必要がないということは無いんだっけ?

サーバー側のstatusと同期をとるようになっているはずで、
できるだけ読み飛ばさない方がいいと思いますが、
それほど問題なさそうに思えます。

@h-kamada
Copy link
Contributor Author

念のためque サイズを200として、 1000s と 2000s wait してからspin-once する時間をはかったところ、0.25s で終了しました。
実用上これで問題はなさそうです。

@k-okada
Copy link
Member

k-okada commented Nov 13, 2015

サーバー側のstatusと同期をとるようになっているはずで、

そもそも,
fb0bdcc
の変更は具体的に何か問題があったんでしょうか?
https://github.com/ros/actionlib/blob/indigo-devel/include/actionlib/client/action_client.h#L208
みると一でもいい気もしてくる.

@k-okada
Copy link
Member

k-okada commented Nov 13, 2015

jsk-ros-pkg/jsk_common#162
jsk-ros-pkg/jsk_common#51
あたりで議論があるのかな.

@k-okada
Copy link
Member

k-okada commented Nov 15, 2015

@k-okada
Copy link
Member

k-okada commented Nov 23, 2015

See #398 for The fix,choose buffet size defined in roscop

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging a pull request may close this issue.

3 participants