[elevator_move_base_fetch?] エレベータの階表示パネルをよみたい

knorth55 commented 3 years ago

@mqcmd196 @708yamaguchi @knorth55

PR2のサブウェイデモ（最近試してるやつ）： #1321

階表示パネルの文字を読む

階表示パネルを認識してTF `/elevator_inside_panel`を出す認識コード

https://github.com/knorth55/jsk_demos/blob/forum_button/elevator_move_base_pr2/launch/elevator_move_base_modules.xml#L10-L33

passthroughで`/narrow_stereo/left/pass_through_feature_to_inside_panel`を通すようにする．

https://github.com/knorth55/jsk_demos/blob/forum_button/elevator_move_base_pr2/launch/elevator_panels_detection_eng2.launch#L39-L70

rosservice call /narrow_stereo/left/pass_through_feature_to_inside_panel/request

階表示パネルの文字を読むコード（`/elevator_inside_panel`のtfが必要）

https://github.com/knorth55/jsk_demos/blob/forum_button/elevator_move_base_pr2/launch/elevator_move_base_modules.xml#L66-L93

passthroughで`/narrow_stereo/left/pass_through_image_rect`を通す

https://github.com/knorth55/jsk_demos/blob/forum_button/elevator_move_base_pr2/launch/elevator_move_base_modules.xml#L67-L78

rosservice call /narrow_stereo/left/pass_through_image_rect/request

`/panel_camera/image`を確認する

rostopic hz /panel_camera/image

`/elevator_number/result`を確認する

https://github.com/jsk-ros-pkg/jsk_demos/blob/master/elevator_move_base_pr2/launch/elevator_move_base_modules.xml#L27-L30

rostopic echo /elevator_number/result

エレベータのドアが空いているかを認識する

`check_elevator_open.xml`を立ち上げる．

https://github.com/knorth55/jsk_demos/blob/forum_button/elevator_move_base_pr2/launch/check_elevator_open.xml https://jsk-recognition.readthedocs.io/en/latest/jsk_pcl_ros/nodes/octree_change_detector.html

`eus`でcheckする

https://github.com/knorth55/jsk_demos/blob/forum_button/elevator_move_base_pr2/src/state/check-elevator-open.l

手元でPR2のコードを試す

rosbagでためす

https://github.com/knorth55/jsk_demos/blob/forum_button/elevator_move_base_pr2/test/test-modules-insidepanel.launch

roslaunch elevator_move_base_pr2 test-modules-insidepanel.launch

rvizをだす．

https://github.com/knorth55/jsk_demos/blob/forum_button/elevator_move_base_pr2/config/door_button_segmentation_3d.rviz

rviz -d $(find elevator_move_pase_pr2)/config/door_button_segmentation_3d.rviz

knorth55 commented 3 years ago

次やるべきこと：

xtionをもってエレベータにのってrosbagをとる
Fetchでエレベータにのって，頭を階表示パネルに回すコードをかく，そのときのRosbagをとっておく

注意点：

PR2とFetchでネームスペースが違うものを対応する

カメラの名前: narrow_stereo, wide_stereo, kinect_head -> head_camera
ベースフレームの名前: base_footprint -> base_link

カメラの画像がモノクロか，カラーか

narrow_stereoはモノクロ，wide_stereo, kinect_headはカラー
fetchは全部カラー

懸念点：

PR2とオドメトリの精度がちがうので，決め打ちでできるかな？

mqcmd196 commented 3 years ago

https://github.com/knorth55/jsk_demos/blob/5b2a3112cdefb1d53ae04ea0e06d345684386f21/elevator_move_base_pr2/launch/elevator_panels_detection_eng2.launch#L24 https://github.com/knorth55/jsk_demos/blob/5b2a3112cdefb1d53ae04ea0e06d345684386f21/elevator_move_base_pr2/launch/elevator_panels_detection_eng2.launch#L50 のtemplateディレクトリが見当たらなかったので，pr1040からscpして手動で置きました．

knorth55 commented 3 years ago

@mqcmd196 jsk_perceptionのここにissueをたてましたが，roseusが入った状態でjsk_perceptionをうまくビルドするとできるようです． https://github.com/jsk-ros-pkg/jsk_recognition/issues/2560

mqcmd196 commented 3 years ago

現時点での進捗です．Cc:@k-okada

状況

エレベータ内のパネルそのものはパターンマッチングで認識に成功している．tfも出ている．
エレベータ内のパネルの階表示ディスプレイは認識ができない．virtual_cameraが出るところまで行く場合と行かない場合があって，行く場合でもパターンマッチングがうまく動作しない．
src以下の環境
jsk_demos: mqcmd196/jsk_demos の fetch_elevator_detectブランチ
jsk_robot: knorth55/jsk_robot の fetch15ブランチ
rosbagファイル
https://drive.google.com/drive/folders/1-OHC69sqhxnFnBmepHvFbjqaNbfv7e9n?usp=sharing の20201217elevator.bag
https://drive.google.com/drive/folders/1gmOHgpOSbfXaviXGf8gBRFmfWAU0di2x?usp=sharing の20201219_elevator_imu.bag
再現
1. rosbagの再生 roslaunch jsk_fetch_startup rosbag_play.launch rosbag:=(rosbagファイル)
2. エレベータ認識ノードのlaunch roslaunch elevator_move_base_fetch elevator_move_base_eng2.launch
3. rvizの表示 roscd elevator_move_base_fetch/config rviz -d elevator.rviz
4. passthroughで認識ノードがsubscribe開始 rosservice call /head_camera/mono/pass_through_image_rect/request rosservice call /head_camera/mono/pass_through_feature_to_inside_panel/request
5. 階認識の結果表示(できていない) rostopic echo /elevator_number/result

k-okada commented 3 years ago

virtual_camera は毎回出ないかな？こちらでは TF_OLD_DATA ignoring data from the past for が出たら一回，elevator_move_base_eng2.launch を起ち上げ直す，という運用をしています．

それが出来たとして， https://github.com/mqcmd196/jsk_demos/pull/2 みたいにして，virtual cameraの位置を直すと上手く認識できそうです．

デバッグ方法ですが，

diff --git a/elevator_move_base_fetch/launch/elevator_move_base_modules.xml b/elevator_move_base_fetch/launch/elevator_move_base_modules.xml
index c1be702d..62e070db 100644
--- a/elevator_move_base_fetch/launch/elevator_move_base_modules.xml
+++ b/elevator_move_base_fetch/launch/elevator_move_base_modules.xml
@@ -59,7 +59,7 @@
     <rosparam command="load"
               file="$(find elevator_move_base_fetch)/launch/template-$(arg scene).yaml"/>
     <rosparam>
-      show_probability: false
+      show_probability: true
       show_image: false
     </ros

として， https://gist.github.com/k-okada/c56f0e41ff9d8fe8be9c0b477d85edc2 を使うと，initial_pos 変更前 Screenshot from 2020-12-22 18-42-00 initial_pos 変更後と成るんですが，ポイントはmatchTemplate をしているのでテンプレートマッチなので両方の画像サイズが同じである必要があります．なので，そうなるように慎重にinitial_pos を調整しました．

ココを頑張る以外に，以下のように少しスケールしてテンプレートマッチしてみたんですが，ちょっと時間かかりすぎるようでした．

ちなみに，virtual_camera_monoでview_pointトピックでこのカメラ位置をセットできますが，PR2でもこれは使っていないかな．

diff --git a/elevator_move_base_fetch/node_scripts/match_template.py b/elevator_move_base_fetch/node_scripts/match_template.py
index 103691a9..a103799b 100755
--- a/elevator_move_base_fetch/node_scripts/match_template.py
+++ b/elevator_move_base_fetch/node_scripts/match_template.py
@@ -99,12 +99,37 @@ class MatchTemplate(ConnectionBasedTransport):
         results = dict()
         for typename, template in sorted(self.templates.items()):
             res = cv2.matchTemplate(img, template.image, template.method)
+            min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res)
+            if template.method == cv2.TM_SQDIFF_NORMED:
+                _score = 1.0
+            else:
+                _score = 0.0
+            for scale in np.arange(0.9, 1.2, 0.01):
+                img_scaled = cv2.resize(img, None, fx=scale, fy=scale)
+                res = cv2.matchTemplate(img_scaled, template.image, template.method)
+                _min_val, _max_val, _min_loc, _max_loc = cv2.minMaxLoc(res)
+                _min_loc = (_min_loc[0]/scale, _min_loc[1]/scale)
+                _max_loc = (_max_loc[0]/scale, _max_loc[1]/scale)
+                if template.method == cv2.TM_SQDIFF_NORMED:
+                    if _min_val < _score:
+                        _score = _min_val
+                        min_val = _min_val
+                        max_val = _max_val
+                        min_loc = _min_loc
+                        max_loc = _max_loc
+                else:
+                    if _max_val > _score:
+                        _score = _max_val
+                        min_val = _min_val
+                        max_val = _max_val
+                        min_loc = _min_loc
+                        max_loc = _max_loc
+                #print("{:.4f} {:.4f} {:.4f}".format(scale, min_val, max_val))

             if self.show_image:
                 cv2.imshow('img', img)
                 cv2.imshow('template'+typename, template.image)
                 cv2.waitKey(1)
-            min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res)

             if template.method == cv2.TM_SQDIFF_NORMED:
                 rospy.loginfo("check {:2s} score={:.4f}, thre={:.4f} > max_val={:.4f}".format(typename, min_val, template.thre, max_val))

mqcmd196 commented 3 years ago

岡田先生のPRで直していただきました．ありがとうございました．

動作しなかった理由

パターンマッチングをするためには，データセット，実際の画像サイズが一致している必要があって，カメラ位置がPR2とfetchで違っていたのでvirtual_cameraのinitial_posで調整してあげる必要があった．これを実現するもう一つの方法は岡田先生のmatch_template.pyの差分のように，このノードでスケールしてあげるという方法．
パブリッシャがパブリッシュする前にサブスクライバが動作を始めてしまうhttps://answers.ros.org/question/141676/ros-subscribe-to-the-latest-available-message/ ので，https://github.com/mqcmd196/jsk_demos/commit/b2d458ea135799aa06306cf54513985a0e58b559 のようにする．

mqcmd196 commented 3 years ago

実機での動作を確認しました

knorth55 commented 3 years ago

うまくいったかな、スクショを共有してくれると嬉しいです

jsk-ros-pkg / jsk_demos