publish_spot を roseusかpr2eus に入れたほうがいいのではないか、と思いました。

[itohdak]

https://github.com/jsk-ros-pkg/jsk_demos/blob/master/jsk_maps/tools/publish_spot.l

[k-okada]

ナイスアイデアですね．まずは、

(defun pub-spot (name coords)
   ...
)

という関数を作って、 (pub-spot "73A2" (make-coords :pos #f(1200 0 600)) みたいにしたら、pinが表示できるような関数を定義してみましょう．

[furushchev]

横から失礼しますが、せっかくsceneクラスに:add-spotや:remove-spotみたいなものがあるので、これに連動してspotの出現や消滅も動的に可視化するともっといいと思いました。

[itohdak]

以下に岡田先生 @k-okada のご助言を元に作成した publish-spot.l を置きました。

(pub-spot-multi (list "73A1" "73A2" "73A3" "73B1" "73B2")
                (list #f(-1250 -675 0) #f(-1250 2400 0) #f(-1250 6300 0) #f(-850 -125 0) #f(-850 6675 0)))

とすると、rvizで以下のようにpinが表示できるようにしました。（mapのpublishの仕方がわからなかったので表示されてません）

https://gist.github.com/itohdak/f7892cf5ceecb0894b95292c0f8e5472

@furushchev :add-spotや:remove-spotなどのsceneのクラス定義はどこに書いてあるかわかりますでしょうか。

[furushchev]

@itohdak https://github.com/euslisp/jskeus/blob/master/irteus/irtscene.l#L107
scene関係のものはこのへんにあります。

[k-okada]

@itohdak おそくなりました．
地図からspotは以下のようにして持ってこれます．

1.irteusgl$ (load "models/room73b2-scene.l")
t
2.irteusgl$ (room73b2)
;; Could not find file models/cola-top.jpg
Color name not found Color name not found #<room73b2-scene #X65c57c0 room73b2  0.0 0.0 0.0 / 0.0 0.0 0.0>
3.irteusgl$ (objects (list *room73b2*))
;; (make-irtviewer) executed
(#<room73b2-scene #X65c57c0 room73b2  0.0 0.0 0.0 / 0.0 0.0 0.0>)
4.irteusgl$ (send *room73b2* :spots)
(#<cascaded-coords #X9986300 /eng2/7f/room73B2-front-of-kitchenboard  1350.0 1850.0 0.0 / 3.142 0.0 0.0> #<cascaded-coords #X9986378 /eng2/7f/room73B2-sink-front  1355.0 2450.0 0.0 / 3.142 0.0 0.0> #<cascaded-coords #X99863f0 /eng2/7f/room73B2-beside-chair  3860.0 350.0 0.0 / 0.0 0.0 0.0> #<cascaded-coords #X9986438 /eng2/7f/room73B2-far-chair-back  3470.0 1150.0 0.0 / 0.0 0.0 0.0> #<cascaded-coords #X99864b0 /eng2/7f/room73B2-tmp-chair-back  3280.0 2150.0 0.0 / 0.0 0.0 0.0> #<cascaded-coords #X99864f8 /eng2/7f/room73B2-chair-back  3580.0 1150.0 0.0 / 0.0 0.0 0.0> #<cascaded-coords #X9986570 /eng2/7f/room73B2-table-front  4200.0 1000.0 0.0 / 0.0 0.0 0.0> #<cascaded-coords #X99865e8 /eng2/7f/room73B2-table-back  5155.0 10.0 0.0 / 1.571 0.0 0.0> #<cascaded-coords #X99847b8 /eng2/7f/room73B2-table-side  5150.0 2180.0 0.0 / -1.571 0.0 0.0> #<cascaded-coords #X9984800 /eng2/7f/room73B2-front-kitchen-table  2293.0 1983.0 0.0 / 3.142 0.0 0.0> #<cascaded-coords #X9984878 /eng2/7f/room73B2-front-of-tv  3700.0 1700.0 0.0 / 1.571 0.0 0.0> #<cascaded-coords #X99848f0 door-spot  675.0 210.0 0.0 / 3.142 0.0 0.0> #<cascaded-coords #X9984938 coe-spot  1200.0 2300.0 0.0 / 0.0 0.0 0.0> #<cascaded-coords #X99849b0 broom-spot  2250.0 1000.0 0.0 / -1.571 0.0 0.0> #<cascaded-coords #X99849f8 table-spot  4100.0 1600.0 0.0 / 1.571 0.0 0.0> #<cascaded-coords #X9984a70 cook-spot  1100.0 1600.0 0.0 / 3.142 0.0 0.0> #<cascaded-coords #X9982eb0 fridge-front-spot  4800.0 1480.0 0.0 / 0.0 0.0 0.0> #<cascaded-coords #X9982ef8 init-spot  500.0 0.0 0.0 / 0.0\
 0.0 0.0>)
5.irteusgl$ (send *room73b2* :spots)

どのようなインターフェースにしたらいいか @furushchev さんからも意見があると思いますが、このプログラムは
https://github.com/jsk-ros-pkg/jsk_roseus/blob/master/roseus/euslisp/roseus-utils.l
に入るとおもうので、

(defun spot-coords->marker-msg ()
)

みたいな関数を作って、

(defclass scene-model
  (:publish-spot ()
))
```
から使うのはどうでしょうか．
```

[itohdak]

遅くなりましたが、ひな形を作ってみました。
１つ目がeuslisp上でのspot-coordsの追加・削除
２つ目がspot-coordsのros用msgの生成
のつもりです。
https://gist.github.com/itohdak/24d1a054c970edba927781daf8128c6e#file-irtscene-l-L157
https://gist.github.com/itohdak/9089e5417fd221bdaf4d0c8f1e076812

(ros::roseus "publish_jskmap_spots")
(ros::roseus-add-msgs "jsk_rviz_plugins")

(defvar *pub-spots-topic* "/spots_marker_array")
(ros::advertise *pub-spots-topic* visualization_msgs::MarkerArray 5)

(ros::rate 0.1)
(do-until-key
  (let ((sc (make-cascoords :pos #f(0 0 0) :name "spot-coords"))
    (header (instance std_msgs::header :init :stamp (ros::time-now) :frame_id "/map"))
    (msg-array (instance visualization_msgs::MarkerArray :init))
    msg)
    (setq msg (spot-coords->markers-msg sc header))
    (send msg-array :markers msg)
    (print (list msg-array))
    (ros::publish *pub-spots-topic* msg-array))
  (ros::spin-once)
  (ros::sleep)))

などとすると、rviz上にピンを表示できました。

何点かわからないことが…

• *room73b2*などのeuslisp側で生成したsceneはどのようにrvizに反映できるのでしょうか。
• (make-pin-body)や(make-color-list)が浮いてる感じになっているのですが、これらはどこに入れるのがよいのでしょうか。