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

knorth55 · 2020-12-15T04:17:36Z

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

The text was updated successfully, but these errors were encountered:

knorth55 · 2020-12-15T04:19:32Z

次やるべきこと：

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 · 2020-12-15T12:36:31Z

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 · 2020-12-15T14:04:17Z

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

mqcmd196 · 2020-12-22T07:08:51Z

現時点での進捗です．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

再現

rosbagの再生
roslaunch jsk_fetch_startup rosbag_play.launch rosbag:=(rosbagファイル)
エレベータ認識ノードのlaunch
roslaunch elevator_move_base_fetch elevator_move_base_eng2.launch
rvizの表示
roscd elevator_move_base_fetch/config
rviz -d elevator.rviz
passthroughで認識ノードがsubscribe開始
rosservice call /head_camera/mono/pass_through_image_rect/request
rosservice call /head_camera/mono/pass_through_feature_to_inside_panel/request
階認識の結果表示(できていない)
rostopic echo /elevator_number/result

k-okada · 2020-12-22T12:04:56Z

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

それが出来たとして，
mqcmd196#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 変更前

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 · 2020-12-27T04:32:17Z

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

動作しなかった理由

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

mqcmd196 · 2021-01-04T07:06:01Z

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

knorth55 · 2021-01-04T09:19:07Z

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

k-okada mentioned this issue Dec 22, 2020

add more debug info for virtual_camera_info jsk-ros-pkg/jsk_recognition#2563

Merged

softyanija mentioned this issue Feb 9, 2021

2020年度ゼミ #1309

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

knorth55 commented Dec 15, 2020 •

edited

Loading

knorth55 commented Dec 15, 2020 •

edited

Loading

mqcmd196 commented Dec 15, 2020 •

edited

Loading

knorth55 commented Dec 15, 2020

mqcmd196 commented Dec 22, 2020 •

edited by k-okada

Loading

k-okada commented Dec 22, 2020

mqcmd196 commented Dec 27, 2020

mqcmd196 commented Jan 4, 2021

knorth55 commented Jan 4, 2021

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

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

Comments

knorth55 commented Dec 15, 2020 • edited Loading

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

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

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

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

passthroughで/narrow_stereo/left/pass_through_image_rectを通す

/panel_camera/imageを確認する

/elevator_number/resultを確認する

eusでcheckする

階表示パネルが光ってるかどうか認識する(optional)

passthroughで/wide_stereo/left/pass_through_image_rect_colorを通す．

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

check_elevator_open.xmlを立ち上げる．

eusでcheckする

手元でPR2のコードを試す

rosbagでためす

rvizをだす．

knorth55 commented Dec 15, 2020 • edited Loading

次やるべきこと：

注意点：

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

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

懸念点：

mqcmd196 commented Dec 15, 2020 • edited Loading

knorth55 commented Dec 15, 2020

mqcmd196 commented Dec 22, 2020 • edited by k-okada Loading

状況

src以下の環境

rosbagファイル

再現

k-okada commented Dec 22, 2020

mqcmd196 commented Dec 27, 2020

動作しなかった理由

mqcmd196 commented Jan 4, 2021

knorth55 commented Jan 4, 2021

knorth55 commented Dec 15, 2020 •

edited

Loading

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

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

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

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

`/panel_camera/image`を確認する

`/elevator_number/result`を確認する

`eus`でcheckする

passthroughで`/wide_stereo/left/pass_through_image_rect_color`を通す．

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

`eus`でcheckする

knorth55 commented Dec 15, 2020 •

edited

Loading

mqcmd196 commented Dec 15, 2020 •

edited

Loading

mqcmd196 commented Dec 22, 2020 •

edited by k-okada

Loading