Added fixed exposure capability

[shaun-edwards]

This PR relies on #1.

The PR addresses the fixed exposure issue.

The changes in the PR attempt to force set the exposure, if auto exposure/white balance has been disabled. It uses several workarounds to address camera issues. In general mixing auto exposure/white balance and fixed exposure is NOT recommended.

[130s]

Just curious; what is this if section for?

[shaun-edwards]

Yeah...that definitely needs some explanation. The camera will ignore any settings it thinks are the same. However, sometimes the exposure is wrong (multiple reasons). In order to "force" the exposure, we increment it (or decrement it if its 255, 255 is the max). After that, we set it to the desired exposure.

[130s]

Tested by physically and manually plug off and in the USB of the Xtion camera.

Immediate re-connection
After re-connection without any operation done to dynamic_reconfigure, you see exposure value stays the same (119 in the case below that I manually set before re-connection).

$ roslaunch openni2_launch openni2.launch device_id:=#1
:
[ INFO] [1504200073.856893735]: Device "1d27/0601@1/2" found.
Warning: USB events thread - failed to set priority. This might cause loss of data...
[ WARN] [1504200074.627286566]: Reconnect has been enabled, only one camera should be plugged into each bus
[ INFO] [1504201946.656811356]: Starting color stream.
[ INFO] [1504201946.795642539]: using default calibration URL
[ INFO] [1504201946.795720509]: camera calibration URL: file:///home/rwc1/.ros/camera_info/rgb_PS1080_PrimeSense.yaml
[ INFO] [1504201946.795863102]: Unable to open camera calibration file [/home/rwc1/.ros/camera_info/rgb_PS1080_PrimeSense.yaml]
[ WARN] [1504201946.795914837]: Camera calibration file /home/rwc1/.ros/camera_info/rgb_PS1080_PrimeSense.yaml not found.
[ WARN] [1504202693.699019863]: Device "1d27/0601@1/2" disconnected

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

[ WARN] [1504202694.628224981]: Detected loss of connection.  Stopping all streams and resetting device
[ INFO] [1504202700.850010662]: Device "1d27/0601@1/4" found.
[ INFO] [1504202701.628196310]: Detected re-connect...attempting reinit
[ INFO] [1504202704.037063059]: Re-applying configuration to camera on re-init
[ INFO] [1504202704.134721608]: Starting color stream to adjust camera
[ INFO] [1504202704.134871049]: Starting color stream.
[ INFO] [1504202704.175497614]: Exposure is set to 119, forcing on color stream start
[ INFO] [1504202704.280763751]: Restarting publishers, if needed
[ INFO] [1504202704.280853352]: Done re-initializing cameras

BEFORE

rwc1@RWC1:~/bolles_ws$ rosrun dynamic_reconfigure dynparam get /camera/driver | grep -i exposure
{'use_device_time': True, 'auto_exposure': True, 'color_mode': 5, 'auto_white_balance': True, 'data_skip': 0, 'z_offset_mm': 0, 'depth_time_offset': -0.033, 'ir_mode': 5, 'color_time_offset': -0.033, 'depth_registration': False, 'ir_time_offset': -0.033, 'z_scaling': 1.0, 'groups': {'use_device_time': True, 'auto_exposure': True, 'ir_time_offset': -0.033, 'parent': 0, 'z_scaling': 1.0, 'groups': {}, 'z_offset_mm': 0, 'depth_ir_offset_y': 4.0, 'data_skip': 0, 'id': 0, 'depth_ir_offset_x': 5.0, 'exposure': 119, 'ir_mode': 5, 'color_mode': 5, 'auto_white_balance': True, 'name': 'Default', 'parameters': {}, 'depth_time_offset': -0.033, 'depth_mode': 5, 'color_time_offset': -0.033, 'depth_registration': False, 'state': True, 'type': '', 'color_depth_synchronization': False}, 'depth_ir_offset_y': 4.0, 'exposure': 119, 'color_depth_synchronization': False, 'depth_ir_offset_x': 5.0, 'depth_mode': 5}

AFTER

rwc1@RWC1:~/bolles_ws$ rosrun dynamic_reconfigure dynparam get /camera/driver | grep -i exposure
{'use_device_time': True, 'auto_exposure': True, 'color_mode': 5, 'auto_white_balance': True, 'data_skip': 0, 'z_offset_mm': 0, 'depth_time_offset': -0.033, 'ir_mode': 5, 'color_time_offset': -0.033, 'depth_registration': False, 'ir_time_offset': -0.033, 'z_scaling': 1.0, 'groups': {'use_device_time': True, 'auto_exposure': True, 'ir_time_offset': -0.033, 'parent': 0, 'z_scaling': 1.0, 'groups': {}, 'z_offset_mm': 0, 'depth_ir_offset_y': 4.0, 'data_skip': 0, 'id': 0, 'depth_ir_offset_x': 5.0, 'exposure': 119, 'ir_mode': 5, 'color_mode': 5, 'auto_white_balance': True, 'name': 'Default', 'parameters': {}, 'depth_time_offset': -0.033, 'depth_mode': 5, 'color_time_offset': -0.033, 'depth_registration': False, 'state': True, 'type': '', 'color_depth_synchronization': False}, 'depth_ir_offset_y': 4.0, 'exposure': 119, 'color_depth_synchronization': False, 'depth_ir_offset_x': 5.0, 'depth_mode': 5}

Re-connection with 4 minutes interval
I've observed, not frequently though, there are cases where the camera didn't reconnect in the OS-USB device level (log that can be found in dmesg and thus syslog) for more than a few minutes.

So I tested with 4 minutes interval between manual disconnection and reconnection. Still the fix worked as expected.

[shaun-edwards]

@130s, I also experienced "hard failures" of the USB driver. When this occurred, lsusb (run from the command line) would also hang. If patient enough, lsusb would return. At that point, re-connection logic worked just as you report.

As a side note, I also observed that when this situation occurs, removing the camera causes the openni camera manager to crash.