Camera re-connection logic

[shaun-edwards]

This is partial fix for ros-drivers#36.

This fix addresses any disconnect/reconnect where the camera is recognized by the OpenNI2::DeviceManager (i.e the following message: [ INFO] [1504148886.334244977]: Device "1d27/0601@1/23" found. is posted). Sometimes the USB driver itself fails to recognize the camera. This scenario cannot be recovered using the changes in this fix.

This PR includes the follow "issues":

• Monitoring thread runs continuously, instead of being event driven as suggested in Driver has no re-connection logic ros-drivers/openni2_camera#36 (won't fix)
• Connection checking assumes only a single camera is attached to each bus (instead of comparing camera serial numbers to be certain when a specific camera is re-connected) (add issue)
• Re-connection allowed when auto exposure/white balance disabled. This should only be allowed when the exposure has been set to non-zero. This requires this issue be addressed. (add issue, might fix)

Due to the potential open issues, this PR should only be merged locally.

[130s]

I tested and am seeing the issue ros-drivers#36 is fixed where only a single camera is connected and only a single bus is available for USB devices.

How tested

• With only a single Xtion device knowing this constraint:

  Connection checking assumes only a single camera is attached to each bus

• Plug out USB cable, wait for a few sec, then plug it back in.

• Show an image /camera/rgb/image_raw/compressed on rqt_image_view

• Repeat 10 times.

Test result
I see the camera image reliably resumes after reconnection.

• Taking a few seconds once USB cable is plugged back in.
• When 2 cameras are connected and one camera was dis- and re-connected, this does not do reconnection as expected.

$ roslaunch openni2_launch openni2.launch device_id:=#1
:
process[camera_base_link3-15]: started with pid [4001]
[ INFO] [1504154277.105838125]: Initializing nodelet with 4 worker threads.
[ INFO] [1504154277.655627347]: Device "1d27/0601@1/2" found.
Warning: USB events thread - failed to set priority. This might cause loss of data...
[ WARN] [1504154278.460049805]: Reconnect has been enabled, only one camera should be plugged into each bus
[ INFO] [1504154284.901295087]: Starting color stream.
[ INFO] [1504154285.288624439]: using default calibration URL
[ INFO] [1504154285.288726199]: camera calibration URL: file:///home/rwc1/.ros/camera_info/rgb_PS1080_PrimeSense.yaml
[ INFO] [1504154285.288874594]: Unable to open camera calibration file [/home/rwc1/.ros/camera_info/rgb_PS1080_PrimeSense.yaml]
[ WARN] [1504154285.288919495]: Camera calibration file /home/rwc1/.ros/camera_info/rgb_PS1080_PrimeSense.yaml not found.
[ INFO] [1504154288.124847904]: Stopping color stream.
[ INFO] [1504154290.610023855]: Starting color stream.
[ WARN] [1504154300.442095353]: Device "1d27/0601@1/2" disconnected

[ WARN] [1504154300.442285699]: Device "1d27/0601@1/2" disconnected

[ WARN] [1504154300.460628426]: Detected loss of connection.  Stopping all streams and resetting device
[ INFO] [1504154308.166650662]: Device "1d27/0601@1/4" found.
[ INFO] [1504154308.460920213]: Detected re-connect...attempting reinit
[ INFO] [1504154311.344939411]: Re-applying configuration to camera on re-init
[ INFO] [1504154311.442364929]: Starting color stream to adjust camera
[ INFO] [1504154311.442585343]: Starting color stream.
[ INFO] [1504154311.483732681]: Restarting publishers, if needed
[ INFO] [1504154311.483825457]: Done re-initializing cameras
[ WARN] [1504154517.841611425]: Device "1d27/0601@1/4" disconnected

[ WARN] [1504154517.841724257]: Device "1d27/0601@1/4" disconnected

[ WARN] [1504154518.485106272]: Detected loss of connection.  Stopping all streams and resetting device
[ INFO] [1504154525.598657646]: Device "1d27/0601@1/5" found.
[ INFO] [1504154526.485029939]: Detected re-connect...attempting reinit
[ INFO] [1504154528.422704709]: Re-applying configuration to camera on re-init
[ INFO] [1504154528.521258127]: Starting color stream to adjust camera
[ INFO] [1504154528.521353007]: Starting color stream.
[ INFO] [1504154528.560892943]: Restarting publishers, if needed
[ INFO] [1504154528.560954914]: Done re-initializing cameras

When the camera is connected:

$ lsusb -t
/:  Bus 02.Port 1: Dev 1, Class=root_hub, Driver=xhci_hcd/4p, 5000M
/:  Bus 01.Port 1: Dev 1, Class=root_hub, Driver=xhci_hcd/10p, 480M
    |__ Port 2: Dev 5, If 0, Class=Vendor Specific Class, Driver=usbfs, 480M
    |__ Port 2: Dev 5, If 1, Class=Audio, Driver=, 480M
    |__ Port 2: Dev 5, If 2, Class=Audio, Driver=, 480M
    |__ Port 4: Dev 3, If 0, Class=Human Interface Device, Driver=usbhid, 12M
    |__ Port 4: Dev 3, If 1, Class=Human Interface Device, Driver=usbhid, 12M

Limitations described in #1 (comment) can be worked around in operation (e.g. for a single-camera limit, having multiple USB buses and connect one device per bus). So IMO this is valuable to be sent to upstream repo.

[130s]

@shaun-edwards I haven't tested (I should've done) but does this change break the existing usage where device_id argument is passed to openni2.launch?

[shaun-edwards]

I know of nothing in my changes that would affect this.