#167 #r4489のactionlib-commstate.lでエラーが出ます。

[YutoInagaki]

<move_base_msgs::movebaseactiongoal #xaa38660=""></move_base_msgs::movebaseactiongoal>

"wiat for result"
/home/applications/ros/groovy/jsk-ros-pkg/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :goal_id in (send ros::action-goal :goal_id :id)

というエラーが出ていたため、
岡田先生に先ほど直して頂いたのですが、
コミットした[r4489]でもまだバグがあったようで
/home/applications/ros/groovy/jsk-ros-pkg/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: mismatch argument in (ros::ros-warn invalid goal id: ~A ~A~% msg :~A~% action-goal:~A (send ros::msg :status :goal_id :id) (if ros::action-goal (send ros::action-goal :goal_id :id)) ros::msg)
のように言われてしまします。

[Kei Okada]

[r4496]で直しました．

ただ，本来はこの部分(:update-resultの　"invalid goal id" に入ってはダメなんだけど，なぜ
ここに来るんだろう？
test/test-multi-queur.launchに関係している？
あるいは:wait-interpolationを取りこぼすのと関係している？

[Yohei Kakiuchi]

バグ自体はごめんなさいなんだけど、このワーニングが出るのはどんな時かな？
同じアクションサーバーに複数のクライアントからgoalを送った時とか？

[Kei Okada]

稲垣くんからの又聞きですが，普通に:move-toを送っていると時どきこの表示が出るという感じでした．
(demo)の途中でもでるのか，あるいは，インタプリタ上からsend ri した時だけでるのか，というのは
気になりますね．

ただコードを見ると，

 (:update-result
   (msg)
   (unless (and action-goal
                (string= (send msg :status :goal_id :id)
                         (send action-goal :goal_id :id)))
     (ros::ros-warn "invalid goal id: ~A ~A~%           msg :~A~%    action-goal:~A"
                    (send msg :status :goal_id :id)
                    (if action-goal (send action-goal :goal_id :id))
                    msg action-goal)
     (return-from :update-result nil))


  (:action-result-cb
   (msg)
   (let ()
     (ros::ros-debug ";; [~A] reuslt ~A" name-space msg)
     (send comm-state :update-result msg)
     ))

となっていて，reusltは型さえ同じだったらresult-cbが呼ばれてしまうという場合なので，
ここでは，update-resultでmsgのgoal_idとaction-goalのgoal_idが違ったら，自分宛のメッセージ
でないからワーニングではなく普通にスルーする，というのがいい気がしてきました．

テストコードが必要そうですね．

[Kei Okada]

test/fibonacci-client.l
test/test-fibonacci.launch
で複数のクライアントからゴールを送るサンプルにしてみました．
それぞれ自分の送ったゴールだけを受け取っているようです．
ここで，invalid goal idが出るのは正しい挙動なので
[r4504]でros-warnでなくros-debugを使うようにしました．

[Kei Okada]

そもそも２人以上が:move-toを使っているという状況ではなかった気がするので，裏で走っているコードがあるのかもしれないんですが，それは別として，稲垣くんによると invalid id はよく表示されているということで，action-goalがnilにっているになっているのが問題じゃないかというきがします．普通はaction-goalはなにか入っているはず．

[YutoInagaki]

合っているかわかりませんが、
今のところ確認できるinvalid goal idが表示されるタイミングは、
「右に曲がります」や「左に曲がります」と
pr2の音声が流れるときだけです。
他にもわかったら随時報告します。

[Kei Okada]

test-fibonacci.launch を更新して２つのクライアントがあるようなテストにしました．
jenkinsを見ると時どき失敗しているんですよね．wait-for-serverとかでros:spin-onceしている
のが問題かと思って，デフォルトでspin queueを使おうと

~~~~~~
(:init
(ns spec &key ((:groupname gp)))
(send self :name ns)
(setq name-space ns)
;; (setq groupname gp)
(setq groupname ns)

とすると，

[ERROR] [1370357880.961906112]: attempted to getNumPublishers to topic fibonacci/feedback, which was not previously subscribed. call (ros::subscribe "fibonacci/feedback") first.
~~~~~~~
とでてくるんですが，どうしたらいいでしょうか？

[Yohei Kakiuchi]

新しいキューを持ったnodehandleをあらかじめ作っておく必要がある仕様でした。
actionlibの中でgroupnameのキューを持つnodehandleを作るように[r4536] で修正しました。

ちなみに、ros::create-nodehandle groupname を同じgroupnameで複数回呼んでも2回目以降は
何もせずに返ってくるので問題ありません。

[Yohei Kakiuchi]

actionlibを異なるキューでsubscribeしたかった元々の理由は、
時間がかかるセンサ系のspinとwait-interpolationなどでのspinを分けたかったからです。

オリジナルのキューか、異なるキューかでactionlib自体の動作が変わる理由は無いように
思います。いずれにしろ、複数クライアントで失敗する理由は追求が必要ですが。

[Kei Okada]

[r4616]でサーバ側もqueue sizeを0にしてみました．

で，添付のactionlib.txtで;; test-fibonacci 56/1000　は上手く行っているけど，;; test-fibonacci 57/1000ではダメなのは以下の状況で，result-cbされたあと，status-cbがきて simple-state が更新されることがきたいされているんだけど，それが来ずに次の（別クライアントの）ゴールのreuslt-cbが来ている気がします．

[ INFO] [1370790250.890998579]: [fibonacci] status-cb  (seq:698793 (id:#i(1370790250 788745430), status:0))
[ INFO] [1370790250.891457829]: [fibonacci] reuslt-cb #i(1370790250 788745430)
[ INFO] [1370790250.891538572]: update-result goal id: #i(1370790250 788745430) ros::*comm-state-pending*
[ INFO] [1370790250.891657237]: COMM-STATE #<actionlib_msgs::goalstatus #X5f4d7f8>
    (ros::*comm-state-pending* -> ros::*comm-state-waiting-for-result*)
    by status:actionlib_msgs::goalstatus::*succeeded* goal_id:#i(1370790250 788745430)
[ INFO] [1370790250.891728368]: update-result state: #i(1370790250 788745430) ros::*comm-state-done*
[ INFO] [1370790250.891791989]: [fibonacci] reuslt-cb #i(1370790250 788745430) -> state ros::*comm-state-done*
[ INFO] [1370790250.892522180]: [fibonacci] reuslt-cb #i(1370790250 792735749)
[ INFO] [1370790250.892619694]: invalid goal id: #i(1370790250 792735749) #i(1370790250 788745430)
           msg :#<actionlib_tutorials::fibonacciactionresult #X5f58ff8>
    action-goal:#<actionlib_tutorials::fibonacciactiongoal #X5e70318>
[ INFO] [1370790250.892677702]: [fibonacci] reuslt-cb #i(1370790250 792735749) -> state ros::*comm-state-done*
[ INFO] [1370790250.989421938]: [fibonacci] wait-for-result ros::*simple-goal-state-pending* gc 528204 1348761
[ INFO] [1370790251.089306269]: [fibonacci] wait-for-result ros::*simple-goal-state-pending*
[ INFO] [1370790251.189301897]: [fibonacci] wait-for-result ros::*simple-goal-state-pending*

以下のようにして治らないでしょうか？

  (:action-result-cb
   (msg)
   (let ()
     (ros::ros-debug "[~A] reuslt-cb ~A" name-space (send msg :status :goal_id :id))
     (send comm-state :update-result msg)
     (ros::ros-debug "[~A] reuslt-cb ~A -> state ~A" name-space (send msg :status :goal_id :id) (send comm-state :state))
     (setq simple-state ros::*simple-goal-state-done*)
     ))

[Kei Okada]

action_client.pyの
def update_result(self, action_result):
でも，
self.transition_to(CommState.DONE)
としている，ということで，以下のようにすると良さそうです．
一応，test-actionlib.lや，pr2eusのpr2-ri-test.launch は通っているようです．他に確認するものはあるかな．[r4619]でコミットしました．

k-okada@kokada-t430s:~/ros/fuerte/jsk-ros-pkg/roseus/euslisp$ svn diff
Index: actionlib.l
===================================================================
--- actionlib.l (リビジョン 4618)
+++ actionlib.l (作業コピー)
@@ -54,10 +54,16 @@
          ))))
   (:action-result-cb
    (msg)

-   (let ()
-     (ros::ros-debug "[~A] reuslt-cb ~A" name-space (send msg :status :goal_id :id))
+   (let (dummy-msg)
+     (ros::ros-debug "[~A] reuslt-cb ~A ~A" name-space (send msg :status :goal_id :id) (goal-status-to-string (send msg :status :status)))
      (send comm-state :update-result msg)
      (ros::ros-debug "[~A] reuslt-cb ~A -> state ~A" name-space (send msg :status :goal_id :id) (send comm-state :state))
+     ;; transition to goal state
+     (setq dummy-msg (instance actionlib_msgs::GoalStatusArray :init))
+     (describe (send msg :result))
+     (send dummy-msg :status_list (list (send msg :status)))
+     (send dummy-msg :header (send msg :header))
+     (send self :goal-status-cb dummy-msg)
      ))
   (:action-feedback-cb
    (msg)

[Kei Okada]

テストも一応通っているのでcloseします.
[#54]の問題はこれと関係あるかは不明ですが，[#54]自体は時どき起こる気がします．