Commit efa6f293 authored by Alex Kushleyev's avatar Alex Kushleyev
Browse files

merge dev to master

parents 473b967b 34cc5f4e
Pipeline #241872177 passed with stage
in 1 minute and 1 second
......@@ -4,7 +4,7 @@ before_script:
include:
- project: 'voxl-public/utilities/ci-tools'
file: '/pipeline-configs/.deploy-dev.yml'
- project: 'voxl-public/utilities/ci-tools'
- project: 'voxl-public/utilities/ci-tools'
file: '/pipeline-configs/.deploy-stable.yml'
stages:
......
......@@ -2,7 +2,7 @@
Hal3 Tof app that gets RAW TOF data from the camera module. The app then calls the PMD libs to do the post-processing of the RAW data. The post-processed data like IR Image, Depth Image etc is published to ROS topics that can be viewed using the RViz tool.
## Build sample project:
## Build and install the project using `voxl-emulator`:
- Clone project:
......@@ -39,96 +39,148 @@ This will generate a binary in "devel/lib/voxl_hal3_tof_cam_ros/voxl_hal3_tof_ca
./install_on_voxl.sh
```
## Visualization
### On VOXL
## Build and install the project directly on `VOXL`:
```
# view network information
ifconfig
bash
mkdir -p /home/root/git
cd /home/root/git
git clone https://gitlab.com/voxl-public/voxl-hal3-tof-cam-ros.git
cd voxl-hal3-tof-cam-ros
./build.sh
./make_package.sh
opkg install ./voxl-hal3-tof-cam-ros_0.0.3.ipk
```
- This will give you the IP address of the VOXL
- For WiFi setup, instructions found [here](https://docs.modalai.com/wifi-setup/)
```
export ROS_IP=IP-ADDR-OF-VOXL
## Run TOF Application on VOXL
### Determine VOXL IP Address
- For WiFi setup, instructions found [here](https://docs.modalai.com/wifi-setup/)
- Run `ifconfig` in a terminal
- Check `inet addr` value for `wlan0` (if using wifi), this will give you the IP address of VOXL
- `hostname -i` also works
### Start TOF ROS Node
- If needed, open a new terminal on VOXL (use adb or ssh)
- Make sure your terminal is using bash (when in doubt just run `bash` after opening the terminal)
- Identify the correct camera ID to use for TOF sensor
- For camera id please check [here](https://docs.modalai.com/camera-connections/#configurations)
- Export TOF_CAM_ID in your environment before running the TOF application, for example
- `export TOF_CAM_ID=1`
- starting with release 0.0.3, you can choose to autodetect TOF camera id
- `export TOF_CAM_ID=-1` will tell the application to attempt to autodetect TOF camera id
- By default all outputs are enabled i.e. IR-Image, Depth-Image, Point-Cloud (see `tof.launch`)
- When running package installed to `/opt/ros/indigo` use:
#### Start Installed TOF ROS Node
```
bash
export ROS_IP=`hostname -i`
export TOF_CAM_ID=-1
source /opt/ros/indigo/setup.bash
roscore &
```
- This will print a line that should read something like this: `ROS_MASTER_URI=http://AAA.BBB.CCC.DDD:XYZUV/`
- The http address will be different
### On Desktop
```
# view network information
ifconfig
roslaunch /opt/ros/indigo/share/voxl_hal3_tof_cam_ros/launch/tof.launch
```
- This will give you the IP address of the PC
- When running a custom build of the package use (assuming it's in `/home/root/git/voxl-hal3-tof-cam-ros`):
#### Start Locally Built TOF ROS Node
```
# this will re-build the code and launch the app
bash
export ROS_IP=`hostname -i`
export TOF_CAM_ID=-1
cd /home/root/git/voxl-hal3-tof-cam-ros
./clean.sh
./build.sh
source ./devel/setup.bash
roslaunch ./source/launch/tof.launch
```
### Expected Behavior
- the very first time the sensor is used, it will be initialized (about 30 seconds)
- lens parameters will be downloaded from sensor if needed to `/data/misc/camera/irs10x0c_lens.cal`
```
yocto:~/git/voxl-hal3-tof-cam-ros# roslaunch ./source/launch/tof.launch
... logging to /home/root/.ros/log/bad5037a-561d-11eb-a4e9-90cdb698b2f1/roslaunch-apq8096-13532.log
...
...
process[rosout-1]: started with pid [13569]
started core service [/rosout]
process[base_link_tof_cam-2]: started with pid [13583]
process[base_link_laser_scan-3]: started with pid [13587]
process[tof/voxl_hal3_tof_cam_ros_node-4]: started with pid [13594]
DEPTH_IMAGE
IR: 1 ..... Cloud: 1 ..... Laser: 1
Camera id: -1
Image width: 224
Image height: 1557
Number of frames to dump: 0
Camera mode: preview
SUCCESS: Camera module opened
Camera Id: 0 .. Available raw sizes:
raw size: 224x1557 (TOF?)
Camera Id: 1 .. Available raw sizes:
raw size: 1280x480
raw size: 640x240
User specified TOF camera id = -1.. Autodetecting..
Found TOF camera with ID 0
SUCCESS: TOF interface created!
=========== modalai Royale3.31, Spectre4.7 CameraDevice::activateUseCase() : return SUCCESS!!
=========== modalai Royale3.31, Spectre4.7 CameraDevice::activateUseCase() : return SUCCESS!!
Libcamera sending RAW16 TOF data. App calling the PMD libs to postprocess the RAW16 data
[ INFO] [1610597128.592318535]: Loading lens parameters from /data/misc/camera/irs10x0c_lens.cal.
[ INFO] [1610597128.595652985]:
cx/cy 115.439 88.3658
fx/fy 111.314 111.314
tan coeff -0.0020162 -0.00255608
rad coeff -0.259339 0.0933604 -0.0150994
Camera HAL3 ToF camera app is now running
Frame: 0 SensorTimestamp = 4134124857341
Frame: 30 SensorTimestamp = 4135124771341
...
```
export ROS_IP=IP-ADDR-OF-PC
export ROS_MASTER_URI=http://AAA.BBB.CCC.DDD:XYZUV/
```
## Visualization
- The ROS_MASTER_URI is what you saw on the VOXL when you ran `roscore &` in the above step
### On VOXL
- Run the launch file per instructions above
- Use the VOXL ip address to determine `ROS_MASTER_URI=http://<voxl-ip>:11311/`
### On Desktop
- Determine IP address of the machine using `ifconfig`
```
source /opt/ros/xxxxx/setup.bash
bash
export ROS_IP=IP-ADDR-OF-PC
export ROS_MASTER_URI=http://<voxl-ip>:11311/
source /opt/ros/<ros-version>/setup.bash
```
- xxxxxx is the ROS version you have installed
The 2 transforms (for both laser scan and point cloud) are in the tof.launch file
* In RViz choose Fixed Frame toption as /base_link instead of map
* Open your .rviz file (by default in /home/xxx/.rviz/default.rviz)
* In RViz choose Fixed Frame toption as `/base_link` instead of `map`
* Open your .rviz file (by default in `/home/xxx/.rviz/default.rviz`)
```
vi /home/xxx/.rviz/default.rviz
```
- Change the "Fixed Frame:" option from "map" to "base_link"
Global Options:
Fixed Frame: /base_link
- Global Options:
- Fixed Frame: /base_link
```
rviz
```
Start the `rviz` program on your desktop
Start the `rviz` program on your desktop (run `rviz` in a terminal)
- On the leftmost column click on the "Add" button
- In the pop-up options click on "Image"
- Change Display Name to "IR-Image"
- In the left column under "IR-Image" tab select type in Image Topic as /voxl_ir_image_raw
- In the left column under "IR-Image" tab select type in Image Topic as `/voxl_ir_image_raw`
- Click on the "Add" button again
- In the pop-up options click on "Image"
- Change Display Name to "Depth-Image"
- In the left column under "Depth-Image" tab select type in Image Topic as /voxl_depth_image_raw
- "Add" PointCloud2 with Topic as "/voxl_point_cloud"
- "Add" LaserScan with Topic as "/voxl_laser_scan"
### On VOXL
Modify the "camera_id" field in tof.launch to select the correct camera id for your setup
- By default it is 1
- File is in this directory: /opt/ros/indigo/share/voxl_hal3_tof_cam_ros/launch/tof.launch
- For camera-id please check [here](https://docs.modalai.com/camera-connections/)
```
roslaunch /opt/ros/indigo/share/voxl_hal3_tof_cam_ros/launch/tof.launch
```
- By default all outputs are enabled i.e. IR-Image, Depth-Image, Point-Cloud
- In the left column under "Depth-Image" tab select type in Image Topic as `/voxl_depth_image_raw`
- "Add" PointCloud2 with Topic as `/voxl_point_cloud`
- "Add" LaserScan with Topic as `/voxl_laser_scan`"`
......@@ -32,7 +32,7 @@
################################################################################
#!/bin/bash
SRC_PATH=/home/root/source/
SRC_PATH=`pwd`/source
if [[ $# -eq 1 ]]; then
SRC_PATH=$1
......
################################################################################
# * Copyright 2020 ModalAI Inc.
# *
# * Redistribution and use in source and binary forms, with or without
# * modification, are permitted provided that the following conditions are met:
# *
# * 1. Redistributions of source code must retain the above copyright notice,
# * this list of conditions and the following disclaimer.
# *
# * 2. Redistributions in binary form must reproduce the above copyright notice,
# * this list of conditions and the following disclaimer in the documentation
# * and/or other materials provided with the distribution.
# *
# * 3. Neither the name of the copyright holder nor the names of its contributors
# * may be used to endorse or promote products derived from this software
# * without specific prior written permission.
# *
# * 4. The Software is used solely in conjunction with devices provided by
# * ModalAI Inc.
# *
# * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OFe
# * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# * POSSIBILITY OF SUCH DAMAGE.
################################################################################
#!/bin/bash
VERSION=$(cat ipk/control/control | grep "Version" | cut -d' ' -f 2)
PACKAGE=$(cat ipk/control/control | grep "Package" | cut -d' ' -f 2)
IPK_NAME=${PACKAGE}_${VERSION}.ipk
DATA_DIR=ipk/data
CONTROL_DIR=ipk/control
sudo rm -rf build
sudo rm -rf devel
sudo rm -rf install
sudo rm -rf ipk/control.tar.gz
sudo rm -rf $DATA_DIR/
sudo rm -rf ipk/data.tar.gz
sudo rm -rf $IPK_NAME
sudo rm -rf ./source/catkin
sudo rm -rf ./source/catkin_generated
sudo rm -rf ./source/CMakeFiles
sudo rm -rf ./source/test_results
sudo rm -rf ./source/CMakeCache.txt
sudo rm -rf ./source/cmake_install.cmake
sudo rm -rf ./source/CTestTestfile.cmake
sudo rm -rf ./source/Makefile
#!/bin/bash
################################################################################
# * Copyright 2020 ModalAI Inc.
# *
# * Redistribution and use in source and binary forms, with or without
# * modification, are permitted provided that the following conditions are met:
# *
# * 1. Redistributions of source code must retain the above copyright notice,
# * this list of conditions and the following disclaimer.
# *
# * 2. Redistributions in binary form must reproduce the above copyright notice,
# * this list of conditions and the following disclaimer in the documentation
# * and/or other materials provided with the distribution.
# *
# * 3. Neither the name of the copyright holder nor the names of its contributors
# * may be used to endorse or promote products derived from this software
# * without specific prior written permission.
# *
# * 4. The Software is used solely in conjunction with devices provided by
# * ModalAI Inc.
# *
# * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# * POSSIBILITY OF SUCH DAMAGE.
# Copyright (c) 2020 ModalAI, Inc. All rights reserved.
#
# Installs the ipk package on target.
# Requires the ipk to be built and an adb connection.
################################################################################
set -e
......@@ -48,15 +22,13 @@ elif [ $NUM_FILES -gt "1" ]; then
exit 1
fi
# now we know only one ipk file exists
FILE=$(ls -1q $PACKAGE*.ipk)
echo "pushing $FILE to target"
echo "searching for ADB device"
adb wait-for-device
echo "adb device found"
# now we know only one ipk file exists
FILE=$(ls -1q $PACKAGE*.ipk)
echo "pushing $FILE to target"
adb push $FILE /home/root/ipk/$FILE
adb shell "opkg remove $PACKAGE"
adb shell "opkg install /home/root/ipk/$FILE"
adb shell "opkg install --force-reinstall --force-downgrade --force-depends /home/root/ipk/$FILE"
Package: voxl-hal3-tof-cam-ros
Version: 0.0.2
Version: 0.0.3
Section: base
Priority: optional
Architecture: aarch64
......
......@@ -51,6 +51,9 @@
class BufferManager;
class PerCameraMgr;
#define TOF_RAW_FRAME_WIDTH 224
#define TOF_RAW_FRAME_HEIGHT 1557
// -----------------------------------------------------------------------------------------------------------------------------
// Thread Data for camera request and result thread
// -----------------------------------------------------------------------------------------------------------------------------
......@@ -83,7 +86,7 @@ enum PreviewFormat
{
PreviewFormatNV21 = 0, ///< NV21
PreviewFormatRAW8 = 1, ///< RAW8 (If the camera doesnt support RAW8, RAW10 will be requested and converted to RAW8)
PreviewFormatBLOB = 2, ///< BLOB (TOF camera uses this for preview format)
PreviewFormatBLOB = 2, ///< BLOB (TOF camera uses this for preview format)
};
// -----------------------------------------------------------------------------------------------------------------------------
......@@ -104,7 +107,7 @@ typedef enum
// RAW only mode for devices that will simultaneously use more than two cameras.
// This mode has following limitations: Back end 3A, Face Detect or any additional functionality depending on image/sensor
// statistics and YUV streams will be disabled
QCAMERA3_VENDOR_STREAM_CONFIGURATION_RAW_ONLY_MODE = 0x8000,
} QCamera3VendorStreamConfiguration;
......@@ -165,7 +168,7 @@ private:
// Return if this resolution corresponds to a TOF camera
void DetermineTOFCamera(int width, int height)
{
if ((width == 224) && (height == 1557))
if ((width == TOF_RAW_FRAME_WIDTH) && (height == TOF_RAW_FRAME_HEIGHT))
{
m_isTOF = true;
}
......@@ -308,6 +311,9 @@ public:
// Stop the opened camera and close it
void Stop();
// Automatically determine ID of TOF camera based on output stream dimensions
int FindTofCamera();
private:
// Callback to indicate device status change
static void CameraDeviceStatusChange(const struct camera_module_callbacks* callbacks, int camera_id, int new_status)
......@@ -321,6 +327,8 @@ private:
// Load the camera module to start communicating with it
int LoadCameraModule();
void PrintCameraCapabilities(camera_info * pCameraInfo);
static const uint32_t MaxCameras = 4;
camera_module_t* m_pCameraModule; ///< Camera module
......
......@@ -68,6 +68,11 @@
<param name="publish_laser_scan" value="$(arg publish_laser_scan)" />
<!-- for now and to stay compatible with older version, we keep the images upside down -->
<param name="flip_image" value="true" />
<remap from="~voxl_ir_image_raw" to="/voxl_ir_image_raw"/>
<remap from="~voxl_depth_image_raw" to="/voxl_depth_image_raw"/>
<remap from="~voxl_point_cloud" to="/voxl_point_cloud"/>
<remap from="~voxl_laser_scan" to="/voxl_laser_scan"/>
</node>
</group>
......
......@@ -49,17 +49,17 @@ int main(int argc, char **argv)
if (status == 0)
{
printf("\n\nCamera HAL3 ToF camera test is now running\n\n");
printf("\n\nCamera HAL3 ToF camera app is now running\n\n");
fflush(stdout);
ros::spin();
ros::AsyncSpinner spinner(1); // Use 1 thread
spinner.start();
ros::waitForShutdown();
printf("\nCamera HAL3 ToF camera test is now stopping");
printf("\nCamera HAL3 ToF camera app is now stopping");
Hal3MainExit();
printf("\nCamera HAL3 ToF camera test is done\n\n");
printf("\nCamera HAL3 ToF camera app is done\n\n");
}
return 0;
......
......@@ -47,6 +47,82 @@ CameraHAL3::CameraHAL3()
}
}
int CameraHAL3::FindTofCamera()
{
int tofCameraId = -1;
for (int camid=0; camid<m_numCameras; camid++)
{
camera_metadata_t* pStaticMetadata = (camera_metadata_t *)m_cameraInfo[camid].static_camera_characteristics;
camera_metadata_ro_entry entry;
// Get the list of all stream resolutions supported and then go through each one of them looking for a match
int status = find_camera_metadata_ro_entry(pStaticMetadata, ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS, &entry);
if (status)
{
printf("ERROR: could not find ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS entry\n");
continue;
}
for (size_t i = 0; i < entry.count; i+=4)
{
if (ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS_OUTPUT == entry.data.i32[i + 3])
{
//int32_t format = entry.data.i32[i];
int32_t width = entry.data.i32[i+1];
int32_t height = entry.data.i32[i+2];
//printf("STREAM: format %d, width %d, height %d\n",format,width,height);
if ((width==TOF_RAW_FRAME_WIDTH) && (height==TOF_RAW_FRAME_HEIGHT))
{
tofCameraId=camid;
return tofCameraId;
}
}
}
}
return tofCameraId;
}
void CameraHAL3::PrintCameraCapabilities(camera_info * pCameraInfo)
{
camera_metadata_t* pStaticMetadata = (camera_metadata_t *)pCameraInfo->static_camera_characteristics;
camera_metadata_ro_entry entry;
// Get the list of all stream resolutions supported and then go through each one of them looking for a match
int status = find_camera_metadata_ro_entry(pStaticMetadata, ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS, &entry);
if (status)
{
printf("ERROR: could not find ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS entry\n");
return;
}
for (size_t i = 0; i < entry.count; i+=4)
{
if (ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS_OUTPUT == entry.data.i32[i + 3])
{
int32_t format = entry.data.i32[i];
int32_t width = entry.data.i32[i+1];
int32_t height = entry.data.i32[i+2];
if (format == 0x20) //HAL_PIXEL_FORMAT_RAW_SENSOR
{
printf("\n raw size: %dx%d",width,height);
}
if ((format == 0x21) && (width==TOF_RAW_FRAME_WIDTH) && (height==TOF_RAW_FRAME_HEIGHT)) //HAL_PIXEL_FORMAT_BLOB
{
printf("\n raw size: %dx%d (TOF?)",width,height);
}
//printf("STREAM: format %d, width %d, height %d\n",format,width,height);
}
}
}
// -----------------------------------------------------------------------------------------------------------------------------
// Performs any one time initialization. This function should only be called once.
// -----------------------------------------------------------------------------------------------------------------------------
......@@ -83,7 +159,9 @@ int CameraHAL3::Initialize()
if (status == 0)
{
printf("\nCamera Id: %d Facing: %d", i, m_cameraInfo[i].facing);
//printf("\nCamera Id: %d Facing: %d", i, m_cameraInfo[i].facing);
printf("\nCamera Id: %d .. Available raw sizes:", i);
PrintCameraCapabilities(&m_cameraInfo[i]);
}
else
{
......@@ -144,7 +222,7 @@ int CameraHAL3::Start(int cameraid, ///< Camera id to open
{
printf("\nInvalid camera id %d. There are only %d cameras", cameraid, m_numCameras);
}
return status;
}
......@@ -161,4 +239,4 @@ void CameraHAL3::Stop()
m_pPerCameraMgr[i] = NULL;
}
}
}
\ No newline at end of file
}
......@@ -77,6 +77,7 @@ int Hal3MainEnter(int argc, char* const argv[], ros::NodeHandle rosNodeHandle)
bool publishPointCloud;
bool publishLaserScan;
rosNodeHandle.param<int>("camera_id", cameraid, -1);
rosNodeHandle.param<bool>("publish_depth_image", publishDepthImage, false);
rosNodeHandle.param<bool>("publish_ir_image", publishIRImage, false);
rosNodeHandle.param<bool>("publish_point_cloud", publishPointCloud, false);
......@@ -108,6 +109,23 @@ int Hal3MainEnter(int argc, char* const argv[], ros::NodeHandle rosNodeHandle)
status = g_pCameraHAL3->Initialize();
}
if ((status == 0) && (cameraid == -1)) //autodetect TOF camera id
{
printf("\nUser specified TOF camera id = -1.. Autodetecting..");
cameraid = g_pCameraHAL3->FindTofCamera();
if (cameraid == -1)
{
printf("\n\nERROR: Could not autodetect TOF camera\n");
status = -1;
}
else
{
printf("\nFound TOF camera with ID %d\n",cameraid);
status = 0;
}
}
if (status == 0)
{
// Making this function call will start the camera (cameraid) and it will start streaming frames
......
......@@ -200,10 +200,13 @@ int PerCameraMgr::Initialize(const camera_module_t* pCameraModule, ///< Cam
{
if (IsTOFCamera())
{
char enableAppTofProcessing = '0';
property_get("persist.camera.modalai.tof", &enableAppTofProcessing, &enableAppTofProcessing);
// Check param whether libcamera should send use raw or processed TOF data
// This param should either be omitted or set to 1 using linux cmd "setprop persist.camera.modalai.tof 1"
// Setting this param to 0 is no longer supported
int enableAppTofProcessing = 1;
enableAppTofProcessing = property_get_bool("persist.camera.modalai.tof",(int8_t)enableAppTofProcessing);
if (enableAppTofProcessing == '1')
if (enableAppTofProcessing == 1)
{
m_pTOFInterface = TOFCreateInterface();
......@@ -263,10 +266,10 @@ int PerCameraMgr::Initialize(const camera_module_t* pCameraModule, ///< Cam
m_rosNodeHandle.param<bool>("publish_point_cloud", m_publishPointCloud, false);
m_rosNodeHandle.param<bool>("publish_laser_scan", m_publishLaserScan, false);
m_rosIRImagePublisher = m_rosImageTransport.advertiseCamera("/voxl_ir_image_raw", ImageQueueSize, false);
m_rosDepthImagePublisher = m_rosImageTransport.advertiseCamera("/voxl_depth_image_raw", ImageQueueSize, false);