contact_detectionノードにおいてbring_in, take_outが出力されない場合がある

[koukemo]

bring_in, take_outの判定を行っているobject_detectionノードと
stayの判定を行っているobject_trackingノードではそれぞれ正しくイベント検知が行われているのにも関わらず、
contact_detectionノードではstayを変換したtouchのみしか出力しない場合がある。

[koukemo]

画像のように bring_in, take_out時の情報をcontact_detectionにてpublishできていない場合がある。

Message Filters->TimeSynchronizerの影響？

• bring_in, take_outが発生したタイミングでcallbackが呼ばれていない

[koukemo]

左から

• shigure/object_detectionのbring_in, take_outを含む箇所のros2 topic echoの結果
• shigure/object_trackingのbring_in, take_outを含む箇所のros2 topic echoの結果
• shigure/contact_detectionのactionを含む箇所のros2 topic echoの結果
• shigure/contact_detectionのbring_in, take_outを含む箇所のros2 topic echoの結果

で1, 2番目の端末でbring_inと検出されているタイムスタンプであってもcontact_detectionではtouchに書き換わってしまっている。

[koukemo]

原因

node_contact_detection.py 35-37

self.time_synchronizer = message_filters.TimeSynchronizer(
    [object_subscriber, people_subscriber, color_subscriber, depth_camera_info_subscriber], 1500)
self.time_synchronizer.registerCallback(self.callback)

にてTimesynchronizerを用いて、4つのtopicをsubscribeしており、
各topicでのフレーム飛びとTimesynchronizer自身の4つのtopicを受け取らないとcallbackが処理されない
また、action=bring_in, action=take_out時の情報を利用してほしいが、その前後フレームの情報を利用してしまっていることが原因と考えられる。

対処

https://qiita.com/srs/items/5c2f600737abb71bb126#3%E7%89%87%E5%81%B4%E3%82%A4%E3%83%99%E3%83%B3%E3%83%88%E9%A7%86%E5%8B%95%E5%9E%8B

上記に習い、object_detectionを中心とした片側イベント駆動型に変更することでこの問題が解決する可能性がある。
また、これはcontact_detection以外のノードでもTimesynchronizerを使う理由のない場合は、それぞれのノードでsubscribeするtopicの優先順位をつけ、片側イベント駆動型で再実装することが望まれる。

[koukemo]

原因2

上記の原因に加えて、物体と人物の接触ロジックに問題があることを確認した。
接触判定は下記のresult, volume = object_cube.is_collided(hand_cube)部分に該当する。
ここでは、openposeの人物の両手首に当たる位置を囲ったBounding Boxと物体を囲ったBounding Boxが接触しているか判定している。
この時、bring-in, take-outの出力が1フレーム分であることに加えて、物体、手の両方が移動しているため
resultがfalseとなり、上記のbring-in, take-outフレームのみ接触判定がなされなかったということになる。
また、指先のみで物体を把持しているような場合も同様に接触があったとみなされない。
結果として、linked_listへ人物、物体情報が追加されず、bring-in, take-outのフレームだけが飛んだように見える。
(touchのみ連続で検出できているのは、物体と手の位置関係も安定していることが多いためであると考えられる)

contact_detection/logic.py 57-74

        linked_list = []
        object_cube: Cube
        hand_cube: Cube
        for object_item in object_cube_list:
            _, object_cube = object_item
            for hand in hand_cube_list:
                _, hand_cube, _ = hand
                result, volume = object_cube.is_collided(hand_cube)
                if result:
                    linked_list.append((hand, object_item, volume))

        result_list = []
        linked_list = sorted(linked_list, key=itemgetter(2), reverse=True)
        for hand, object_item, _ in linked_list:
            if hand in hand_cube_list and object_item in object_cube_list:
                hand_cube_list.remove(hand)
                object_cube_list.remove(object_item)
                result_list.append((hand, object_item))

対処

pull request : #65

下記のようにbring-in, take-out時のみ、linked_listへappendする直前の判定を緩めることで回避

        linked_list = []
        object_cube: Cube
        hand_cube: Cube
        for object_item in object_cube_list:
            tracked_object_info, object_cube = object_item
            result = False
            is_bring_in_or_take_out = tracked_object_info.action == TrackedObjectActionEnum.BRING_IN.value or tracked_object_info.action == TrackedObjectActionEnum.TAKE_OUT.value
            for hand in hand_cube_list:
                _, hand_cube, _ = hand
                result, volume = object_cube.is_collided(hand_cube)
                if result | is_bring_in_or_take_out:
                    linked_list.append((hand, object_item, volume))
            if (not result) & is_bring_in_or_take_out:
                    print("Note : The system did not detect contact between the wrist and the object, but prioritized the determination of whether the object was brought in or taken away.")

        result_list = []
        linked_list = sorted(linked_list, key=itemgetter(2), reverse=True)
        for hand, object_item, _ in linked_list:
            if hand in hand_cube_list and object_item in object_cube_list:
                hand_cube_list.remove(hand)
                object_cube_list.remove(object_item)
                result_list.append((hand, object_item))

接触判定は確認していないが、bring-in, take-outのみを判断基準にした場合はNote: ...の文言を出力するように設定した。