2023年度ゼミ

[k-okada]

@keitayoneda ロボットの認識・行動判断に興味がある。Spotを使って細いところを通り抜けるのを見てみたい。
@yoshiki-447 理学療法。介護ロボット。人を持ち上げたり、あるいは人との会話。アドバイスや寄り添い。
@KIMJUNMO1106 月で建築作業をするロボット。人間のような機構、パワーを出す、筋電を使う。
@nockoy ロボットかっこいいな。かわいいな。

https://github.com/jsk-ros-pkg/jsk_demos/tree/jsk_2023_10_semi

[k-okada]

https://raw.githubusercontent.com/mqcmd196/jsk_robot/14e0f511a8878d4623b3e7ae43aa18cfecca81d7/jsk_fetch_robot/jsk_fetch_user.rosinstall.noetic

別ターミナルでroscore を立ち上げておいて、、、、

M-x shell
> source ~/semi_ws/devel/setup.bash
> roscd jsk_2023_10_semi
> roseus
$ (load "package://fetcheus/fetch-interface.l")
$ (fetch-init)
$ (objects (list *fetch*))
$ (send *fetch* :angle-vector)
$ (send *ri* :angle-vector (send *fetch* :angle-vector))
$ (send *fetch* :rarm :elbow-p :joint-angle 90)

;; 関節名は https://github.com/jsk-ros-pkg/jsk_robot/tree/master/jsk_fetch_robot

[mqcmd196]

今日はお疲れ様でした．D1の大日方(おびなた)です．

今日のおさらいです

Fetchを動かしたいとき

JSKに来たら

JSKに来たら自分のPCを有線につないで，WiFiをオフにする．
ターミナルで ip a と打って133.11.216....というアドレスが見えていることを確認

ターミナルごとの設定

以下は新しくターミナルを開くごとに行う必要があります

ターミナルで

source ~/semi_ws/devel/setup.bash # ROSを使うためのおまじない
rossetmaster fetch1075 # Fetchの中のROSネットワークに入るためのおまじない
rossetip # おまじない

のおまじないを打ったら

rostopic list

でFetchのtopic群が見えることを確認
そうしたら

roseus

でeusのインタプリタを起動

[mqcmd196]

roseusでロボットを動かす準備をする

(load "package://fetcheus/fetch-interface.l") ;; Fetchを動かすために必要なパッケージを読み込む
(fetch-init) ;; Fetchのインタフェース，モデルなどの初期化
(objects (list *fetch*)) ;; ビューワの表示

実際に動かしてみる

今日使ったコードの一覧

;; ビューワの中のFetchで姿勢を作ってみる
(send *fetch* :rarm :elbow-p :joint-angle 90)

;; 自分で作った姿勢をロボットに送りたい．
;; ビューワ中のFetchモデル，*fetch* の姿勢を自分で作ったと仮定する 
(send *ri* :angle-vector (send *fetch* :angle-vector) 8000) ;; 8000ms でロボット実機の姿勢を変更

;; Fetchの姿勢を標準姿勢に戻したい
(send *fetch* :reset-pose) ;; ビューワ中のFetchモデル，*fetch* の姿勢をリセットする
(send *ri* :angle-vector (send *fetch* :angle-vector) 8000)

なお *ri* は robot interfaceの略(多分)

[k-okada]

画像処理の結果は

$ rostopic list | grep edge
/edgetpu_human_pose_estimator/output/class
/edgetpu_human_pose_estimator/output/image
/edgetpu_human_pose_estimator/output/image/compressed
/edgetpu_human_pose_estimator/output/poses
/edgetpu_human_pose_estimator/output/rects
/edgetpu_human_pose_estimator/parameter_descriptions
/edgetpu_human_pose_estimator/parameter_updates
/edgetpu_object_detector/output/class
/edgetpu_object_detector/output/image
/edgetpu_object_detector/output/image/compressed
/edgetpu_object_detector/output/rects
/edgetpu_object_detector/parameter_descriptions
/edgetpu_object_detector/parameter_updates

等から始めるとよい
rqt_image_view を用いて、/edgetpu_object_detector/output/image/compressed や /edgetpu_human_pose_estimator/output/image/compressed を見ると、どのような結果か確認できる。

euslisp で書いたサンプルプログラムは以下。

(ros::load-ros-manifest "roseus")
(ros::load-ros-manifest "jsk_recognition_msgs")

;;;
(ros::roseus "rect_client")

(defun cb (msg)
  (dolist (rect (send msg :rects))
    (ros::ros-info "x:~3A y:~3A w:~3A h:~3A ~5A" (send rect :x) (send rect :y)  (send rect :width)  (send rect :height) (* (send rect :width)  (send rect :height))))

  (when (> (length (send msg :rects)) 0)
    (ros::ros-error "~A human found" (length (send msg :rects))))
  )

(ros::subscribe "/edgetpu_human_pose_estimator/output/rects" jsk_recognition_msgs::rectarray #'cb)

(ros::spin)

[k-okada]

誰かがプログラムを更新したら

cd ~/semi_ws/src/jsk_demos
git pull origin jsk_2023_10_semi

で手元に持ってこれる。
自分でプログラムを更新したら

git add TEST.L
git commit -m "COMMIT" TEST.L
git push USERNAME jsk_2023_10_semi

とする。その後、https://github.com/jsk-ros-pkg/jsk_demos からPullRequestを作成する。
大文字の部分は各自の変える事

一番最初だけ、

1. https://github.com/jsk-ros-pkg/jsk_demos からフォークする
2. git remote add USERNAME [email protected]:USERNAME/jsk_demos.git
3. https://docs.github.com/ja/authentication/connecting-to-github-with-ssh/adding-a-new-ssh-key-to-your-github-account　を参考にssh key を追加

とする。

[k-okada]

@keitayoneda 日常の面倒なことをやってくれる。ごみを集める、カップラーメンを作る。マラソン一緒に走ってくれる。 -> スポット環境を作る　https://github.com/jsk-ros-pkg/jsk_robot/blob/4e2bee530757a88b27562fb0b8d3f65a6bfef61f/jsk_spot_robot/README.md
@yoshiki-447 料理出来ました。という、銀杏掃除。走っているときの速度を言ってくれる、遅くなったら応援してくれる。　-> 音声認識合成/GPT環境を作る　オウム返しプログラムを作成する (https://github.com/jsk-ros-pkg/jsk_3rdparty/tree/master/ros_speech_recognition )
@KIMJUNMO1106 カップシャッフルをする、見破る。ロボット同士でじゃんけんして、買ったら、たたいて被ってじゃんけんぽん。疲れた姿勢を見たら、水を渡す -> 姿勢認識／トラッキング -> bagファイルをとる、再生する (https://wiki.ros.org/rosbag, https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/jsk_fetch_startup/launch/rosbag_record.launch or https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/rosbag_record.launch )
@nockoy 軽く料理、肉まんをレンチン。キャッチボール。ロボット二台がテープ引っ張ってゴールしてれる　ロボットが癒してくれる・頑張ったね　→　Nao環境を作ってみる (https://github.com/jsk-ros-pkg/jsk_robot/tree/master/jsk_naoqi_robot )
＝＝

[keitayoneda]

spot環境を作る際にcatkin buildが失敗する

• エラー文（の例)

  Project 'collada_urdf' specifies '/usr/../include/include' as an include
  dir, which is not found.  It does neither exist as an absolute directory
  nor in '${{prefix}}//usr/../include/include'.  Check the issue tracker
  'https://github.com/ros/collada_urdf/issues' and consider creating a ticket
  if the problem has not been reported yet.

• 対策

/opt/ros/noetic/share/collada_urdf/cmake/collada_urdfConfig.cmakeにおいてincludeディレクトリを指定するパスが間違えているので、該当箇所を直接書き換える必要がある。

sudo sed -i 's#/usr/../include/include;##' /opt/ros/noetic/share/collada_urdf/cmake/collada_urdfConfig.cmakeとすれば該当箇所が置換される

[KIMJUNMO1106]

@KIMJUNMO1106
Use https://github.com/jsk-ros-pkg/coral_usb_ros#readme when you want to use human pose estimation.
You can follow setup section.

Cc:@tkmtnt7000

[tkmtnt7000]

@KIMJUNMO1106 に今日取ったバグファイルを使って家でも人のポーズ推定ができるようにcoral edgetpuを渡しました．
この後予定があるとのことで環境構築ができていないのでどこかで詰まったらこのissueなどで教えてもらうか，他が忙しければ来週一緒にやりましょう．

[keitayoneda]

@k-okada すみません。本日のゼミですが、家の方で外せない急用ができてしまったので今日はお休みして明日、明後日研究室の方に伺いたいのですが可能でしょうか？現状のタスクとしては

1. spotの環境構築 -> 前回通信ができないところで止まっている
2. （環境構築ができたら）spotについているカメラを使って人認識を試す
3. （環境構築ができたら）spotにgo_velocityに類する移動指令を試す

だと思っていて、1に関しては @pazeshun さんや @tkmtnt7000 さんが先週解決を試みてくださっていたと認識しています

[k-okada]

はい。大丈夫です。というか、今日はそもそも補講日なのでマストではないです。 明日来るなら時間をあらかじめ教えてください。 明後日は平日なのでいつでも良いです。 -- ◉ Kei Okada 2023年11月22日(水) 13:53 keitayoneda ***@***.***>:

…

@k-okada <https://github.com/k-okada> すみません。本日のゼミですが、家の方で外せない急用ができてしまったので今日はお休みして明日、明後日研究室の方に伺いたいのですが可能でしょうか？現状のタスクとしては 1. spotの環境構築 -> 前回通信ができないところで止まっている 2. （環境構築ができたら）spotについているカメラを使って人認識を試す 3. （環境構築ができたら）spotにgo_velocityに類する移動指令を試す だと思っていて、1に関しては @pazeshun <https://github.com/pazeshun> さんや @tkmtnt7000 <https://github.com/tkmtnt7000> さんが先週解決を試みてくださっていたと認識しています — Reply to this email directly, view it on GitHub <#1393 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AADYNXFUBSG35M6S3TM6IJTYFWALJAVCNFSM6AAAAAA6YZ47VOVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMYTQMRSGEYTEOJRG4> . You are receiving this because you were mentioned.Message ID: ***@***.***>

[tkmtnt7000]

11/23は勤労感謝の日で73B2に人が少ないorいない(この場合は部屋に入れない)と思うので
明日来る場合は早めに連絡をくれたら時間次第ですが対応できる可能性が有ります．

[keitayoneda]

書き方が良くなかったのですが明日か明後日のどちらかに伺うつもりだったので、明後日にします。対応いただきありがとうございます。

[keitayoneda]

話は変わるのですが、spotとの通信(rossetmaster & rossetipまわり)についてはどうなりましたでしょうか？

[tkmtnt7000]

話は変わるのですが、spotとの通信(rossetmaster & rossetipまわり)についてはどうなりましたでしょうか？

これはspotとクライアントPCの接続の仕方の問題で，クライアントPCをSpotが出しているSSID名がBelkaのWi-fiにつないでrosssetip, rossetmaster belka.localとするとできると思います．
次来たときに一緒にやりましょう

[keitayoneda]

了解です、ありがとうございます！

[sktometometo]

@yoshiki-447

while true
  message = speech_to_text メッセージを取得
   completion = client.chat.completions.create(
    ...:   model="gpt-4",
    ...:   messages=[
    ...:     {"role": "system", "content": "dummy"},
    ...:     {"role": "user", "content": message},
    ...:     ]
    ...:     )
  robot_message = completeion.....
  print(robot_message)
  text_to_speechで話す(robot_message)
  
  
messages = [{"role": "system", "content": "dummy"}]
while true
  message = speech_to_text メッセージを取得
  messages.append({"role": "user", "content": message})
  completion = client.chat.completions.create(
      model="gpt-4",
      messages=messages
      )
  robot_message = completeion.....
  print(robot_message)
  text_to_speechで話す(robot_message)
  messages.append({"role": "assistant", "content": robot_message})


messages = [{"role": "system", "content": "special"}]
while true
  message = speech_to_text メッセージを取得
  messages.append({"role": "user", "content": message})
  completion = client.chat.completions.create(
      model="gpt-4",
      messages=messages
      )
  robot_message = completeion.....
  if is_json(robot_message):
    data = parse_json(robot_message)
    robot.do(data)  
  else:
    print(robot_message)
    text_to_speechで話す(robot_message)
    messages.append({"role": "assistant", "content": robot_message})

[keitayoneda]

@tkmtnt7000 研究室つきました。もし今日いらしていたらbelkaとの接続方法など教えていただきたいです

[tkmtnt7000]

明日塚本がゼミの時間帯に席を外しているかもしれないので念のため連絡しておきます。

Naoについて、73B2の掃除道具入れ横のDia制御用PCと書かれたPCにros melodicのNaoのros master用リモートPCを設置しています。近くにNao(Diaという名前の機体です)もいます。

Naoを動かしやすい場所で133系のネットワークに繋いで起動した後にDia制御用PCも適当な場所で133系に繋いで起動して（パスワードはユーザ名から類推すればJSKの人ならわかるはず）

source ~/ros/nao_ws/devel/setup.bash
roslaunch jsk_nao_startup jsk_nao_startup.launch network_interface:=<your network interaface (ex. eth0, enp0s31f6...)>
# or if you want to use speech recognition through nao's internal mic, use the following command
# roslaunch jsk_nao_startup jsk_nao_startup.launch network_interface:=enp4s0 launch_ros_speech_recognition:=true audio_org:=/nao_robot/naoqi_driver/audio

を実行してください。
<your network interface>の部分は、別ターミナルで

ifconfig

して133.11.xxx.xxxと書かれている部分のインターフェース名(eth0とかenp0s3...とか)を例に従ってコピペして書いてください。
(僕が先週か先々週かあたりに試したコマンドのログが残っていると思うので、Ctrl-rして検索してもらっても良いと思います。)
launchできるとnao_dashboardみたいな画面が出てきてGUIから立たせたり座らせたりできるようになります。

以上をDia制御PCで実行できたら自分のローカルpcで

rossetip
rossetmaster 133.11.xxx.xxx <IP address of Dia control PC. You can find this with command `ifconfig`>

してrostopic listとかrosservice listするとtopicが見えるようになると思います。あとはnaoのREADMEのサンプルに従って色々動かすことができると思います。
2年前に73B1にいる吉村くんが作っていたsampleなんかはhttps://github.com/jsk-ros-pkg/jsk_robot/tree/ddf454dd5f4f4c1aa43b81fda6e17a24d96c721f/jsk_naoqi_robot/naoeus/sample
に見つかるのでやりたいことに対して直接役に立つかどうか不明ですが見てみても良いと思います。(nao-test-ik.lだったかな？違うかもしれない)

（開発者向け連絡：noeticクライアントから動かすことを失念していたのでもし動かなかったらデバッグ対応いただけたら嬉しいです。move_baseとか使っていなければmsg型は変わっていないはずなので問題ない気はしています。）

[mqcmd196]

@tkmtnt7000 naoの環境ありがとうございました！めっちゃ助かりました

[k-okada]

@keitayoneda, @yoshiki-447, @KIMJUNMO1106, @nockoy 申し訳ないんだけど、昨晩から風邪で熱が出てしまって学校に登校するのは控えることにしました。@pazeshun, @mqcmd196, @tkmtnt7000, @sktometometo で代りに対応してもらえるかな？

[sktometometo]

対応します

[tkmtnt7000]

はい．インフル等も流行っているようなのでおだいじになさってください．

[tkmtnt7000]

@yoshiki-447
rostopic echoしたときのunicode文字列を読む方法
#1347 (comment)

[Kanazawanaoaki]

@nockoy くんかな？
NAOでできそうな色抽出の2次元画像認識関係のメモです．参考までに．

• camshift
  https://github.com/jsk-enshu/robot-programming#start-color-tracking-node
  https://wiki.ros.org/opencv_apps#camshift
• hsv_color_filter
  https://jsk-docs.readthedocs.io/projects/jsk_recognition/en/latest/tutorials/find_object_with_color_filtering.html
  https://wiki.ros.org/opencv_apps#hsv_color_filter

[tkmtnt7000]

https://drive.google.com/file/d/1MYJ6jxYHE64TLcvbMR2oyQBQ-j7yFlKw/view?usp=drivesdk
先ほどの動画です

[keitayoneda]

ありがとうございます！まだ追従はおぼつかないのですが7階の通路を一周することができました！

[keitayoneda]

@KIMJUNMO1106 遅くなってごめん！BoundingBoxArrayをspotから見たPoseStampedに変換するコード上げた！
https://github.com/keitayoneda/jsk_robot/blob/dev/running/jsk_spot_robot/script/nearest_person.py

[KIMJUNMO1106]

明日午後に書いたコードがうまく行くかspotを動かしてみたいんですけどいつ研究室に行ったら大丈夫でしょうか？

[k-okada]

PCに関しては来てくれたら何とかします．
来週の月曜日まで修論発表準備でM2の人が大変だけど，ロボット使わないならSpot使えるかな？と思ったんだけど， @tkmtnt7000 は追加実験を予定しているということみたいなので，だったら来週の火曜日ぐらいが良いですね．あるいは，来てもらって実験手伝いする，というのも，見ているだけで勉強になるとおもいます．

[keitayoneda]

PCの環境について見ていただけるとのこと、ありがとうございます。
またもし邪魔にならないなら、実験のお手伝いもやらせていただきたいです。(見学させていただくだけでも大丈夫です)
その場合いつ行けばいいでしょうか？

[tkmtnt7000]

来週の月曜日まで修論発表準備でM2の人が大変だけど，ロボット使わないならSpot使えるかな？と思ったんだけど，

Spot自体はLidar以外？は使えます．

またもし邪魔にならないなら、実験のお手伝いもやらせていただきたいです。(見学させていただくだけでも大丈夫です

申し訳ないですが，追加実験の予定は僕の発表スライドの進捗具合で変わるのでアナウンスできるかどうかは怪しいです．2/6以降僕の発表が終わっていれば来てくれたときによねだくんの実験でも僕のデモでも何か一緒に動かしましょう．

[keitayoneda]

@tkmtnt7000 了解です！ではとりあえず2/7に伺おうと思います。
@k-okada pcについても2/7に見ていただくことは可能でしょうか？もし別日の方が都合がよければ教えていただきたいです。

[k-okada]

はい。2/7の午後なら大丈夫です。それ以前でも @mqcmd196 @sktometometo が時間あれば、適当なPCを用意してセットアップするのが良いと思います

[mqcmd196]

はい，PCのセットアップについては今日明日でラップトップ一台用意しておきます．

ちなみにUbuntu 20.04が入っていればSpotはパッと使える環境になると思うけど，それで大丈夫そうでしょうか？

[keitayoneda]

ありがとうございます。ros1が入る環境であれば最低限は大丈夫だと思います

[sktometometo]

@keitayoneda

こんな感じで 可視画像の情報付きの点群を可視化&出せるようになりました．
手元のPCの jsk_spot_startup を 僕のフォークの develop/spot_arm_with_lifelog ブランチ ( https://github.com/sktometometo/jsk_robot/tree/develop/spot_arm_with_lifelog ) にする必要があります

Spotに直接つないで見るのはまだできていないのですが，StrelkaのPC上で rosbag_record.launch で記録したものを手元のnoetic PCで rosbag_play.launch すると上記のように見えるようになります．

また，そのうえで roslaunch jsk_spot_startup generate_point_cloud.launch を走らせると以下の PointCloud2 型のトピックが出るようになります．
立ち上がったrvizでプラグインを有効化するとそちらも見えるようになります．

• /spot/depth/points: 体の5個のカメラを合わせたグレースケール点群
• /spot/camera/hand_depth_in_hand_color_frame/points: 手のカメラを使ったカラー点群

[mqcmd196]

対応ありがとうございます．
FYIですが，昨日 rosbag playでdepthから点群を構成するlaunchを @keitayoneda 君のPCで書きました．ロボット側から出ることになったので，自分の家とかで書いた点群処理コードを実機でそのまま試せるようになったと思います

[keitayoneda]

@sktometometo ありがとうございます！むちゃくちゃきれいに出てますね！
@mqcmd196 理解しました、点群処理なども含め、最終的にはspot内ですべて完結できたほうが通信のオーバーヘッド等がなくなるのでいい気がしますね！

[sktometometo]

@keitayoneda
あと1点注意なんですが、Spot側のPC（SpotCORE）のCPUロードを下げるためにパノラマの画像のレートを1Hzに落としてます

もとのレート（10-15くらい？）に上げるには、 https://github.com/sktometometo/jsk_robot/blob/develop%2Fspot_arm_with_lifelog/jsk_spot_robot%2Fjsk_spot_startup%2Flaunch%2Finclude%2Fperipheral.launch のthrottled_rateパラメータを変更して tmux.sh を立ち上げ直してください

[email protected] に入って、roscd jsk_spot_startup で移動して ./launch/include/peripheral.launch をvimとかemacsで編集

[keitayoneda]

@sktometometo

throttled_rateを2.0等の大きい値にすると画像のようにrosbag_record.launchを立ち上げたときに process has diedとなってしまうのですがどうすればいいでしょうか？

[keitayoneda]

throttled_rate=1でも起きてました。発生条件がthrottled_rateというわけではなさそうです

[sktometometo]

@keitayoneda

https://qiita.com/Shunmo17/items/3382e9b9dffb93806cba
みたいにreindexしてみるとどうかな

[keitayoneda]

原因はrosbagの引数にrosbag=~/{path of bagfile}と書いていたのが原因でした、しょうもないミスで申し訳ないです
~を$HOMEにしたところちゃんと動きました

[sktometometo]

どんなバグ報告も誰かのためになりうるという意味で、githubのissueに上げるのにしょうもないものは無いのでどんどん書きましょう

[keitayoneda]

あと今後の方針について少し相談したいです。
現状の目標はspotの人追従をなめらかにしたいというもので、問題点は

• 遠ざかると認識できなくなってしまう
• 周囲に複数の人がいるときに目標の人からトラッキングが外れてしまう

というものでした。
昨日はdepth等の情報を使ってカメラで認識できない情報を取り込めたらいいということでspotのdepthから色付き点群を出せるようにしていただいたのですが、今日見た感想だと、人以外にも床や壁、その他置かれているものなどいわゆる不要な点群も含まれていてそれらから人を区別するのはかなり難易度が高いのではないかと感じました。
そこで点群ではなく、カメラ画像のみから人の位置推定を行う方針に切り替えようと思うのですがどうでしょうか？今のcoral_usbに入っているモデルを改良するか、新しいモデルに差し替えることで認識できる距離やトラッキング精度をあげることに取り組むほうがいいのではないかと考えています

[sktometometo]

解像度と画角の話をのぞけば、パノラマとボディカメラの違いはRGBの3チャンネルかグレースケール+距離画像の2チャンネルみたいな感じのはずで、人の部分を抽出する問題そのものが本質的に変わる気はちょっとしないけど、画角と解像度とサンプリングレート的には有利だと思うのでその面で使うのはありだと思います。

ただトラッキングの性能突き詰めるとそれこそ自動運転の人たちが使ってるであろうLidarベースのトラッキングアルゴリズム持ってきてファインチューンするのと質的に変わらない話だし、ただトラッキングするじゃなくて人との伴走とか人と一緒にずっと活動するロボットならではのトラッキングの観点（運動とかの傾向の認識とか意図の推定とか？興味次第でいろいろありそう）を組み合わせて何かできるかみたいな話を取り込んでも面白いかなと思います。

[keitayoneda]

まさにサンプリングレートと解像度のことを考えると画像のほうが時間あたりに得られる情報量が多く、扱いやすいのかなと思いました。
確かにトラッキングについてはいろいろ方向性がありそうですよね...
もう少し考えてみます。

[sktometometo]

研究室でロボットをなるべくすぐ動かせるように整備してるのはちょっとした実験がすぐにできるようにするためなので、どんなことでもちょっと試したくなったら是非ロボット動かしてみてください。

実際にロボット動かしながらじゃないとわからないこととかアイディアもあると思うので。

[k-okada]

取ったbagファイルやそこから作成した動画を張り付けることが出来るかな．
ー　近いところから遠いところまで追いかけたい人がいたときに，それぞれのカメラでどれぐらい見えているか．
が確認できると良いと思います．また，各カメラでオリジナルでどれぐらいの解像度でrosbagに入っているのはどれぐらいの解像度，というのは確認しましょう．
あと，https://support.bostondynamics.com/s/article/Moving-object-detection の機能を確認したくて，今の7階にあるSpotでLidarは治ったかな，それで見えるのか，8階のSpotのセットアップでないと有効になっていないのか．いずれもこれで，どこまで遠くに行って見つかるか，人込みののなかを歩くとどうなるか？を見てみたいです．

[keitayoneda]

@sktometometo

1. yoneda@StrelKa を作成、~/catkin_ws で spot-ros と jsk_robot をおいてコンパイル
2. ~/spot_driver_bringup.launch を作戦し、これを立ち上げて最低機能が動くことを確認
3. spot_driver/src/spot_driver の spot_wrapper.py / sport_ros.py あたりを見れば良いこと理解した

目標1) spot_driver でget_world_objects.py と同じような結果がprintされるようにする
目標2) 上記結果を（おそらく）TFでpublish できるようにする

[keitayoneda]

起動するとこんな感じでtf2のエラーが出るのですが、どうすればいでしょうか？

[keitayoneda]

printするところまではできました
下がdiffです

diff --git a/spot_driver/src/spot_driver/spot_ros.py b/spot_driver/src/spot_driver/spot_ros.py
index d097858..4ca2fd8 100644
--- a/spot_driver/src/spot_driver/spot_ros.py
+++ b/spot_driver/src/spot_driver/spot_ros.py
@@ -64,6 +64,7 @@ class SpotROS():
         self.callbacks["rear_image"] = self.RearImageCB
         self.callbacks["gripper_image"] = self.GripperImageCB
         self.callbacks["lidar_points"] = self.LidarPointCloudCB
+        self.callbacks["world_object"] = self.WorldObjectCB
 
     def RobotStateCB(self, results):
         """Callback for when the Spot Wrapper gets new robot state data.
@@ -127,6 +128,9 @@ class SpotROS():
             behavior_fault_state_msg = getBehaviorFaultsFromState(state, self.spot_wrapper)
             self.behavior_faults_pub.publish(behavior_fault_state_msg)
 
+
+
+
     def MetricsCB(self, results):
         """Callback for when the Spot Wrapper gets new metrics data.
 
@@ -280,6 +284,17 @@ class SpotROS():
             point_cloud_msg = GetLidarPointCloudMsg(data[0], self.spot_wrapper)
             self.lidar_point_cloud_pub.publish(point_cloud_msg)
             self.populate_lidar_static_transforms(data[0])
+            
+    def WorldObjectCB(self, results):
+        """Callback for when the Spot Wrapper gets new world_object data.
+
+        Args:
+            results: FutureWrapper object of AsyncPeriodicQuery callback
+        """
+        del results
+        data = self.spot_wrapper.world_object
+        print(data)
+        
 
     def handle_claim(self, req):
         """ROS service handler for the claim service"""
@@ -701,6 +716,7 @@ class SpotROS():
         rate = rospy.Rate(50)
 
         self.rates = rospy.get_param('~rates', {})
+        self.rates["world_object"] = 1.0
         self.username = rospy.get_param('~username', 'default_value')
         self.password = rospy.get_param('~password', 'default_value')
         self.hostname = rospy.get_param('~hostname', 'default_value')
diff --git a/spot_driver/src/spot_driver/spot_wrapper.py b/spot_driver/src/spot_driver/spot_wrapper.py
index 84e8a3f..091d064 100644
--- a/spot_driver/src/spot_driver/spot_wrapper.py
+++ b/spot_driver/src/spot_driver/spot_wrapper.py
@@ -9,6 +9,7 @@ from bosdyn.geometry import EulerZXY
 from bosdyn import geometry
 
 from bosdyn.client.robot_state import RobotStateClient
+from bosdyn.client.world_object import WorldObjectClient
 from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder
 from bosdyn.client.graph_nav import GraphNavClient
 from bosdyn.client.recording import GraphNavRecordingServiceClient
@@ -98,6 +99,28 @@ class AsyncRobotState(AsyncPeriodicQuery):
             callback_future.add_done_callback(self._callback)
             return callback_future
 
+class AsyncWorldObject(AsyncPeriodicQuery):
+    """Class to get world object at regular intervals.  get_world_object_async query sent to the robot at every tick.  Callback registered to defined callback function.
+
+        Attributes:
+            client: The Client to a service on the robot
+            logger: Logger object
+            rate: Rate (Hz) to trigger the query
+            callback: Callback function to call when the results of the query are available
+    """
+    def __init__(self, client, logger, rate, callback):
+        super(AsyncWorldObject, self).__init__("world_object", client, logger,
+                                           period_sec=1.0/max(rate, 1.0))
+        self._callback = None
+        if rate > 0.0:
+            self._callback = callback
+
+    def _start_query(self):
+        if self._callback:
+            callback_future = self._client.list_world_objects_async()
+            callback_future.add_done_callback(self._callback)
+            return callback_future
+
 class AsyncMetrics(AsyncPeriodicQuery):
     """Class to get robot metrics at regular intervals.  get_robot_metrics_async query sent to the robot at every tick.  Callback registered to defined callback function.
 
@@ -361,6 +384,7 @@ class SpotWrapper():
             # Clients
             try:
                 self._robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name)
+                self._world_object_client = self._robot.ensure_client(WorldObjectClient.default_service_name)
                 self._robot_command_client = self._robot.ensure_client(RobotCommandClient.default_service_name)
                 self._graph_nav_client = self._robot.ensure_client(GraphNavClient.default_service_name)
                 self._power_client = self._robot.ensure_client(PowerClient.default_service_name)
@@ -421,11 +445,14 @@ class SpotWrapper():
                     self._point_cloud_requests)
             self._idle_task = AsyncIdle(self._robot_command_client,
                     self._logger, 10.0, self)
+            self._world_object_task = AsyncWorldObject(self._world_object_client,
+                    self._logger, max(0.0, self._rates.get("world_object",
+                        0.0)), self._callbacks.get("world_object", lambda:None))
 
             async_task_list = [self._robot_state_task,
                     self._robot_metrics_task, self._lease_task,
                     self._front_image_task, self._side_image_task,
-                    self._rear_image_task, self._point_cloud_task, self._idle_task]
+                    self._rear_image_task, self._point_cloud_task, self._idle_task, self._world_object_task]
 
             if self._robot.has_arm:
                 rospy.loginfo("Robot has arm so adding gripper async camera task")
@@ -467,6 +494,12 @@ class SpotWrapper():
         """Return latest proto from the _robot_state_task"""
         return self._robot_state_task.proto
 
+    @property
+    def world_object(self):
+        """Return latest proto from the _world_object_task"""
+        print("in property world_object")
+        return self._world_object_task.proto
+
     @property
     def metrics(self):
         """Return latest proto from the _robot_metrics_task"""

[keitayoneda]

今日は用事があるのでここで終わりますが、次はproto内にどんなデータが入っているかを見て、ros_msgにどういう形で出すか考えるところをやろうと思います

[keitayoneda]

https://github.com/boston-dynamics/spot-sdk/blob/d5e68145ac936495b2787e09b00b948e453b1ef2/protos/bosdyn/api/world_object.proto#L47
おそらくここにframeの情報は入っていそうというところまで調べました

[keitayoneda]

tf出せました！

[k-okada]

素晴らしい．ぜひ動画にもしてみましょう．こういう時にどういうシーンにしたらよいか，というのを考えるのが，将来とっても大切です．

• DockとかB2のドアのマーカが見えている状態で，ロボットがドックから離れて，廊下にでて５ｍぐらい歩く
• その間，ロボットの横？後ろを一緒に歩く人と，廊下では向かいのかなり遠くからジョギングして走ってすれ違う人がいる
• これのＴＦだけ表示して（ロボットのＴＦは消してよい），上のシーンが想像できれば，認識機能として使い物になりそう．

[sktometometo]

sktometometo/spot_ros-arm#2
上記の @keitayoneda 君の変更