| 1 | /* 3dv-client/disk_writer.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 |
|
|---|
| 21 | disk_writer::disk_writer(const std::vector<boost::filesystem::path>& paths, data_file_format::types data_file_format, bool autonumber, device_params params, uint8_t guid_type ,uint8_t log_level) :
|
|---|
| 22 | m_autonumber(autonumber),
|
|---|
| 23 | m_guid_type(guid_type),
|
|---|
| 24 | m_log_level(log_level)
|
|---|
| 25 | {
|
|---|
| 26 | for(unsigned int p = 0; p < paths.size(); ++p)
|
|---|
| 27 | {
|
|---|
| 28 | std::cout << "[II] 3dv-client: creating writing queue for path " << paths[p] << std::endl;
|
|---|
| 29 | m_disk_queues.push_back(boost::shared_ptr<disk_queue>(new disk_queue(paths[p], data_file_format, log_level)));
|
|---|
| 30 | if(!boost::filesystem::exists(paths[p]))
|
|---|
| 31 | {
|
|---|
| 32 | std::cout << "[II] 3dv-client: writing to path " << paths[p] << std::endl;
|
|---|
| 33 | boost::filesystem::create_directories(paths[p]);
|
|---|
| 34 | }
|
|---|
| 35 |
|
|---|
| 36 | std::string filename = "params.ini";
|
|---|
| 37 | boost::filesystem::path path_filename = paths[p] / filename;
|
|---|
| 38 |
|
|---|
| 39 | std::cout << "[II] 3dv-client: writing INI file " << path_filename << std::endl;
|
|---|
| 40 | std::ofstream file (path_filename.string().c_str(), std::ios::out);
|
|---|
| 41 | file <<"calibration.u0="<<params.intrinsics.m_u0<<std::endl;
|
|---|
| 42 | file <<"calibration.v0="<<params.intrinsics.m_v0<<std::endl;
|
|---|
| 43 | file <<"calibration.ku="<<params.intrinsics.m_ku<<std::endl;
|
|---|
| 44 | file <<"calibration.kv="<<params.intrinsics.m_kv<<std::endl;
|
|---|
| 45 | file <<"calibration.x="<<params.position.m_x<<std::endl;
|
|---|
| 46 | file <<"calibration.y="<<params.position.m_y<<std::endl;
|
|---|
| 47 | file <<"calibration.z="<<params.position.m_z<<std::endl;
|
|---|
| 48 | file <<"calibration.yaw="<<params.orientation.m_yaw<<std::endl;
|
|---|
| 49 | file <<"calibration.pitch="<<params.orientation.m_pitch<<std::endl;
|
|---|
| 50 | file <<"calibration.roll="<<params.orientation.m_roll<<std::endl;
|
|---|
| 51 | file <<"calibration.baseline="<<params.baseline<<std::endl;
|
|---|
| 52 | file <<"depth mapping.downsample ratio="<<params.downsample<<std::endl;
|
|---|
| 53 | file <<"advanced.detection.area.step="<<params.area_step<<std::endl;
|
|---|
| 54 | file.close();
|
|---|
| 55 | }
|
|---|
| 56 | }
|
|---|
| 57 |
|
|---|
| 58 | #ifdef ARCHIVE
|
|---|
| 59 | disk_writer::disk_writer(const std::vector<boost::filesystem::path>& paths, const std::string& archive_name, archive_file_format::types archive_format, data_file_format::types data_file_format, bool autonumber, device_params params, uint8_t guid_type, uint8_t log_level) :
|
|---|
| 60 | m_autonumber(autonumber),
|
|---|
| 61 | m_guid_type(guid_type),
|
|---|
| 62 | m_log_level(log_level)
|
|---|
| 63 | {
|
|---|
| 64 | for(unsigned int p = 0; p < paths.size(); ++p)
|
|---|
| 65 | {
|
|---|
| 66 | std::cout << "[II] 3dv-client: creating writing queue for path " << paths[p] << std::endl;
|
|---|
| 67 | m_disk_queues.push_back(boost::shared_ptr<disk_queue>(new disk_queue(paths[p], archive_name, archive_format, data_file_format,params,log_level)));
|
|---|
| 68 | }
|
|---|
| 69 | }
|
|---|
| 70 | #endif
|
|---|
| 71 |
|
|---|
| 72 | void disk_writer::run()
|
|---|
| 73 | {
|
|---|
| 74 | for(unsigned int q = 0; q < m_disk_queues.size(); ++q)
|
|---|
| 75 | m_disk_queue_threads.create_thread(boost::bind(&boost::asio::io_service::run, &m_disk_queues[q]->m_io_service));
|
|---|
| 76 |
|
|---|
| 77 | m_disk_queue_threads.join_all();
|
|---|
| 78 | }
|
|---|
| 79 |
|
|---|
| 80 | void disk_writer::stop()
|
|---|
| 81 | {
|
|---|
| 82 | for(unsigned int q = 0; q < m_disk_queues.size(); ++q)
|
|---|
| 83 | {
|
|---|
| 84 | delete m_disk_queues[q]->m_io_service_work;
|
|---|
| 85 | m_disk_queues[q]->m_io_service_work = NULL;
|
|---|
| 86 | }
|
|---|
| 87 | }
|
|---|
| 88 |
|
|---|
| 89 | void disk_writer::image_callback(boost::shared_ptr<const lib3dv::image> image, uint32_t guid)
|
|---|
| 90 | {
|
|---|
| 91 | int queue_id = (guid / m_guid_type) % m_disk_queues.size();
|
|---|
| 92 |
|
|---|
| 93 | #ifdef ARCHIVE
|
|---|
| 94 | if(m_disk_queues[queue_id]->m_archive)
|
|---|
| 95 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_image_archive, image, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_log_level));
|
|---|
| 96 | else
|
|---|
| 97 | #endif
|
|---|
| 98 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_image_file, image, guid, m_guid_type, m_disk_queues[queue_id]->m_path, m_autonumber, m_log_level));
|
|---|
| 99 | }
|
|---|
| 100 |
|
|---|
| 101 | void disk_writer::terrain_callback(boost::shared_ptr<const lib3dv::terrain> terrain, uint32_t guid)
|
|---|
| 102 | {
|
|---|
| 103 | int queue_id = (guid / m_guid_type) % m_disk_queues.size();
|
|---|
| 104 |
|
|---|
| 105 | #ifdef ARCHIVE
|
|---|
| 106 | if(m_disk_queues[queue_id]->m_archive)
|
|---|
| 107 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_terrain_archive, terrain, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
|
|---|
| 108 | else
|
|---|
| 109 | #endif
|
|---|
| 110 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_terrain_file, terrain, guid, m_guid_type,m_disk_queues[queue_id]->m_path, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
|
|---|
| 111 | }
|
|---|
| 112 |
|
|---|
| 113 | void disk_writer::obstacles_callback(boost::shared_ptr<const std::vector<lib3dv::obstacle> > obstacles, uint32_t guid)
|
|---|
| 114 | {
|
|---|
| 115 | int queue_id = (guid / m_guid_type) % m_disk_queues.size();
|
|---|
| 116 |
|
|---|
| 117 | #ifdef ARCHIVE
|
|---|
| 118 | if(m_disk_queues[queue_id]->m_archive)
|
|---|
| 119 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_obstacles_archive, obstacles, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
|
|---|
| 120 | else
|
|---|
| 121 | #endif
|
|---|
| 122 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_obstacles_file, obstacles, guid, m_guid_type, m_disk_queues[queue_id]->m_path, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
|
|---|
| 123 | }
|
|---|
| 124 |
|
|---|
| 125 | void disk_writer::motion_callback(boost::shared_ptr<const lib3dv::motion> motion, uint32_t guid)
|
|---|
| 126 | {
|
|---|
| 127 | int queue_id = (guid / m_guid_type) % m_disk_queues.size();
|
|---|
| 128 |
|
|---|
| 129 | #ifdef ARCHIVE
|
|---|
| 130 | if(m_disk_queues[queue_id]->m_archive)
|
|---|
| 131 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_motion_archive, motion, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
|
|---|
| 132 | else
|
|---|
| 133 | #endif
|
|---|
| 134 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_motion_file, motion, guid, m_guid_type, m_disk_queues[queue_id]->m_path, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
|
|---|
| 135 | }
|
|---|
| 136 |
|
|---|
| 137 | void disk_writer::classification_callback(boost::shared_ptr<const lib3dv::classification> classification, uint32_t guid)
|
|---|
| 138 | {
|
|---|
| 139 | int queue_id = (guid / m_guid_type) % m_disk_queues.size();
|
|---|
| 140 |
|
|---|
| 141 | #ifdef ARCHIVE
|
|---|
| 142 | if(m_disk_queues[queue_id]->m_archive)
|
|---|
| 143 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_classification_archive, classification, guid, m_guid_type, m_disk_queues[queue_id]->m_archive, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
|
|---|
| 144 | else
|
|---|
| 145 | #endif
|
|---|
| 146 | m_disk_queues[queue_id]->m_io_service.post(boost::bind(write_classification_file, classification, guid, m_guid_type, m_disk_queues[queue_id]->m_path, m_autonumber, m_disk_queues[queue_id]->m_data_file_format, m_log_level));
|
|---|
| 147 | }
|
|---|
| 148 |
|
|---|
| 149 | disk_writer::disk_queue::disk_queue(const boost::filesystem::path& path, data_file_format::types data_file_format, uint8_t log_level) :
|
|---|
| 150 | m_io_service_work(new boost::asio::io_service::work(m_io_service)),
|
|---|
| 151 | m_path(path),
|
|---|
| 152 | m_data_file_format(data_file_format),
|
|---|
| 153 | m_log_level(log_level)
|
|---|
| 154 | #ifdef ARCHIVE
|
|---|
| 155 | , m_archive(NULL)
|
|---|
| 156 | #endif
|
|---|
| 157 | {}
|
|---|
| 158 |
|
|---|
| 159 | #ifdef ARCHIVE
|
|---|
| 160 | disk_writer::disk_queue::disk_queue(const boost::filesystem::path& path, const std::string& archive_name, archive_file_format::types archive_format, data_file_format::types data_file_format,device_params params, uint8_t log_level) :
|
|---|
| 161 | m_io_service_work(new boost::asio::io_service::work(m_io_service)),
|
|---|
| 162 | m_path(path),
|
|---|
| 163 | m_data_file_format(data_file_format),
|
|---|
| 164 | m_log_level(log_level)
|
|---|
| 165 | {
|
|---|
| 166 | m_archive = open_archive(path, archive_name, archive_format,params, log_level);
|
|---|
| 167 | }
|
|---|
| 168 | #endif
|
|---|
| 169 |
|
|---|
| 170 | disk_writer::disk_queue::~disk_queue()
|
|---|
| 171 | {
|
|---|
| 172 | delete m_io_service_work;
|
|---|
| 173 | #ifdef ARCHIVE
|
|---|
| 174 | if(m_archive)
|
|---|
| 175 | close_archive(m_archive, m_log_level);
|
|---|
| 176 | #endif
|
|---|
| 177 | }
|
|---|
