/* 3dv-client/main.cc
*
* Copyright (C) 2013 VisLab
*
* This file is part of 3dv-client; you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation; either version 3 of the License, or (at
* your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, see .
*/
#include "disk_writer.h"
#include "display.h"
#include "utils.h"
#include
#include "playback.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#define VERSION 20150310
int main(int argc, char* argv[])
{
std::string address;
std::vector addresses;
unsigned int log_level;
bool autonumber;
std::vector pathnames;
data_file_format::types data_format;
#ifdef ARCHIVE
archive_file_format::types archive_format;
std::string archive_name;
#endif
std::string device_params_file_name;
std::vector set_value;
std::string get_value;
boost::uuids::uuid device_guid;
uint64_t timeout;
std::vector playback_value;
boost::program_options::typed_value* log_option = new boost::program_options::typed_value(&log_level);
boost::program_options::typed_value* interface_option = new boost::program_options::typed_value(&address);
boost::program_options::typed_value >* pathnames_option = new boost::program_options::typed_value >(&pathnames);
boost::program_options::typed_value* autonumber_option = new boost::program_options::typed_value(&autonumber);
boost::program_options::typed_value* data_format_option = new boost::program_options::typed_value(&data_format);
#ifdef ARCHIVE
boost::program_options::typed_value* archive_format_option = new boost::program_options::typed_value(&archive_format);
boost::program_options::typed_value* archive_name_option = new boost::program_options::typed_value(&archive_name);
#endif
boost::program_options::typed_value* export_properties_option = new boost::program_options::typed_value(&device_params_file_name);
boost::program_options::typed_value >* set_value_option = new boost::program_options::typed_value >(&set_value);
boost::program_options::typed_value* get_value_option = new boost::program_options::typed_value(&get_value);
boost::program_options::typed_value* device_guid_option = new boost::program_options::typed_value(&device_guid);
boost::program_options::typed_value* timeout_option = new boost::program_options::typed_value(&timeout);
boost::program_options::typed_value >* playback_option = new boost::program_options::typed_value >(&playback_value);
log_option->value_name("");
log_option->default_value(0U);
interface_option->value_name("");
pathnames_option->multitoken();
pathnames_option->value_name("");
pathnames_option->default_value(std::vector(), "$GUID_$DATE-$TIME | .");
autonumber_option->value_name("");
autonumber_option->default_value(true, "true");
data_format_option->value_name("{raw|binary|text|gnuplot}");
data_format_option->default_value(data_file_format::TEXT, "text");
#ifdef ARCHIVE
archive_name_option->default_value("$GUID_$DATE-$TIME");
archive_format_option->value_name("{zip|tar}");
#ifdef __GNUC__
archive_format_option->default_value(archive_file_format::TAR, "tar");
#else
archive_format_option->default_value(archive_file_format::ZIP, "zip");
#endif
#endif
export_properties_option->value_name("");
export_properties_option->default_value("$GUID_$DATE-$TIME.xml");
set_value_option->multitoken();
set_value_option->value_name(" {bool|int64|int32|int16|int8|uint64|uint32|uint16|uint8|float32|float64|string}");
get_value_option->value_name("");
device_guid_option->value_name("");
timeout_option->value_name("");
timeout_option->default_value(lib3dv::device::DEFAULT_TIMEOUT.total_milliseconds());
playback_option->multitoken();
playback_option->value_name("");
boost::program_options::options_description description("Options");
description.add_options()
("help", "this help message")
("log", log_option, "log level")
("list-interfaces", "list the available network connections")
("interface", interface_option, "the IP address of the local interface to bind to")
("device-guid", device_guid_option, "target device GUID")
("record", "perform recording")
#ifdef PCL
("display", "display live data")
("playback",playback_option, "display recorded data in a given path")
#endif
("recording-paths", pathnames_option, "the recording path(s)")
("autonumber", autonumber_option, "append the frame number to the file name")
("data-format", data_format_option, "data files format")
#ifdef ARCHIVE
("archive", "use a single archive to store recorded files")
("archive-name", archive_name_option, "archive name")
("archive-format", archive_format_option, "archive file format")
#endif
("list-devices", "list devices")
("list-properties", "list device properties")
("export-properties", export_properties_option, "export device properties to an xml file")
("save-properties", "save property values on the device")
("reset-properties", "restore default property values on the device")
("set-value", set_value_option, "set a property value")
("get-value", get_value_option, "get a property value")
("poweroff", "shut down the device")
("reboot", "reboot the device")
("timeout", timeout_option, "network timeout")
("version", "program version");
boost::program_options::variables_map variables_map;
try
{
boost::program_options::store(boost::program_options::parse_command_line(argc, argv,
description,
boost::program_options::command_line_style::unix_style ^ boost::program_options::command_line_style::allow_short),
variables_map);
boost::program_options::notify(variables_map);
if(variables_map.count("help") || argc == 1)
{
std::cout << description << std::endl;
return 0;
}
if(variables_map.count("version"))
{
std::cout << "3dv-client " << VERSION << " using lib3dv " << LIB3DV_VERSION_MAJOR << '.' << LIB3DV_VERSION_MINOR << '.' << LIB3DV_VERSION_PATCH << std::endl;
std::cout << "Copyright (C) 2013 VisLab S.r.L.\nThis is free software; see the source for copying conditions.\nThere is NO warranty; not even for MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE." << std::endl << std::endl;
return 0;
}
if(variables_map.count("list-interfaces"))
{
addresses = list_interface_addresses();
if(addresses.empty())
{
std::cerr << "[EE] 3dv-client: " << "no configured address found" << std::endl;
return lib3dv::error::NETWORK_FAILURE;
}
std::cout << "[II] 3dv-client: available interfaces are:" << std::endl;
for(unsigned int a = 0; a < addresses.size(); ++a)
std::cout << "\t" << addresses[a].to_v4().to_string() << std::endl;
return lib3dv::error::NONE;
}
if(!variables_map.count("interface"))
{
addresses = list_interface_addresses();
if(addresses.empty())
{
std::cerr << "[EE] 3dv-client: " << "no configured address found" << std::endl;
return lib3dv::error::NETWORK_FAILURE;
}
}
else
addresses.push_back(boost::asio::ip::address_v4::from_string(address));
}
catch(boost::program_options::error& e)
{
std::cerr << "[EE] 3dv-client: " << e.what() << std::endl;
std::cout << description << std::endl;
return lib3dv::error::NONE;
}
std::vector devices;
std::vector::iterator device = devices.end();
lib3dv::error error;
if(variables_map.count("playback"))
{
return playback(playback_value,log_level);
}
for(unsigned int a = 0; a < addresses.size(); ++a)
{
std::cout << "[II] 3dv-client: detecting devices on " << addresses[a].to_v4().to_string() << std::endl;
std::vector curr_devices = lib3dv::device::enumerate(addresses[a].to_v4(), log_level, error, boost::posix_time::milliseconds(timeout));
if(error != lib3dv::error::NONE)
std::cout << "[EE] 3dv-client: error detected: " << error << std::endl;
else
devices.insert(devices.end(), curr_devices.begin(), curr_devices.end());
}
if(devices.empty())
{
std::cout << "[EE] 3dv-client: no devices found" << std::endl;
return lib3dv::error::NONE;
}
if(variables_map.count("timeout"))
{
if(!devices.empty())
for(std::vector::iterator dd = devices.begin(); dd != devices.end(); ++dd)
dd->timeout(boost::posix_time::milliseconds(timeout));
}
if(variables_map.count("list-devices"))
{
for(std::vector::iterator dd = devices.begin(); dd != devices.end(); ++dd)
std::cout << '\t' << *dd << std::endl;
return lib3dv::error::NONE;
}
if(variables_map.count("device-guid"))
{
for(std::vector::iterator dd = devices.begin(); dd != devices.end(); ++dd)
if(dd->guid() == device_guid)
{
device = dd;
break;
}
if(device == devices.end())
{
std::cerr << "[EE] 3dv-client: device " << device_guid << " not found" << std::endl;
return lib3dv::error::DEVICE_NOT_FOUND;
}
}
else
{
device = devices.begin();
if(devices.size() > 1)
std::cout << "[WW] 3dv-client: multiple devices found, but --device-guid not specified" << std::endl;
}
std::cout << "[II] 3dv-client: selected device " << device->guid() << std::endl;
if(variables_map.count("list-properties"))
{
std::vector properties = device->enumerate_properties(error);
if(error == lib3dv::error::NONE)
{
std::cout << "[II] 3dv-client: device properties:" << std::endl;
for(unsigned int p = 0; p < properties.size(); ++p)
std::cout << '\t' << properties[p] << std::endl;
}
else
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
return error;
}
if(variables_map.count("save-properties"))
{
device->save_properties(error);
if(error != lib3dv::error::NONE)
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
else
std::cout << "[EE] 3dv-client: operation completed" << std::endl;
return error;
}
if(variables_map.count("reset-properties"))
{
device->reset_properties(error);
if(error != lib3dv::error::NONE)
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
else
std::cout << "[EE] 3dv-client: operation completed" << std::endl;
return error;
}
if(variables_map.count("set-value"))
{
if(set_value.size() == 3)
{
std::stringstream ss;
uint16_t address;
const std::string& value = set_value[1];
const std::string& type = set_value[2];
ss << std::hex << set_value[0];
ss >> address;
std::cout << "[II] 3dv-client: setting property " << set_value[0] << " value to " << value << std::endl;
if(type == "bool") device->set_property_value(address, boost::any(boost::lexical_cast(value)), error);
else if(type == "int64") device->set_property_value(address, boost::any(boost::lexical_cast(value)), error);
else if(type == "int32") device->set_property_value(address, boost::any(boost::numeric_cast(boost::lexical_cast(value))), error);
else if(type == "int16") device->set_property_value(address, boost::any(boost::numeric_cast(boost::lexical_cast(value))), error);
else if(type == "int8") device->set_property_value(address, boost::any(boost::numeric_cast(boost::lexical_cast(value))), error);
else if(type == "uint64") device->set_property_value(address, boost::any(boost::lexical_cast(value)), error);
else if(type == "uint32") device->set_property_value(address, boost::any(boost::numeric_cast(boost::lexical_cast(value))), error);
else if(type == "uint16") device->set_property_value(address, boost::any(boost::numeric_cast(boost::lexical_cast(value))), error);
else if(type == "uint8") device->set_property_value(address, boost::any(boost::numeric_cast(boost::lexical_cast(value))), error);
else if(type == "float32") device->set_property_value(address, boost::any(boost::lexical_cast(value)), error);
else if(type == "float64") device->set_property_value(address, boost::any(boost::lexical_cast(value)), error);
else if(type == "string") device->set_property_value(address, boost::any(value), error);
else std::cout << "[EE] 3dv-client: unsupported data type " << type << std::endl;
if(error != lib3dv::error::NONE)
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
else
std::cout << "[EE] 3dv-client: operation completed" << std::endl;
return error;
}
else
{
std::cerr << "[EE] 3dv-client: exactly 3 arguments must be provided to --set-value" << std::endl;
return lib3dv::error::NONE;
}
}
if(variables_map.count("get-value"))
{
std::stringstream ss;
uint16_t address;
ss << std::hex << get_value;
ss >> address;
const boost::any& value = device->get_property_value(address, error);
if(error != lib3dv::error::NONE)
{
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
return error;
}
const std::type_info& type = value.type();
std::cout << "[II] 3dv-client: property " << get_value << " value is ";
if(type == typeid(bool)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(int64_t)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(int32_t)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(int32_t)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(int16_t)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(int8_t)) std::cout << static_cast(boost::any_cast(value)) << std::endl;
else if(type == typeid(uint64_t)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(uint32_t)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(uint32_t)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(uint16_t)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(uint8_t)) std::cout << static_cast(boost::any_cast(value)) << std::endl;
else if(type == typeid(float)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(double)) std::cout << boost::any_cast(value) << std::endl;
else if(type == typeid(std::string)) std::cout << boost::any_cast(value) << std::endl;
return lib3dv::error::NONE;
}
if(variables_map.count("record")
#ifdef PCL
|| variables_map.count("display")
#endif
)
{
boost::asio::io_service sighandler_io_service;
boost::asio::io_service::work sighandler_io_service_work(sighandler_io_service);
boost::thread disk_writer_thread;
boost::shared_ptr disk_writer;
device_params params;
params.downsample=(uint32_t)2;
std::vector properties = device->enumerate_properties(error);
if(error == lib3dv::error::NONE)
{
for(unsigned int p = 0; p < properties.size(); ++p)
{
const std::string& name = properties[p].m_name;
const boost::any& value = properties[p].m_value;
if(name == "calibration.u0")
params.intrinsics.m_u0 = boost::any_cast(value);
else if(name == "calibration.v0")
params.intrinsics.m_v0 = boost::any_cast(value);
else if(name == "calibration.ku")
params.intrinsics.m_ku = boost::any_cast(value);
else if(name == "calibration.kv")
params.intrinsics.m_kv = boost::any_cast(value);
else if(name == "calibration.x")
params.position.m_x = boost::any_cast(value);
else if(name == "calibration.y")
params.position.m_y = boost::any_cast(value);
else if(name == "calibration.z")
params.position.m_z = boost::any_cast(value);
else if(name == "calibration.yaw")
params.orientation.m_yaw = boost::any_cast(value);
else if(name == "calibration.pitch")
params.orientation.m_pitch = boost::any_cast(value);
else if(name == "calibration.roll")
params.orientation.m_roll = boost::any_cast(value);
else if(name == "calibration.baseline")
params.baseline = boost::any_cast(value);
else if(name == "depth mapping.downsample ratio")
params.downsample =boost::any_cast(value);
else if(name =="advanced.detection.area.step")
params.area_step = boost::any_cast(value);
}
}
else
{
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
return error;
}
if(variables_map.count("record"))
{
if(pathnames.empty())
pathnames.push_back(variables_map.count("archive") ? "." : "$GUID_$DATE-$TIME");
std::vector paths;
// remove duplicates
std::sort(pathnames.begin(), pathnames.end());
pathnames.erase(std::unique(pathnames.begin(), pathnames.end()), pathnames.end());
// expand variables
std::stringstream guid_ss;
guid_ss << device->guid();
//Check 3DV application version
uint32_t app_version=device->version().m_application[2]+device->version().m_application[1]*100+device->version().m_application[0]*10000;
uint8_t guid_type=9;
if (app_version <10100) guid_type=7;
else if (app_version <10200) guid_type=8;
for(unsigned int p = 0; p < pathnames.size(); ++p)
boost::replace_all(pathnames[p], "$GUID", guid_ss.str());
boost::posix_time::ptime timestamp(boost::posix_time::second_clock::local_time());
std::stringstream date_ss;
boost::posix_time::time_facet* date_facet = new boost::posix_time::time_facet("%Y%m%d");
date_ss.imbue(std::locale(date_ss.getloc(), date_facet));
date_ss << timestamp;
for(unsigned int p = 0; p < pathnames.size(); ++p)
boost::replace_all(pathnames[p], "$DATE", date_ss.str());
std::stringstream time_ss;
boost::posix_time::time_facet* time_facet = new boost::posix_time::time_facet("%H%M%S");
time_ss.imbue(std::locale(time_ss.getloc(), time_facet));
time_ss << timestamp;
for(unsigned int p = 0; p < pathnames.size(); ++p)
boost::replace_all(pathnames[p], "$TIME", time_ss.str());
paths.reserve(pathnames.size());
for(unsigned int p = 0; p < pathnames.size(); ++p)
{
paths.push_back(boost::filesystem::path(pathnames[p]));
paths.back().make_preferred();
}
#ifdef ARCHIVE
if(variables_map.count("archive"))
{
boost::replace_all(archive_name, "$GUID", guid_ss.str());
boost::replace_all(archive_name, "$DATE", date_ss.str());
boost::replace_all(archive_name, "$TIME", time_ss.str());
disk_writer = boost::shared_ptr< ::disk_writer>(new ::disk_writer(paths, archive_name, archive_format, data_format, autonumber, params, guid_type, log_level));
}
else
#endif
disk_writer = boost::shared_ptr< ::disk_writer>(new ::disk_writer(paths, data_format, autonumber, params, guid_type, log_level));
disk_writer_thread = boost::thread(boost::bind(&::disk_writer::run, disk_writer.get()));
//device->connect_image_callback(boost::bind(&::disk_writer::image_callback, disk_writer.get(), _1, _2));
//device->connect_obstacles_callback(boost::bind(&::disk_writer::obstacles_callback, disk_writer.get(), _1, _2));
//device->connect_terrain_callback(boost::bind(&::disk_writer::terrain_callback, disk_writer.get(), _1, _2));
//device->connect_motion_callback(boost::bind(&::disk_writer::motion_callback, disk_writer.get(), _1, _2));
device->connect_image_callback(boost::function, unsigned int)>(boost::bind(&::disk_writer::image_callback, disk_writer.get(), _1, _2)));
device->connect_obstacles_callback(boost::function >, unsigned int)>(boost::bind(&::disk_writer::obstacles_callback, disk_writer.get(), _1, _2)));
device->connect_terrain_callback(boost::function, unsigned int)>(boost::bind(&::disk_writer::terrain_callback, disk_writer.get(), _1, _2)));
device->connect_motion_callback( boost::function, unsigned int)>(boost::bind(&::disk_writer::motion_callback, disk_writer.get(), _1, _2)));
device->connect_classification_callback( boost::function, unsigned int)>(boost::bind(&::disk_writer::classification_callback, disk_writer.get(), _1, _2)));
}
#ifdef PCL
boost::thread display_thread;
boost::shared_ptr display;
if(variables_map.count("display"))
{
display = boost::shared_ptr< ::display>(new ::display(params.intrinsics, params.position, params.orientation, params.baseline, params.downsample, params.area_step,log_level));
display_thread = boost::thread(boost::bind(&::display::run, display.get()));
//device->connect_image_callback(boost::bind(image_callback_display, _1, _2, boost::ref(*display), log_level));
//device->connect_terrain_callback(boost::bind(terrain_callback_display, _1, _2, boost::ref(*display), log_level));
//device->connect_obstacles_callback(boost::bind(obstacles_callback_display, _1, _2, boost::ref(*display), log_level));
//device->connect_motion_callback(boost::bind(motion_callback_display, _1, _2, boost::ref(*display), log_level));
device->connect_image_callback(boost::function, unsigned int)>(boost::bind(image_callback_display, _1, _2, boost::ref(*display), log_level)));
device->connect_terrain_callback(boost::function, unsigned int)>(boost::bind(terrain_callback_display, _1, _2, boost::ref(*display), log_level)));
device->connect_obstacles_callback(boost::function >, unsigned int)>(boost::bind(obstacles_callback_display, _1, _2, boost::ref(*display), log_level)));
device->connect_motion_callback(boost::function, unsigned int)>(boost::bind(motion_callback_display, _1, _2, boost::ref(*display), log_level)));
device->connect_classification_callback(boost::function, unsigned int)>(boost::bind(classification_callback_display, _1, _2, boost::ref(*display), log_level)));
device->connect_classification_callback(boost::function, unsigned int)>(boost::bind(classification_callback_display, _1, _2, boost::ref(*display), log_level)));
//std::cout << "binded callback" <connect_timeout_callback(boost::bind(timeout_callback, disk_writer, display, boost::ref(sighandler_io_service)));
boost::asio::signal_set signals(sighandler_io_service, SIGINT, SIGTERM);
signals.async_wait(boost::bind(signal_handler, boost::asio::placeholders::error, boost::asio::placeholders::signal_number, disk_writer, display, boost::ref(sighandler_io_service)));
#else
device->connect_timeout_callback(boost::bind(timeout_callback, disk_writer, boost::ref(sighandler_io_service)));
boost::asio::signal_set signals(sighandler_io_service, SIGINT, SIGTERM);
signals.async_wait(boost::bind(signal_handler, boost::asio::placeholders::error, boost::asio::placeholders::signal_number, disk_writer, boost::ref(sighandler_io_service)));
#endif
device->stop_transmission(error);
if(error != lib3dv::error::NONE)
{
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
return error;
}
device->start_transmission(error);
if(error != lib3dv::error::NONE)
{
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
return error;
}
sighandler_io_service.run();
device->stop_transmission(error);
if(disk_writer)
{
disk_writer->stop();
disk_writer_thread.join();
}
#ifdef PCL
if(display)
{
display->stop();
display_thread.join();
}
#endif
std::cout << "[II] 3dv-client: operation completed" << std::endl;
return lib3dv::error::NONE;
}
if(variables_map.count("poweroff"))
{
device->poweroff(error);
if(error != lib3dv::error::NONE)
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
else
std::cout << "[EE] 3dv-client: operation completed" << std::endl;
return error;
}
if(variables_map.count("reboot"))
{
device->reboot(error);
if(error != lib3dv::error::NONE)
std::cout << "[EE] 3dv-client: device error " << error << std::endl;
else
std::cout << "[EE] 3dv-client: operation completed" << std::endl;
return error;
}
return lib3dv::error::NONE;
}