Compiler/OPT8241-CDK-EVM: steram multiple OPT8241 cameras synchrously.

Dear Larry:

We modify the CamerasSaveStreamTest Project from TI Voxel SDK ,then registered three separate callbacks for each Grabber and each callback has its own buffer and loop.
After finding some bugs, we can roughly run 3 different T.O.F Camera streaming together. Thanks your help.

Best Regards,
ShengHua

/*****************************************************************/
The main program is as follows.
/////////////////////////////////////////////////////////////////////////////
The program flow :
1. Connect Camera 1/2/3 to the computer with USB cable.
2. Initial grabber1/2/3 with specific callback function
3. Start grabber1/2/3 streaming.
////////////////////////////////////////////////////////////////////////////

/*
* TI Voxel Lib component.
*
* Copyright (c) 2014 Texas Instruments Inc.
*/


#include "CameraSystem.h"

#include "SimpleOpt.h"
#include "Common.h"
#include "Logger.h"
#include "UVCStreamer.h"
#include <iomanip>
#include <fstream>
//2017.0928 by ysh .....st ......
#include "grabber.h"
///// .....end ......
using namespace Voxel;

enum Options
{
VENDOR_ID = 0,
PRODUCT_ID = 1,
SERIAL_NUMBER = 2,
DUMP_FILE = 3,
NUM_OF_FRAMES = 4
};

Vector<CSimpleOpt::SOption> argumentSpecifications =
{
{ VENDOR_ID, "-v", SO_REQ_SEP, "Vendor ID of the USB device (hexadecimal)"}, // Only worker count is needed here
{ PRODUCT_ID, "-p", SO_REQ_SEP, "Comma separated list of Product IDs of the USB devices (hexadecimal)"},
{ SERIAL_NUMBER,"-s", SO_REQ_SEP, "Serial number of the USB device (string)"},
{ DUMP_FILE, "-f", SO_REQ_SEP, "Name of the file to dump extracted frames"},
{ NUM_OF_FRAMES,"-n", SO_REQ_SEP, "Number of frames to dump [default = 1]"},
SO_END_OF_OPTIONS
};

void help()
{
std::cout << "CameraSystemSaveStreamTest v1.0" << std::endl;

CSimpleOpt::SOption *option = argumentSpecifications.data();

while(option->nId >= 0)
{
std::cout << option->pszArg << " " << option->helpInfo << std::endl;
option++;
}
}

#if 1
typedef enum {
FRAMEFLAG_RAW_UNPROCESSED = (1 << DepthCamera::FRAME_RAW_FRAME_UNPROCESSED),
FRAMEFLAG_RAW_PROCESSED = (1 << DepthCamera::FRAME_RAW_FRAME_PROCESSED),
FRAMEFLAG_DEPTH_FRAME = (1 << DepthCamera::FRAME_DEPTH_FRAME),
FRAMEFLAG_XYZI_POINT_CLOUD_FRAME = (1 << DepthCamera::FRAME_XYZI_POINT_CLOUD_FRAME),
FRAMEFLAG_ALL = (FRAMEFLAG_RAW_UNPROCESSED
| FRAMEFLAG_RAW_PROCESSED
| FRAMEFLAG_DEPTH_FRAME
| FRAMEFLAG_XYZI_POINT_CLOUD_FRAME)
} FrameFlag;
#endif


int main(int argc, char *argv[])
{

CSimpleOpt s(argc, argv, argumentSpecifications);

logger.setDefaultLogLevel(LOG_INFO);

uint16_t vid = 0;

Vector<uint16_t> pids;
String serialNumber;
//2017.0929 by ysh .....
String serialNumber1 = "10408201039135";
String serialNumber2 = "10410711038992";
String serialNumber3 = "10408671038987";

String dumpFileName;

int32_t frameCount = 1;

//2017.0929 by ysh
bool test_flag = 0;

char *endptr;

while (s.Next())
{
if (s.LastError() != SO_SUCCESS)
{
std::cout << s.GetLastErrorText(s.LastError()) << ": '" << s.OptionText() << "' (use -h to get command line help)" << std::endl;
help();
return -1;
}

//std::cout << s.OptionId() << ": " << s.OptionArg() << std::endl;

Vector<String> splits;
switch (s.OptionId())
{
case VENDOR_ID:
vid = (uint16_t)strtol(s.OptionArg(), &endptr, 16);
break;

case PRODUCT_ID:
split(s.OptionArg(), ',', splits);

for(auto &s1: splits)
pids.push_back((uint16_t)strtol(s1.c_str(), &endptr, 16));

break;

case SERIAL_NUMBER:
serialNumber = s.OptionArg();
break;

case DUMP_FILE:
dumpFileName = s.OptionArg();
break;

case NUM_OF_FRAMES:
frameCount = (int32_t)strtol(s.OptionArg(), &endptr, 10);
break;

default:
help();
break;
};
}

if(vid == 0 || pids.size() == 0 || pids[0] == 0 || dumpFileName.size() == 0)
{
logger(LOG_ERROR) << "Required argument missing." << std::endl;
help();
return -1;
}

CameraSystem sys;

// Get all valid detected devices
const Vector<DevicePtr> &devices = sys.scan();

DevicePtr toConnect;
DevicePtr toConnect1;
DevicePtr toConnect2;
DevicePtr toConnect3;

std::cout << "Detected devices: " << std::endl;
for(auto &d: devices)
{
std::cout << d->id() << std::endl;

if(d->interfaceID() == Device::USB)
{
USBDevice &usb = (USBDevice &)*d;

//if (usb.vendorID() == vid && (serialNumber.size() == 0 || usb.serialNumber() == serialNumber)){
//if (usb.vendorID() == vid && (serialNumber1.size() == 0 || serialNumber2.size() == 0 || usb.serialNumber() == serialNumber1 || usb.serialNumber() == serialNumber2)){
if (usb.vendorID() == vid){
for(auto pid: pids)
if (usb.productID() == pid)
{
if (usb.serialNumber() == serialNumber1)
{ toConnect1 = d;}
if (usb.serialNumber() == serialNumber2)
{ toConnect2 = d; }
if (usb.serialNumber() == serialNumber3)
{toConnect3 = d;}
}
//toConnect = d;
}
}
}

//if(!toConnect)
//{
// logger(LOG_ERROR) << "No valid device found for the specified VID:PID:serialnumber" << std::endl;
// return -1;
//}

if (!toConnect1 && !toConnect2)
{
logger(LOG_ERROR) << "No valid device found for the specified VID:PID:serialnumber" << std::endl;
return -1;
}

//DepthCameraPtr depthCamera = sys.connect(toConnect);
DepthCameraPtr depthCamera1 = sys.connect(toConnect1);
DepthCameraPtr depthCamera2 = sys.connect(toConnect2);
DepthCameraPtr depthCamera3 = sys.connect(toConnect3);

//2017.0928 by ysh ......
CameraSystem sys1, sys2,sys3;

//2017.0928 by ysh......It is OK　........
//Grabber Grabber1(depthCamera,Grabber::FRAMEFLAG_XYZI_POINT_CLOUD_FRAME, sys);

Grabber Grabber1(depthCamera1, Grabber::FRAMEFLAG_DEPTH_FRAME, sys1,1);
Grabber Grabber2(depthCamera2, Grabber::FRAMEFLAG_XYZI_POINT_CLOUD_FRAME, sys2,2);
Grabber Grabber3(depthCamera3, Grabber::FRAMEFLAG_XYZI_POINT_CLOUD_FRAME, sys2,3);
// test_flag=Grabber1.isInitialized();

//std::cout << "Successfully loaded depth camera for device " << toConnect->id() << std::endl;
Grabber1.start();
Grabber2.start();
Grabber3.start();

while (1){};

return 0;
}