Support multi-thread service call in RTC's ROSBridge.

[snozawa]

Currently multi-thread service call is not supported in RTC's ROSBridge.

Example

terminal 1

 rtmlaunch hrpsys_ros_bridge samplerobot.launch RUN_RVIZ:=false USE_UNSTABLE_RTC:=true

terminal 2

roseus
(progn (load "package://hrpsys_ros_brige/euslisp/samplerobot-interface.l") (samplerobot-init))
(send *ri* :angle-vector (send *sr* :angle-vector) 2000)
(send *ri* :start-auto-balancer)
(send *ri* :go-pos 0 0.4 0)

terminal 3

roseus
(progn (load "package://hrpsys_ros_brige/euslisp/samplerobot-interface.l") (samplerobot-init))
(send *ri* :emergency-walking-stop) ;; call during go-pos in terminal 2

Expected behaviour
Stop walking on :emergency-walking-stop

Current behavior
:emergency-walking-stop is not called soon.
In :go-pos, goPos() and waitFootSteps() service calls are called.
In calling waitFootSteps(), AutoBalancerROSBridge stops at ros::spinOnce() in onExetuce until walking has finished.
After finishing waitFootSteps(), AutoBalancerROSBridge receives emergencyStop() service call.

To resolve this, Multi-threaded spinning is necessary:
http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning#Multi-threaded_Spinning
But, it seems only to support spin() instead of spinOnce().

[garaemon]

actionlib is designed for this kind of purpose. topic based i/o is good for asynchronous implementation.

[garaemon]

Another way is to use ros::Timer to run main loop.
http://wiki.ros.org/roscpp/Overview/Timers

[snozawa]

Thanks.
I'll check both way.
Using the ways other than ros::Timer changes specification of ROSBridge in terms of not using ros service.

[k-okada]

fixing https://github.com/start-jsk/rtmros_common/blob/master/rtmbuild/scripts/idl2srv.py to provide both service call and actionlib may good solution.

[k-okada]

But, in
In :go-pos, goPos() and waitFootSteps() service calls are called.
case, how about implementing go-pos-nowait ?
jsk-ros-pkg/jsk_pr2eus#160

[snozawa]

Thanks you for your comment.

fixing https://github.com/start-jsk/rtmros_common/blob/master/rtmbuild/scripts/idl2srv.py to provide both service call and actionlib may good solution.

Yes, I think so.

how about implementing go-pos-nowait ?

:go-pos-no-wait is already added.
https://github.com/start-jsk/rtmros_common/blob/master/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l#L799
More than one node calls :wait-foot-steps, this problem occurs.

[Naoki-Hiraoka]

#1127

OpenRTMのサービスサーバーは、サービスが呼ばれるたびにそのサービスを処理するスレッドを生成して割り当てるような挙動をします。それに対して、ROSはSingle-threaded spinningもMulti-threaded spinningもあらかじめスレッドの数が決められていて、サービスやトピックやタイマーが呼ばれるたびにスレッドに余りがあれば割り当てていくような挙動をします。

そのため、RTCではスレッド数の上限を意識せずにwaitFootSteps()などのサービスが書かれているのに、ServiceROSBridgeにはスレッド数の上限があるので、ServiceROSBridgeのスレッド数が足りなくなると意図せぬ振る舞いをしてしまいます。

しっかり直すなら、ROSのActionServerを独自実装して、OpenRTMのようにgoalが呼ばれるたびにそのgoalを処理するスレッドを生成して割り当てるような挙動をするようなActionServerをServiceROSBridgeに入れて、各スレッドの中でRTCに対してOpenRTMのサービスを呼ぶようにする必要があります。

ただ、そこまでしっかり直さなくても、AutoBalancer等の一部のServiceROSBridgeのスレッド数を少し増やせばスレッド数が足りなくなることはほぼ無くなるので、自分の場合は#1128 で実用上は困ることはなくなりました。