source: pacpussensors/trunk/Vislab/20150310_lib3dv-1.2.0/src/main.cc

Last change on this file was 136, checked in by ldecherf, 8 years ago

Doc

File size: 29.5 KB
Line 
1/* 3dv-client/main.cc
2 *
3 * Copyright (C) 2013 VisLab
4 *
5 * This file is part of 3dv-client; you can redistribute it and/or modify
6 * it under the terms of the GNU Lesser General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU Lesser General Public License
16 * along with this program; if not, see <http://www.gnu.org/licenses/>.
17 */
18
19#include "disk_writer.h"
20#include "display.h"
21#include "utils.h"
22
23#include <lib3dv/3dv.h>
24#include "playback.h"
25
26#include <boost/algorithm/string/replace.hpp>
27#include <boost/asio/io_service.hpp>
28#include <boost/asio/signal_set.hpp>
29#include <boost/asio/ip/tcp.hpp>
30#include <boost/asio/ip/udp.hpp>
31#include <boost/asio/placeholders.hpp>
32#include <boost/cast.hpp>
33#include <boost/filesystem/path.hpp>
34#include <boost/program_options.hpp>
35#include <boost/property_tree/ptree.hpp>
36#include <boost/thread.hpp>
37#include <boost/uuid/uuid.hpp>
38#include <boost/uuid/uuid_io.hpp>
39
40#include <algorithm>
41#include <iostream>
42#include <sstream>
43#include <string>
44#include <vector>
45
46#define VERSION 20150310
47
48int main(int argc, char* argv[])
49{
50 std::string address;
51 std::vector<boost::asio::ip::address> addresses;
52 unsigned int log_level;
53 bool autonumber;
54 std::vector<std::string> pathnames;
55 data_file_format::types data_format;
56#ifdef ARCHIVE
57 archive_file_format::types archive_format;
58 std::string archive_name;
59#endif
60 std::string device_params_file_name;
61 std::vector<std::string> set_value;
62 std::string get_value;
63 boost::uuids::uuid device_guid;
64 uint64_t timeout;
65 std::vector<std::string> playback_value;
66
67 boost::program_options::typed_value<unsigned int>* log_option = new boost::program_options::typed_value<unsigned int>(&log_level);
68 boost::program_options::typed_value<std::string>* interface_option = new boost::program_options::typed_value<std::string>(&address);
69 boost::program_options::typed_value<std::vector<std::string> >* pathnames_option = new boost::program_options::typed_value<std::vector<std::string> >(&pathnames);
70 boost::program_options::typed_value<bool>* autonumber_option = new boost::program_options::typed_value<bool>(&autonumber);
71 boost::program_options::typed_value<data_file_format::types>* data_format_option = new boost::program_options::typed_value<data_file_format::types>(&data_format);
72#ifdef ARCHIVE
73 boost::program_options::typed_value<archive_file_format::types>* archive_format_option = new boost::program_options::typed_value<archive_file_format::types>(&archive_format);
74 boost::program_options::typed_value<std::string>* archive_name_option = new boost::program_options::typed_value<std::string>(&archive_name);
75#endif
76 boost::program_options::typed_value<std::string>* export_properties_option = new boost::program_options::typed_value<std::string>(&device_params_file_name);
77 boost::program_options::typed_value<std::vector<std::string> >* set_value_option = new boost::program_options::typed_value<std::vector<std::string> >(&set_value);
78 boost::program_options::typed_value<std::string>* get_value_option = new boost::program_options::typed_value<std::string>(&get_value);
79 boost::program_options::typed_value<boost::uuids::uuid>* device_guid_option = new boost::program_options::typed_value<boost::uuids::uuid>(&device_guid);
80 boost::program_options::typed_value<uint64_t>* timeout_option = new boost::program_options::typed_value<uint64_t>(&timeout);
81 boost::program_options::typed_value<std::vector<std::string> >* playback_option = new boost::program_options::typed_value<std::vector<std::string> >(&playback_value);
82 log_option->value_name("<level>");
83 log_option->default_value(0U);
84
85 interface_option->value_name("<address>");
86
87 pathnames_option->multitoken();
88 pathnames_option->value_name("<paths>");
89 pathnames_option->default_value(std::vector<std::string>(), "$GUID_$DATE-$TIME | .");
90
91 autonumber_option->value_name("<enable>");
92 autonumber_option->default_value(true, "true");
93
94 data_format_option->value_name("{raw|binary|text|gnuplot}");
95 data_format_option->default_value(data_file_format::TEXT, "text");
96
97#ifdef ARCHIVE
98 archive_name_option->default_value("$GUID_$DATE-$TIME");
99
100 archive_format_option->value_name("{zip|tar}");
101#ifdef __GNUC__
102 archive_format_option->default_value(archive_file_format::TAR, "tar");
103#else
104 archive_format_option->default_value(archive_file_format::ZIP, "zip");
105#endif
106#endif
107
108 export_properties_option->value_name("<file>");
109 export_properties_option->default_value("$GUID_$DATE-$TIME.xml");
110
111 set_value_option->multitoken();
112 set_value_option->value_name("<address> <value> {bool|int64|int32|int16|int8|uint64|uint32|uint16|uint8|float32|float64|string}");
113
114 get_value_option->value_name("<address>");
115
116 device_guid_option->value_name("<guid>");
117
118 timeout_option->value_name("<ms>");
119 timeout_option->default_value(lib3dv::device::DEFAULT_TIMEOUT.total_milliseconds());
120
121 playback_option->multitoken();
122 playback_option->value_name("<path>");
123
124
125 boost::program_options::options_description description("Options");
126
127 description.add_options()
128 ("help", "this help message")
129 ("log", log_option, "log level")
130 ("list-interfaces", "list the available network connections")
131 ("interface", interface_option, "the IP address of the local interface to bind to")
132 ("device-guid", device_guid_option, "target device GUID")
133 ("record", "perform recording")
134#ifdef PCL
135 ("display", "display live data")
136 ("playback",playback_option, "display recorded data in a given path")
137#endif
138 ("recording-paths", pathnames_option, "the recording path(s)")
139 ("autonumber", autonumber_option, "append the frame number to the file name")
140 ("data-format", data_format_option, "data files format")
141#ifdef ARCHIVE
142 ("archive", "use a single archive to store recorded files")
143 ("archive-name", archive_name_option, "archive name")
144 ("archive-format", archive_format_option, "archive file format")
145#endif
146 ("list-devices", "list devices")
147 ("list-properties", "list device properties")
148 ("export-properties", export_properties_option, "export device properties to an xml file")
149 ("save-properties", "save property values on the device")
150 ("reset-properties", "restore default property values on the device")
151 ("set-value", set_value_option, "set a property value")
152 ("get-value", get_value_option, "get a property value")
153 ("poweroff", "shut down the device")
154 ("reboot", "reboot the device")
155 ("timeout", timeout_option, "network timeout")
156 ("version", "program version");
157
158 boost::program_options::variables_map variables_map;
159
160 try
161 {
162 boost::program_options::store(boost::program_options::parse_command_line(argc, argv,
163 description,
164 boost::program_options::command_line_style::unix_style ^ boost::program_options::command_line_style::allow_short),
165 variables_map);
166 boost::program_options::notify(variables_map);
167
168 if(variables_map.count("help") || argc == 1)
169 {
170 std::cout << description << std::endl;
171 return 0;
172 }
173
174 if(variables_map.count("version"))
175 {
176 std::cout << "3dv-client " << VERSION << " using lib3dv " << LIB3DV_VERSION_MAJOR << '.' << LIB3DV_VERSION_MINOR << '.' << LIB3DV_VERSION_PATCH << std::endl;
177 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;
178
179 return 0;
180 }
181
182 if(variables_map.count("list-interfaces"))
183 {
184 addresses = list_interface_addresses();
185
186 if(addresses.empty())
187 {
188 std::cerr << "[EE] 3dv-client: " << "no configured address found" << std::endl;
189
190 return lib3dv::error::NETWORK_FAILURE;
191 }
192
193 std::cout << "[II] 3dv-client: available interfaces are:" << std::endl;
194
195 for(unsigned int a = 0; a < addresses.size(); ++a)
196 std::cout << "\t" << addresses[a].to_v4().to_string() << std::endl;
197
198 return lib3dv::error::NONE;
199 }
200
201 if(!variables_map.count("interface"))
202 {
203 addresses = list_interface_addresses();
204
205 if(addresses.empty())
206 {
207 std::cerr << "[EE] 3dv-client: " << "no configured address found" << std::endl;
208
209 return lib3dv::error::NETWORK_FAILURE;
210 }
211 }
212 else
213 addresses.push_back(boost::asio::ip::address_v4::from_string(address));
214 }
215 catch(boost::program_options::error& e)
216 {
217 std::cerr << "[EE] 3dv-client: " << e.what() << std::endl;
218 std::cout << description << std::endl;
219
220 return lib3dv::error::NONE;
221 }
222
223 std::vector<lib3dv::device> devices;
224 std::vector<lib3dv::device>::iterator device = devices.end();
225 lib3dv::error error;
226
227 if(variables_map.count("playback"))
228 {
229 return playback(playback_value,log_level);
230 }
231
232
233
234 for(unsigned int a = 0; a < addresses.size(); ++a)
235 {
236 std::cout << "[II] 3dv-client: detecting devices on " << addresses[a].to_v4().to_string() << std::endl;
237
238 std::vector<lib3dv::device> curr_devices = lib3dv::device::enumerate(addresses[a].to_v4(), log_level, error, boost::posix_time::milliseconds(timeout));
239
240 if(error != lib3dv::error::NONE)
241 std::cout << "[EE] 3dv-client: error detected: " << error << std::endl;
242 else
243 devices.insert(devices.end(), curr_devices.begin(), curr_devices.end());
244 }
245
246 if(devices.empty())
247 {
248 std::cout << "[EE] 3dv-client: no devices found" << std::endl;
249 return lib3dv::error::NONE;
250 }
251
252 if(variables_map.count("timeout"))
253 {
254 if(!devices.empty())
255 for(std::vector<lib3dv::device>::iterator dd = devices.begin(); dd != devices.end(); ++dd)
256 dd->timeout(boost::posix_time::milliseconds(timeout));
257 }
258
259 if(variables_map.count("list-devices"))
260 {
261 for(std::vector<lib3dv::device>::iterator dd = devices.begin(); dd != devices.end(); ++dd)
262 std::cout << '\t' << *dd << std::endl;
263
264 return lib3dv::error::NONE;
265 }
266
267 if(variables_map.count("device-guid"))
268 {
269 for(std::vector<lib3dv::device>::iterator dd = devices.begin(); dd != devices.end(); ++dd)
270 if(dd->guid() == device_guid)
271 {
272 device = dd;
273 break;
274 }
275
276 if(device == devices.end())
277 {
278 std::cerr << "[EE] 3dv-client: device " << device_guid << " not found" << std::endl;
279
280 return lib3dv::error::DEVICE_NOT_FOUND;
281 }
282 }
283 else
284 {
285 device = devices.begin();
286
287 if(devices.size() > 1)
288 std::cout << "[WW] 3dv-client: multiple devices found, but --device-guid not specified" << std::endl;
289 }
290
291 std::cout << "[II] 3dv-client: selected device " << device->guid() << std::endl;
292
293 if(variables_map.count("list-properties"))
294 {
295 std::vector<lib3dv::device::property> properties = device->enumerate_properties(error);
296
297 if(error == lib3dv::error::NONE)
298 {
299 std::cout << "[II] 3dv-client: device properties:" << std::endl;
300
301 for(unsigned int p = 0; p < properties.size(); ++p)
302 std::cout << '\t' << properties[p] << std::endl;
303 }
304 else
305 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
306
307 return error;
308 }
309
310 if(variables_map.count("save-properties"))
311 {
312 device->save_properties(error);
313
314 if(error != lib3dv::error::NONE)
315 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
316 else
317 std::cout << "[EE] 3dv-client: operation completed" << std::endl;
318
319 return error;
320 }
321
322 if(variables_map.count("reset-properties"))
323 {
324 device->reset_properties(error);
325
326 if(error != lib3dv::error::NONE)
327 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
328 else
329 std::cout << "[EE] 3dv-client: operation completed" << std::endl;
330
331 return error;
332 }
333
334 if(variables_map.count("set-value"))
335 {
336 if(set_value.size() == 3)
337 {
338 std::stringstream ss;
339 uint16_t address;
340 const std::string& value = set_value[1];
341 const std::string& type = set_value[2];
342
343 ss << std::hex << set_value[0];
344 ss >> address;
345
346 std::cout << "[II] 3dv-client: setting property " << set_value[0] << " value to " << value << std::endl;
347
348 if(type == "bool") device->set_property_value(address, boost::any(boost::lexical_cast<bool>(value)), error);
349 else if(type == "int64") device->set_property_value(address, boost::any(boost::lexical_cast<int64_t>(value)), error);
350 else if(type == "int32") device->set_property_value(address, boost::any(boost::numeric_cast<int32_t>(boost::lexical_cast<int64_t>(value))), error);
351 else if(type == "int16") device->set_property_value(address, boost::any(boost::numeric_cast<int16_t>(boost::lexical_cast<int64_t>(value))), error);
352 else if(type == "int8") device->set_property_value(address, boost::any(boost::numeric_cast<int8_t>(boost::lexical_cast<int64_t>(value))), error);
353 else if(type == "uint64") device->set_property_value(address, boost::any(boost::lexical_cast<uint64_t>(value)), error);
354 else if(type == "uint32") device->set_property_value(address, boost::any(boost::numeric_cast<uint32_t>(boost::lexical_cast<uint64_t>(value))), error);
355 else if(type == "uint16") device->set_property_value(address, boost::any(boost::numeric_cast<uint16_t>(boost::lexical_cast<uint64_t>(value))), error);
356 else if(type == "uint8") device->set_property_value(address, boost::any(boost::numeric_cast<uint8_t>(boost::lexical_cast<uint64_t>(value))), error);
357 else if(type == "float32") device->set_property_value(address, boost::any(boost::lexical_cast<float>(value)), error);
358 else if(type == "float64") device->set_property_value(address, boost::any(boost::lexical_cast<double>(value)), error);
359 else if(type == "string") device->set_property_value(address, boost::any(value), error);
360 else std::cout << "[EE] 3dv-client: unsupported data type " << type << std::endl;
361
362 if(error != lib3dv::error::NONE)
363 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
364 else
365 std::cout << "[EE] 3dv-client: operation completed" << std::endl;
366
367 return error;
368 }
369 else
370 {
371 std::cerr << "[EE] 3dv-client: exactly 3 arguments must be provided to --set-value" << std::endl;
372
373 return lib3dv::error::NONE;
374 }
375 }
376
377 if(variables_map.count("get-value"))
378 {
379 std::stringstream ss;
380 uint16_t address;
381
382 ss << std::hex << get_value;
383 ss >> address;
384
385 const boost::any& value = device->get_property_value(address, error);
386
387 if(error != lib3dv::error::NONE)
388 {
389 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
390
391 return error;
392 }
393
394 const std::type_info& type = value.type();
395
396 std::cout << "[II] 3dv-client: property " << get_value << " value is ";
397
398 if(type == typeid(bool)) std::cout << boost::any_cast<bool>(value) << std::endl;
399 else if(type == typeid(int64_t)) std::cout << boost::any_cast<int64_t>(value) << std::endl;
400 else if(type == typeid(int32_t)) std::cout << boost::any_cast<int32_t>(value) << std::endl;
401 else if(type == typeid(int32_t)) std::cout << boost::any_cast<int32_t>(value) << std::endl;
402 else if(type == typeid(int16_t)) std::cout << boost::any_cast<int16_t>(value) << std::endl;
403 else if(type == typeid(int8_t)) std::cout << static_cast<int32_t>(boost::any_cast<int8_t>(value)) << std::endl;
404 else if(type == typeid(uint64_t)) std::cout << boost::any_cast<uint64_t>(value) << std::endl;
405 else if(type == typeid(uint32_t)) std::cout << boost::any_cast<uint32_t>(value) << std::endl;
406 else if(type == typeid(uint32_t)) std::cout << boost::any_cast<uint32_t>(value) << std::endl;
407 else if(type == typeid(uint16_t)) std::cout << boost::any_cast<uint16_t>(value) << std::endl;
408 else if(type == typeid(uint8_t)) std::cout << static_cast<uint32_t>(boost::any_cast<uint8_t>(value)) << std::endl;
409 else if(type == typeid(float)) std::cout << boost::any_cast<float>(value) << std::endl;
410 else if(type == typeid(double)) std::cout << boost::any_cast<double>(value) << std::endl;
411 else if(type == typeid(std::string)) std::cout << boost::any_cast<std::string>(value) << std::endl;
412
413 return lib3dv::error::NONE;
414 }
415
416 if(variables_map.count("record")
417#ifdef PCL
418 || variables_map.count("display")
419#endif
420 )
421 {
422 boost::asio::io_service sighandler_io_service;
423 boost::asio::io_service::work sighandler_io_service_work(sighandler_io_service);
424
425 boost::thread disk_writer_thread;
426 boost::shared_ptr<disk_writer> disk_writer;
427
428 device_params params;
429 params.downsample=(uint32_t)2;
430
431 std::vector<lib3dv::device::property> properties = device->enumerate_properties(error);
432
433 if(error == lib3dv::error::NONE)
434 {
435 for(unsigned int p = 0; p < properties.size(); ++p)
436 {
437 const std::string& name = properties[p].m_name;
438 const boost::any& value = properties[p].m_value;
439
440 if(name == "calibration.u0")
441 params.intrinsics.m_u0 = boost::any_cast<double>(value);
442 else if(name == "calibration.v0")
443 params.intrinsics.m_v0 = boost::any_cast<double>(value);
444 else if(name == "calibration.ku")
445 params.intrinsics.m_ku = boost::any_cast<double>(value);
446 else if(name == "calibration.kv")
447 params.intrinsics.m_kv = boost::any_cast<double>(value);
448 else if(name == "calibration.x")
449 params.position.m_x = boost::any_cast<double>(value);
450 else if(name == "calibration.y")
451 params.position.m_y = boost::any_cast<double>(value);
452 else if(name == "calibration.z")
453 params.position.m_z = boost::any_cast<double>(value);
454 else if(name == "calibration.yaw")
455 params.orientation.m_yaw = boost::any_cast<double>(value);
456 else if(name == "calibration.pitch")
457 params.orientation.m_pitch = boost::any_cast<double>(value);
458 else if(name == "calibration.roll")
459 params.orientation.m_roll = boost::any_cast<double>(value);
460 else if(name == "calibration.baseline")
461 params.baseline = boost::any_cast<double>(value);
462 else if(name == "depth mapping.downsample ratio")
463 params.downsample =boost::any_cast<uint32_t>(value);
464 else if(name =="advanced.detection.area.step")
465 params.area_step = boost::any_cast<double>(value);
466 }
467 }
468 else
469 {
470 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
471 return error;
472 }
473
474 if(variables_map.count("record"))
475 {
476 if(pathnames.empty())
477 pathnames.push_back(variables_map.count("archive") ? "." : "$GUID_$DATE-$TIME");
478
479 std::vector<boost::filesystem::path> paths;
480
481 // remove duplicates
482 std::sort(pathnames.begin(), pathnames.end());
483 pathnames.erase(std::unique(pathnames.begin(), pathnames.end()), pathnames.end());
484
485 // expand variables
486 std::stringstream guid_ss;
487 guid_ss << device->guid();
488
489 //Check 3DV application version
490 uint32_t app_version=device->version().m_application[2]+device->version().m_application[1]*100+device->version().m_application[0]*10000;
491
492 uint8_t guid_type=9;
493
494 if (app_version <10100) guid_type=7;
495 else if (app_version <10200) guid_type=8;
496
497 for(unsigned int p = 0; p < pathnames.size(); ++p)
498 boost::replace_all(pathnames[p], "$GUID", guid_ss.str());
499
500 boost::posix_time::ptime timestamp(boost::posix_time::second_clock::local_time());
501
502 std::stringstream date_ss;
503 boost::posix_time::time_facet* date_facet = new boost::posix_time::time_facet("%Y%m%d");
504 date_ss.imbue(std::locale(date_ss.getloc(), date_facet));
505 date_ss << timestamp;
506
507 for(unsigned int p = 0; p < pathnames.size(); ++p)
508 boost::replace_all(pathnames[p], "$DATE", date_ss.str());
509
510 std::stringstream time_ss;
511 boost::posix_time::time_facet* time_facet = new boost::posix_time::time_facet("%H%M%S");
512 time_ss.imbue(std::locale(time_ss.getloc(), time_facet));
513 time_ss << timestamp;
514
515 for(unsigned int p = 0; p < pathnames.size(); ++p)
516 boost::replace_all(pathnames[p], "$TIME", time_ss.str());
517
518 paths.reserve(pathnames.size());
519
520 for(unsigned int p = 0; p < pathnames.size(); ++p)
521 {
522 paths.push_back(boost::filesystem::path(pathnames[p]));
523 paths.back().make_preferred();
524 }
525#ifdef ARCHIVE
526
527 if(variables_map.count("archive"))
528 {
529 boost::replace_all(archive_name, "$GUID", guid_ss.str());
530 boost::replace_all(archive_name, "$DATE", date_ss.str());
531 boost::replace_all(archive_name, "$TIME", time_ss.str());
532
533 disk_writer = boost::shared_ptr< ::disk_writer>(new ::disk_writer(paths, archive_name, archive_format, data_format, autonumber, params, guid_type, log_level));
534 }
535 else
536#endif
537 disk_writer = boost::shared_ptr< ::disk_writer>(new ::disk_writer(paths, data_format, autonumber, params, guid_type, log_level));
538
539 disk_writer_thread = boost::thread(boost::bind(&::disk_writer::run, disk_writer.get()));
540
541 //device->connect_image_callback(boost::bind(&::disk_writer::image_callback, disk_writer.get(), _1, _2));
542 //device->connect_obstacles_callback(boost::bind(&::disk_writer::obstacles_callback, disk_writer.get(), _1, _2));
543 //device->connect_terrain_callback(boost::bind(&::disk_writer::terrain_callback, disk_writer.get(), _1, _2));
544 //device->connect_motion_callback(boost::bind(&::disk_writer::motion_callback, disk_writer.get(), _1, _2));
545 device->connect_image_callback(boost::function<void(boost::shared_ptr< const lib3dv::image>, unsigned int)>(boost::bind(&::disk_writer::image_callback, disk_writer.get(), _1, _2)));
546 device->connect_obstacles_callback(boost::function<void (boost::shared_ptr< const std::vector<lib3dv::obstacle> >, unsigned int)>(boost::bind(&::disk_writer::obstacles_callback, disk_writer.get(), _1, _2)));
547 device->connect_terrain_callback(boost::function<void(boost::shared_ptr< const lib3dv::terrain>, unsigned int)>(boost::bind(&::disk_writer::terrain_callback, disk_writer.get(), _1, _2)));
548 device->connect_motion_callback( boost::function<void(boost::shared_ptr< const lib3dv::motion>, unsigned int)>(boost::bind(&::disk_writer::motion_callback, disk_writer.get(), _1, _2)));
549 device->connect_classification_callback( boost::function<void(boost::shared_ptr< const lib3dv::classification >, unsigned int)>(boost::bind(&::disk_writer::classification_callback, disk_writer.get(), _1, _2)));
550 }
551 #ifdef PCL
552
553 boost::thread display_thread;
554 boost::shared_ptr<display> display;
555
556 if(variables_map.count("display"))
557 {
558
559 display = boost::shared_ptr< ::display>(new ::display(params.intrinsics, params.position, params.orientation, params.baseline, params.downsample, params.area_step,log_level));
560 display_thread = boost::thread(boost::bind(&::display::run, display.get()));
561
562 //device->connect_image_callback(boost::bind(image_callback_display, _1, _2, boost::ref(*display), log_level));
563 //device->connect_terrain_callback(boost::bind(terrain_callback_display, _1, _2, boost::ref(*display), log_level));
564 //device->connect_obstacles_callback(boost::bind(obstacles_callback_display, _1, _2, boost::ref(*display), log_level));
565 //device->connect_motion_callback(boost::bind(motion_callback_display, _1, _2, boost::ref(*display), log_level));
566 device->connect_image_callback(boost::function<void(boost::shared_ptr<const lib3dv::image>, unsigned int)>(boost::bind(image_callback_display, _1, _2, boost::ref(*display), log_level)));
567 device->connect_terrain_callback(boost::function<void(boost::shared_ptr<const lib3dv::terrain>, unsigned int)>(boost::bind(terrain_callback_display, _1, _2, boost::ref(*display), log_level)));
568 device->connect_obstacles_callback(boost::function<void (boost::shared_ptr<const std::vector<lib3dv::obstacle> >, unsigned int)>(boost::bind(obstacles_callback_display, _1, _2, boost::ref(*display), log_level)));
569 device->connect_motion_callback(boost::function<void (boost::shared_ptr< const lib3dv::motion>, unsigned int)>(boost::bind(motion_callback_display, _1, _2, boost::ref(*display), log_level)));
570 device->connect_classification_callback(boost::function<void (boost::shared_ptr< const lib3dv::classification>, unsigned int)>(boost::bind(classification_callback_display, _1, _2, boost::ref(*display), log_level)));
571 device->connect_classification_callback(boost::function<void (boost::shared_ptr< const lib3dv::classification>, unsigned int)>(boost::bind(classification_callback_display, _1, _2, boost::ref(*display), log_level)));
572 //std::cout << "binded callback" <<std::endl;
573 }
574
575 device->connect_timeout_callback(boost::bind(timeout_callback, disk_writer, display, boost::ref(sighandler_io_service)));
576 boost::asio::signal_set signals(sighandler_io_service, SIGINT, SIGTERM);
577 signals.async_wait(boost::bind(signal_handler, boost::asio::placeholders::error, boost::asio::placeholders::signal_number, disk_writer, display, boost::ref(sighandler_io_service)));
578#else
579
580 device->connect_timeout_callback(boost::bind(timeout_callback, disk_writer, boost::ref(sighandler_io_service)));
581 boost::asio::signal_set signals(sighandler_io_service, SIGINT, SIGTERM);
582 signals.async_wait(boost::bind(signal_handler, boost::asio::placeholders::error, boost::asio::placeholders::signal_number, disk_writer, boost::ref(sighandler_io_service)));
583#endif
584
585 device->stop_transmission(error);
586
587 if(error != lib3dv::error::NONE)
588 {
589 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
590
591 return error;
592 }
593
594 device->start_transmission(error);
595
596 if(error != lib3dv::error::NONE)
597 {
598 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
599
600 return error;
601 }
602
603 sighandler_io_service.run();
604
605 device->stop_transmission(error);
606
607 if(disk_writer)
608 {
609 disk_writer->stop();
610 disk_writer_thread.join();
611 }
612
613#ifdef PCL
614 if(display)
615 {
616 display->stop();
617 display_thread.join();
618 }
619#endif
620
621 std::cout << "[II] 3dv-client: operation completed" << std::endl;
622
623 return lib3dv::error::NONE;
624 }
625
626 if(variables_map.count("poweroff"))
627 {
628 device->poweroff(error);
629
630 if(error != lib3dv::error::NONE)
631 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
632 else
633 std::cout << "[EE] 3dv-client: operation completed" << std::endl;
634
635 return error;
636 }
637
638 if(variables_map.count("reboot"))
639 {
640 device->reboot(error);
641
642 if(error != lib3dv::error::NONE)
643 std::cout << "[EE] 3dv-client: device error " << error << std::endl;
644 else
645 std::cout << "[EE] 3dv-client: operation completed" << std::endl;
646
647 return error;
648 }
649
650 return lib3dv::error::NONE;
651}
Note: See TracBrowser for help on using the repository browser.