[PR1040] Azure Kinect で AprilTag を使おうとすると synchronize されていないというエラーが出て使用できない。

sktometometo commented 7 months ago

PR1040のAzure KinectでAprilTagを使用しようとすると、以下のようなエラーが表示されて使用できなくなります

CC: @HiroIshida

エラー

[ WARN] [1701077351.860531721] [/apriltag_pose_detector:ros.image_transport.sync]: [image_transport] Topics '/k4a/rgb/image_rect_color' and '/k4a/rgb/camera_info' do not appear to be synchronized. In the last 10s:
        Image messages received:      17
        CameraInfo messages received: 211
        Synchronized pairs:           0

Full log

applications@pr1040:~/shinjo_ws$ roslaunch test_apriltag_ros april_detection.launch 
... logging to /home/applications/.ros/log/20231127-163840_fc1c78e0-8cf7-11ee-88e0-950136cc82ed/roslaunch-pr1040-89571.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/applications/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://pr1040:33273/

SUMMARY
========

PARAMETERS
 * /apriltag_pose_detector/max_hamming_dist: 2
 * /apriltag_pose_detector/publish_tf: True
 * /apriltag_pose_detector/standalone_tags: [{'id': 0, 'size'...
 * /apriltag_pose_detector/tag_blur: 0.0
 * /apriltag_pose_detector/tag_debug: 0
 * /apriltag_pose_detector/tag_decimate: 1.0
 * /apriltag_pose_detector/tag_family: tagStandard41h12
 * /apriltag_pose_detector/tag_refine_edges: 1
 * /apriltag_pose_detector/tag_threads: 2
 * /apriltag_pose_detector/transport_hint: raw
 * /rosdistro: noetic
 * /rosversion: 1.15.15

NODES
  /
    apriltag_pose_detector (apriltag_ros/apriltag_ros_continuous_node)

ROS_MASTER_URI=http://pr1040:11311

process[apriltag_pose_detector-1]: started with pid [89608]
[ WARN] [1701077341.749180223] [/apriltag_pose_detector:ros.apriltag_ros]: No tag bundles specified
[ WARN] [1701077341.749410110] [/apriltag_pose_detector:ros.apriltag_ros]: remove_duplicates parameter not provided. Defaulting to true
[ WARN] [1701077351.860531721] [/apriltag_pose_detector:ros.image_transport.sync]: [image_transport] Topics '/k4a/rgb/image_rect_color' and '/k4a/rgb/camera_info' do not appear to be synchronized. In the last 10s:
        Image messages received:      17
        CameraInfo messages received: 211
        Synchronized pairs:           0

実行環境

PC: PR1040体内(C1)
ワークスペース: /home/applications/shinjo_ws
実行ファイル群

launch

<launch>

   <node
        pkg="apriltag_ros"
        type="apriltag_ros_continuous_node"
        name="apriltag_pose_detector"
        >
        <remap from="image_rect" to="/k4a/rgb/image_rect_color" />
        <remap from="camera_info" to="/k4a/rgb/camera_info" />
        <rosparam command="load" file="$(find test_apriltag_ros)/setting.yaml" />
        <rosparam command="load" file="$(find test_apriltag_ros)/tags.yaml" />

    </node>
</launch>

settings.yaml

tag_family: "tagStandard41h12" # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
tag_threads: 2 # default: 2
tag_decimate: 1.0 # default: 1.0
tag_blur: 0.0 # default: 0.0
tag_refine_edges: 1 # default: 1
tag_debug: 0 # default: 0
max_hamming_dist: 2 # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.)
# Other parameters
publish_tf: true # default: false
transport_hint: "raw"

tags.yaml

standalone_tags:
  [
    {id: 0, size: 0.0215, name: object},
  ]

インストールされているapriltag-rosのバージョン

applications@pr1040:~/shinjo_ws/src/test_apriltag_ros$ apt show ros-noetic-apriltag-ros
Package: ros-noetic-apriltag-ros
Version: 3.2.1-3focal.20231013.192954
Priority: optional
Section: misc
Maintainer: Danylo Malyuta <danylo.malyuta@gmail.com>
Installed-Size: 1,078 kB
Depends: libc6 (>= 2.14), libconsole-bridge0.4, libgcc-s1 (>= 3.0), libopencv-calib3d4.2 (>= 4.2.0+dfsg), libopencv-core4.2 (>= 4.2.0+dfsg), libopencv-imgcodecs4.2 (>= 4.2.0+dfsg), libopencv-imgproc4.2 (>= 4.2.0+dfsg), libstdc++6 (>= 9), ros-noetic-apriltag, libeigen3-dev, libopencv-dev, ros-noetic-cv-bridge, ros-noetic-geometry-msgs, ros-noetic-image-geometry, ros-noetic-image-transport, ros-noetic-message-runtime, ros-noetic-nodelet, ros-noetic-pluginlib, ros-noetic-roscpp, ros-noetic-sensor-msgs, ros-noetic-std-msgs, ros-noetic-tf
Homepage: http://www.ros.org/wiki/apriltag_ros
Download-Size: 174 kB
APT-Manual-Installed: yes
APT-Sources: http://packages.ros.org/ros/ubuntu focal/main amd64 Packages
Description: A ROS wrapper of the AprilTag 3 visual fiducial detection algorithm.
 Provides full access to the core AprilTag 3 algorithm's customizations and makes the tag detection image and detected tags' poses available over ROS topics (including tf). The core AprilTag 3 algorithm is extended to allow the detection of tag bundles and a bundle calibration script is provided (bundle detection is more accurate than single tag detection). Continuous (camera image stream) and single image detector nodes are available.

インストールされているimage_transportのバージョン

applications@pr1040:~/shinjo_ws/src/test_apriltag_ros$ apt show ros-noetic-image-transport
Package: ros-noetic-image-transport
Version: 1.12.0-1focal.20230620.191833
Priority: optional
Section: misc
Maintainer: Jack O'Quin <jack.oquin@gmail.com>
Installed-Size: 1,428 kB
Depends: libboost-filesystem1.71.0, libc6 (>= 2.14), libconsole-bridge0.4, libgcc-s1 (>= 3.0), libstdc++6 (>= 5.2), libtinyxml2-6a (>= 5.0.0), ros-noetic-message-filters, ros-noetic-pluginlib, ros-noetic-rosconsole, ros-noetic-roscpp, ros-noetic-roslib, ros-noetic-sensor-msgs
Homepage: http://ros.org/wiki/image_transport
Download-Size: 248 kB
APT-Sources: http://packages.ros.org/ros/ubuntu focal/main amd64 Packages
Description: image_transport should always be used to subscribe to and publish images.
 It provides transparent support for transporting images in low-bandwidth compressed formats. Examples (provided by separate plugin packages) include JPEG/PNG compression and Theora streaming video.

N: There is 1 additional record. Please use the '-a' switch to see it

sktometometo commented 7 months ago

From posts below

This will happens when

apriltag_ros node computer and camera computer are different
Image size is large, rate is high. So camera info message (which is very small and can be subscribed with high frequency from remote computer) buffer for synchonization will be full before image message (which is very large and can be subscribed only with very low frequency from remote computer) reached.

And our Kinect azure runs on c3 which is different from c1. This issue may be caused from this reason.

Below is subscribed rate for message and camera info from C1

applications@pr1040:~$ rostopic hz /k4a/rgb/camera_info
subscribed to [/k4a/rgb/camera_info]
no new messages
average rate: 29.960
        min: 0.029s max: 0.039s std dev: 0.00255s window: 21
average rate: 29.969
        min: 0.028s max: 0.039s std dev: 0.00249s window: 51
^Caverage rate: 30.035
        min: 0.028s max: 0.039s std dev: 0.00243s window: 80
applications@pr1040:~$ rostopic hz /k4a/rgb/image_rect_color
subscribed to [/k4a/rgb/image_rect_color]
no new messages
no new messages
no new messages
average rate: 4.371
        min: 0.229s max: 0.229s std dev: 0.00000s window: 2
average rate: 2.600
        min: 0.221s max: 0.668s std dev: 0.18206s window: 5
average rate: 2.247
        min: 0.221s max: 0.687s std dev: 0.20284s window: 6
^Caverage rate: 2.407
        min: 0.215s max: 0.687s std dev: 0.17736s window: 9

sktometometo commented 7 months ago

解決方策案

c3でapriltag走らせる => 試した。同様のエラーが発生
rawではなくcompressedで ( https://github.com/AprilRobotics/apriltag_ros/pull/108 ) => C1で試したが同様のエラーが発生
throttled する

sktometometo commented 7 months ago

https://github.com/AprilRobotics/apriltag_ros/blob/94b3b843324f6b64edc6ca59f9729e9021308406/apriltag_ros/src/continuous_detector.cpp#L54C1-L59C76

CameraSubscriberのqueue_size = TimeSynchronizerのqueue_sizeのデフォルトが1なのが問題かも

c.f. https://github.com/ros-perception/image_common/blob/cd57529952b65d5180ff27780a0370ab7504e64d/image_transport/src/camera_subscriber.cpp#L100-L122

k-okada commented 7 months ago

this is interesting, did you increase queue_size ? https://github.com/AprilRobotics/apriltag_ros/blob/94b3b843324f6b64edc6ca59f9729e9021308406/apriltag_ros/src/continuous_detector.cpp#L55 ( or other code)

◉ Kei Okada

2023年11月27日(月) 18:43 Koki Shinjo @.***>:

解決方策案

c3でapriltag走らせる => 試した。同様のエラーが発生

rawではなくcompressedで ( AprilRobotics/apriltag_ros#108 https://github.com/AprilRobotics/apriltag_ros/pull/108 ) => C1で試したが同様のエラーが発生

throttled する

— Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/1891#issuecomment-1827480945, or unsubscribe https://github.com/notifications/unsubscribe-auth/AADYNXGYVATHIE54BFTTJKTYGROCPAVCNFSM6AAAAAA73WVXI6VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMYTQMRXGQ4DAOJUGU . You are receiving this because you are subscribed to this thread.Message ID: @.***>

sktometometo commented 7 months ago

(Pythonですが) TimeSynchronizerのqueue_sizeを変えるとsubscribeされるかどうかが変わりました。

#!/usr/bin/env python

import rospy
import argparse
import message_filters
from sensor_msgs.msg import Image, CameraInfo

def callback(image, camera_info):
    print("=====================")
    print(f"image.header: {image.header}")
    print(f"camera_info.header: {camera_info.header}")

if __name__ == "__main__":

    parser = argparse.ArgumentParser()
    parser.add_argument("queue_size", type=int)
    args = parser.parse_args()

    rospy.init_node("test_synchronization")

    image_sub = message_filters.Subscriber('/k4a/rgb/image_rect_color', Image)
    info_sub = message_filters.Subscriber('/k4a/rgb/camera_info', CameraInfo)

    ts = message_filters.TimeSynchronizer([image_sub, info_sub], args.queue_size)
    ts.registerCallback(callback)
    rospy.spin()

queue_size=1

applications@pr1040:~/shinjo_ws/src/test_apriltag_ros$ python test_synchronization.py 1
<何も出力なし>

queue_size=30

pplications@pr1040:~/shinjo_ws/src/test_apriltag_ros$ python test_synchronization.py 30
=====================
image.header: seq: 4929
stamp: 
  secs: 1701079093
  nsecs: 439512209
frame_id: "rgb_camera_link"
camera_info.header: seq: 57731
stamp: 
  secs: 1701079093
  nsecs: 439512209
frame_id: "rgb_camera_link"
=====================
image.header: seq: 4932
stamp: 
  secs: 1701079093
  nsecs: 639967217
frame_id: "rgb_camera_link"
camera_info.header: seq: 57737
stamp: 
  secs: 1701079093
  nsecs: 639967217
frame_id: "rgb_camera_link"
=====================
image.header: seq: 4939
stamp: 
  secs: 1701079094
  nsecs: 172930532
frame_id: "rgb_camera_link"
camera_info.header: seq: 57753
stamp: 
  secs: 1701079094
  nsecs: 172930532
frame_id: "rgb_camera_link"

sktometometo commented 7 months ago

compressed + queue_size=300 設定でもエラーが出る。反映されているか不明

# Other parameters
publish_tf: true # default: false
transport_hint: "compressed"
queue_size: 300

NODES
  /
    apriltag_pose_detector (apriltag_ros/apriltag_ros_continuous_node)

ROS_MASTER_URI=http://pr1040:11311

process[apriltag_pose_detector-1]: started with pid [116216]
[ WARN] [1701079394.532434332] [/apriltag_pose_detector:ros.apriltag_ros]: No tag bundles specified
[ WARN] [1701079394.532691752] [/apriltag_pose_detector:ros.apriltag_ros]: remove_duplicates parameter not provided. Defaulting to true
[ WARN] [1701079404.649039892] [/apriltag_pose_detector:ros.image_transport.sync]: [image_transport] Topics '/k4a/rgb/image_rect_color/compressed' and '/k4a/rgb/camera_info' do not appear to be synchronized. In the last 10s:
        Image messages received:      59
        CameraInfo messages received: 212
        Synchronized pairs:           0
[ WARN] [1701079414.660945642] [/apriltag_pose_detector:ros.image_transport.sync]: [image_transport] Topics '/k4a/rgb/image_rect_color/compressed' and '/k4a/rgb/camera_info' do not appear to be synchronized. In the last 10s:
        Image messages received:      114
        CameraInfo messages received: 296
        Synchronized pairs:           0
[ WARN] [1701079424.648984749] [/apriltag_pose_detector:ros.image_transport.sync]: [image_transport] Topics '/k4a/rgb/image_rect_color/compressed' and '/k4a/rgb/camera_info' do not appear to be synchronized. In the last 10s:
        Image messages received:      118
        CameraInfo messages received: 298
        Synchronized pairs:           0

sktometometo commented 7 months ago

queue_size オプション ( https://github.com/AprilRobotics/apriltag_ros/commit/e4ad5f9c178d1e81cfa97d44299ceb7b5e816034 ) は 3.2.1 のリリースには含まれていないためのようです。

Screenshot from 2023-11-27 19-06-49

sktometometo commented 7 months ago

compressed + queue_size=300 設定 + master (> 3.2.1) でsynchronized_pairs > 0 と /tag_detections へのpublishを確認

applications@pr1040:~/shinjo_ws/src$ roslaunch test_apriltag_ros april_detection.launch 
... logging to /home/applications/.ros/log/20231127-163840_fc1c78e0-8cf7-11ee-88e0-950136cc82ed/roslaunch-pr1040-120936.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/applications/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://pr1040:37407/

SUMMARY
========

PARAMETERS
 * /apriltag_pose_detector/max_hamming_dist: 2
 * /apriltag_pose_detector/publish_tf: True
 * /apriltag_pose_detector/queue_size: 300
 * /apriltag_pose_detector/standalone_tags: [{'id': 0, 'size'...
 * /apriltag_pose_detector/tag_blur: 0.0
 * /apriltag_pose_detector/tag_debug: 0
 * /apriltag_pose_detector/tag_decimate: 1.0
 * /apriltag_pose_detector/tag_family: tagStandard41h12
 * /apriltag_pose_detector/tag_refine_edges: 1
 * /apriltag_pose_detector/tag_threads: 2
 * /apriltag_pose_detector/transport_hint: compressed
 * /rosdistro: noetic
 * /rosversion: 1.15.15

NODES
  /
    apriltag_pose_detector (apriltag_ros/apriltag_ros_continuous_node)

ROS_MASTER_URI=http://pr1040:11311

process[apriltag_pose_detector-1]: started with pid [120956]
[ WARN] [1701079712.153282424] [/apriltag_pose_detector:ros.apriltag_ros]: No tag bundles specified
[ WARN] [1701079712.153539258] [/apriltag_pose_detector:ros.apriltag_ros]: remove_duplicates parameter not provided. Defaulting to true
[ WARN] [1701079725.989215532] [/apriltag_pose_detector:ros.image_transport.sync]: [image_transport] Topics '/k4a/rgb/image_rect_color/compressed' and '/k4a/rgb/camera_info' do not appear to be synchronized. In the last 10s:
        Image messages received:      68
        CameraInfo messages received: 216
        Synchronized pairs:           68
[ WARN] [1701079755.582701613] [/apriltag_pose_detector:ros.image_transport.sync]: [image_transport] Topics '/k4a/rgb/image_rect_color/compressed' and '/k4a/rgb/camera_info' do not appear to be synchronized. In the last 10s:
        Image messages received:      124
        CameraInfo messages received: 297
        Synchronized pairs:           91

sktometometo commented 7 months ago

@HiroIshida 本件についての解決策ですが、

apriltag_ros の master バージョンを使う
queue_size パラメータを増やす
( transport_hint を compressed に )
( C3と高速・大容量の通信の出来るコンピュータ上で apriltag_ros の continuous_detector を実行する)

で解決するはずです。

起きていた現象のまとめですが、

apriltag_ros が画像とCameraInfoの取得(サブスクライブ)に使用している、image_transport パッケージの image_transport::CameraSuscriber クラスでは、message_filters の TimeSynchronizer を使用しているのですが、これはバッファに溜め込んだ各トピックのメッセージにタイムスタンプが同一のものがあるかを探すため、ネットワーク越しになっていることによる遅延のためにメッセージ間でのsubscribe rate に差がでたときにバッファーが十分に大きくないと、low frequencyでsubscribeされるトピックのメッセージが届く前に、high frequencyでsubscribeできるトピックのバッファーが溢れて古いメッセージが押し出されてしまうと synchronizationが取れないというものでした。

sktometometo commented 7 months ago

今回はsubscribeする側でどうにかしていましたが、一般的にはpublishする側のthrottleを落としてあげた方が扱いやすそう。synchonizationを保ったまま throttle する方法はある？

sktometometo commented 7 months ago

今回はsubscribeする側でどうにかしていましたが、一般的にはpublishする側のthrottleを落としてあげた方が扱いやすそう。synchonizationを保ったまま throttle する方法はある？

https://github.com/jsk-ros-pkg/jsk_common/blob/master/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp from @iory

jsk-ros-pkg / jsk_robot

[PR1040] Azure Kinect で AprilTag を使おうとすると synchronize されていないというエラーが出て使用できない。 #1891

エラー

実行環境

this is interesting, did you increase queue_size ? https://github.com/AprilRobotics/apriltag_ros/blob/94b3b843324f6b64edc6ca59f9729e9021308406/apriltag_ros/src/continuous_detector.cpp#L55 ( or other code)