void ViSensorInterface::StartIntegratedSensor(uint32_t image_rate, uint32_t imu_rate)
{
if (image_rate > 30)
{
image_rate = 30;
std::cout << "Desired image rate is too hight, setting it to 30 Hz." << std::endl;
}
if (imu_rate > 800)
{
imu_rate = 800;
std::cout << "Desired imu rate is too hight, setting it to 800 Hz." << std::endl;
}
try
{
drv_.init();
}
catch (visensor::exceptions::ConnectionException const &ex)
{
std::cout << ex.what() << "\n";
return;
}
// re-configure camera parameters
drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM0, "row_flip", 0);
drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM0, "column_flip", 0);
drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM1, "row_flip", 1);
drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM1, "column_flip", 1);
drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM0, "min_coarse_shutter_width", 2);
drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM0, "max_coarse_shutter_width", 550);
drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM1, "min_coarse_shutter_width", 2);
drv_.setSensorConfigParam(visensor::SensorId::SensorId::CAM1, "max_coarse_shutter_width", 550);
// set callback for image messages
drv_.setCameraCallback(boost::bind(&ViSensorInterface::ImageCallback, this, _1));
// set callback for IMU messages
drv_.setImuCallback(boost::bind(&ViSensorInterface::ImuCallback, this, _1));
drv_.startAllCameras(image_rate);
drv_.startAllImus(imu_rate);
vi_sensor_connected_ = true;
boost::thread drawWorker_0(boost::bind(&ViSensorInterface::worker, this, 0));
boost::thread drawWorker_1(boost::bind(&ViSensorInterface::worker, this, 1));
boost::thread drawWorker_2(boost::bind(&ViSensorInterface::worker, this, 2));
boost::thread drawWorker_3(boost::bind(&ViSensorInterface::worker, this, 3));
drawWorker_0.join();
drawWorker_1.join();
drawWorker_2.join();
drawWorker_3.join();
}
void ViSensorInterface::StartIntegratedSensor(uint32_t image_rate, uint32_t imu_rate) {
if (image_rate > 30) {
image_rate = 30;
std::cout << "Desired image rate is too hight, setting it to 30 Hz." << std::endl;
}
if (imu_rate > 800) {
imu_rate = 800;
std::cout << "Desired imu rate is too hight, setting it to 800 Hz." << std::endl;
}
try {
drv_.init();
} catch (visensor::exceptions::ConnectionException const &ex) {
std::cout << ex.what() << "\n";
return;
}
std::cout << "Libvisensor version is " << visensor::LIBRARY_VERSION_MAJOR << "."
<< visensor::LIBRARY_VERSION_MINOR << "." << visensor::LIBRARY_VERSION_PATCH << std::endl;
// load available calibration and configure sensors corresponding to it
drv_.selectCameraCalibration();
// set callback for image messages
drv_.setCameraCallback(boost::bind(&ViSensorInterface::ImageCallback, this, _1));
// set callback for IMU messages
drv_.setImuCallback(boost::bind(&ViSensorInterface::ImuCallback, this, _1));
drv_.startAllCameras(image_rate);
drv_.startAllImus(imu_rate);
vi_sensor_connected_ = true;
boost::thread drawWorker_0(boost::bind(&ViSensorInterface::worker, this, 0));
boost::thread drawWorker_1(boost::bind(&ViSensorInterface::worker, this, 1));
boost::thread drawWorker_2(boost::bind(&ViSensorInterface::worker, this, 2));
boost::thread drawWorker_3(boost::bind(&ViSensorInterface::worker, this, 3));
drawWorker_0.join();
drawWorker_1.join();
drawWorker_2.join();
drawWorker_3.join();
}