source: pacpussensors/trunk/Vislab/lib3dv-1.2.0/lib3dv/detail/device_impl.hpp@ 141

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

Doc
File size: 7.2 KB
Line 
1#ifndef LIB3DV_DETAIL_ENDPOINT_IMPL_HPP
2#define LIB3DV_DETAIL_ENDPOINT_IMPL_HPP
3
4/* lib3dv/detail/endpoint_impl.hpp
5 *
6 * Copyright (C) 2013 VisLab
7 *
8 * This file is part of lib3dv; you can redistribute it and/or modify
9 * it under the terms of the GNU Lesser General Public License as published by
10 * the Free Software Foundation; either version 3 of the License, or (at
11 * your option) any later version.
12 *
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 * General Public License for more details.
17 *
18 * You should have received a copy of the GNU Lesser General Public License
19 * along with this program; if not, see <http://www.gnu.org/licenses/>.
20 */
21
22#include "protocol.h"
23
24#include <boost/shared_ptr.hpp>
25
26#include <algorithm>
27#include <iterator>
28
29template<typename C, typename F, typename H>
30typename C::mapped_type lib3dv::detail::device_impl::init_reassembly_container(C& container, F& fragments_container, const H& data_info_header, uint32_t key)
31{
32 typedef typename C::mapped_type::element_type data_type;
33 typedef typename F::mapped_type fragments_type;
34
35 boost::shared_ptr<data_type> data = boost::shared_ptr<data_type>(new data_type());
36 container[key] = data;
37
38 data->m_header = data_info_header;
39 data->m_buffer.resize(data_info_header.m_size);
40
41 // the following is needed to handle out-of-order image fragments arriving before the corresponding image header
42 const fragments_type& data_fragments = fragments_container[key];
43
44 if(!data_fragments.empty())
45 {
46 data->m_fragments.resize(std::max(data_fragments.front()->m_total_fragments, 1U)); // it's time to initialize the bitset
47
48 for(size_t f = 0; f < data_fragments.size(); ++f)
49 {
50 std::copy(data_fragments[f]->m_buffer.begin(), data_fragments[f]->m_buffer.end(), data->m_buffer.data() + data_fragments[f]->m_header.m_offset);
51 data->m_fragments.set(data_fragments[f]->m_fragment);
52 }
53
54 if(data->m_fragments.count() == data->m_fragments.size())
55 container.erase(key);
56 else
57 data.reset();
58 }
59 else
60 data.reset();
61
62
63 fragments_container.erase(key);
64
65 return data;
66}
67
68template<typename C, typename F>
69typename C::mapped_type lib3dv::detail::device_impl::fill_reassembly_container(C& container, F& fragments_container, size_t bytes_received, uint32_t key, uint32_t fragment, uint32_t total_fragments)
70{
71 typedef typename C::mapped_type::element_type data_type;
72 typedef typename F::mapped_type::value_type::element_type fragments_type;
73
74 const protocol::data_header* raw_data_header = reinterpret_cast<const protocol::data_header*>(m_data_payload.c_array() + sizeof(protocol::packet_header));
75 const uint8_t* payload_data = m_data_payload.c_array() + sizeof(protocol::packet_header) + sizeof(protocol::data_header);
76 uint32_t payload_size = static_cast<uint32_t>(bytes_received - (sizeof(protocol::packet_header) + sizeof(protocol::data_header)));
77
78 if(m_log_level == 1)
79 {
80 std::cout << "°";
81 std::cout.flush();
82 }
83
84 boost::shared_ptr<data_type> data;
85
86 typename C::iterator dd = container.find(key);
87
88 // we check that the data info header has already been received
89 if(dd != container.end())
90 data = dd->second;
91
92 if(data && (!data->m_buffer.empty() || !data->m_header.m_size))
93 {
94 if(!data->m_header.m_size)
95 {
96 container.erase(key);
97 return data;
98 }
99
100
101 std::copy(payload_data, payload_data + payload_size, data->m_buffer.data() + raw_data_header->m_offset);
102
103 if(data->m_fragments.empty()) // the first data fragment has been received, it's time to initialize the bitset
104 data->m_fragments.resize(std::max(total_fragments,1U));
105
106 data->m_fragments.set(fragment);
107
108 if(data->m_fragments.count() == data->m_fragments.size())
109 {
110 container.erase(key);
111 return data;
112 }
113 }
114 else // the fragment arrived before the corresponding header, we keep it aside for later processing
115 {
116 boost::shared_ptr<fragments_type> fragments = boost::shared_ptr<fragments_type>(new fragments_type());
117 fragments->m_header = *raw_data_header;
118 fragments->m_buffer.resize(payload_size);
119 fragments->m_fragment = fragment;
120 fragments->m_total_fragments = total_fragments;
121 std::copy(payload_data, payload_data + payload_size, fragments->m_buffer.data());
122 fragments_container[key].push_back(fragments);
123 }
124
125 return boost::shared_ptr<data_type>();
126}
127
128template<typename C, typename F>
129void lib3dv::detail::device_impl::cleanup_reassembly_container(C& container, F& fragments_container, size_t max_size)
130{
131 typedef typename C::iterator container_iterator_type;
132 typedef typename F::iterator fragments_container_iterator_type;
133
134 if(m_log_level > 3){
135 std::cout << "container size "<<container.size()<< " ";
136 if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_image_type> > >::type::value){
137 std::cout << "image!" << std::endl;
138 }
139 if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_terrain_type> > >::type::value){
140 std::cout << "terrain!" << std::endl;
141 }
142 if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_obstacles_type> > >::type::value){
143 std::cout << "obstacle!" << std::endl;
144 }
145 if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_motion_type> > >::type::value){
146 std::cout << "motion!" << std::endl;
147 }
148 if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_candidate_type> > >::type::value){
149 std::cout << "classification!" << std::endl;
150 }
151
152 }
153
154 if(container.size() > max_size)
155 {
156 std::cout << "[WW] lib3dv: dropping " << container.size() - max_size << " objects ";
157 if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_image_type> > >::type::value){
158 std::cout << "image!" << std::endl;
159 }
160 if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_terrain_type> > >::type::value){
161 std::cout << "terrain!" << std::endl;
162 }
163 if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_obstacles_type> > >::type::value){
164 std::cout << "obstacle!" << std::endl;
165 }
166 if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_motion_type> > >::type::value){
167 std::cout << "motion!" << std::endl;
168 }
169 container_iterator_type bb = container.begin();
170 container_iterator_type ee = container.begin();
171 std::advance(ee, container.size() - max_size);
172
173 container.erase(bb, ee);
174 }
175
176 if(fragments_container.size() > max_size)
177 {
178 std::cout << "[WW] lib3dv: dropping " << fragments_container.size() - max_size << " object fragments!" << std::endl;
179
180 fragments_container_iterator_type bb = fragments_container.begin();
181 fragments_container_iterator_type ee = fragments_container.begin();
182 std::advance(ee, fragments_container.size() - max_size);
183
184 fragments_container.erase(bb, ee);
185 }
186}
187
188#endif
Note: See TracBrowser for help on using the repository browser.