#ifndef LIB3DV_DETAIL_ENDPOINT_IMPL_HPP
#define LIB3DV_DETAIL_ENDPOINT_IMPL_HPP
/* lib3dv/detail/endpoint_impl.hpp
*
* Copyright (C) 2013 VisLab
*
* This file is part of lib3dv; 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 "protocol.h"
#include
#include
#include
template
typename C::mapped_type lib3dv::detail::device_impl::init_reassembly_container(C& container, F& fragments_container, const H& data_info_header, uint32_t key)
{
typedef typename C::mapped_type::element_type data_type;
typedef typename F::mapped_type fragments_type;
boost::shared_ptr data = boost::shared_ptr(new data_type());
container[key] = data;
data->m_header = data_info_header;
data->m_buffer.resize(data_info_header.m_size);
// the following is needed to handle out-of-order image fragments arriving before the corresponding image header
const fragments_type& data_fragments = fragments_container[key];
if(!data_fragments.empty())
{
data->m_fragments.resize(std::max(data_fragments.front()->m_total_fragments, 1U)); // it's time to initialize the bitset
for(size_t f = 0; f < data_fragments.size(); ++f)
{
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);
data->m_fragments.set(data_fragments[f]->m_fragment);
}
if(data->m_fragments.count() == data->m_fragments.size())
container.erase(key);
else
data.reset();
}
else
data.reset();
fragments_container.erase(key);
return data;
}
template
typename 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)
{
typedef typename C::mapped_type::element_type data_type;
typedef typename F::mapped_type::value_type::element_type fragments_type;
const protocol::data_header* raw_data_header = reinterpret_cast(m_data_payload.c_array() + sizeof(protocol::packet_header));
const uint8_t* payload_data = m_data_payload.c_array() + sizeof(protocol::packet_header) + sizeof(protocol::data_header);
uint32_t payload_size = static_cast(bytes_received - (sizeof(protocol::packet_header) + sizeof(protocol::data_header)));
if(m_log_level == 1)
{
std::cout << "°";
std::cout.flush();
}
boost::shared_ptr data;
typename C::iterator dd = container.find(key);
// we check that the data info header has already been received
if(dd != container.end())
data = dd->second;
if(data && (!data->m_buffer.empty() || !data->m_header.m_size))
{
if(!data->m_header.m_size)
{
container.erase(key);
return data;
}
std::copy(payload_data, payload_data + payload_size, data->m_buffer.data() + raw_data_header->m_offset);
if(data->m_fragments.empty()) // the first data fragment has been received, it's time to initialize the bitset
data->m_fragments.resize(std::max(total_fragments,1U));
data->m_fragments.set(fragment);
if(data->m_fragments.count() == data->m_fragments.size())
{
container.erase(key);
return data;
}
}
else // the fragment arrived before the corresponding header, we keep it aside for later processing
{
boost::shared_ptr fragments = boost::shared_ptr(new fragments_type());
fragments->m_header = *raw_data_header;
fragments->m_buffer.resize(payload_size);
fragments->m_fragment = fragment;
fragments->m_total_fragments = total_fragments;
std::copy(payload_data, payload_data + payload_size, fragments->m_buffer.data());
fragments_container[key].push_back(fragments);
}
return boost::shared_ptr();
}
template
void lib3dv::detail::device_impl::cleanup_reassembly_container(C& container, F& fragments_container, size_t max_size)
{
typedef typename C::iterator container_iterator_type;
typedef typename F::iterator fragments_container_iterator_type;
if(m_log_level > 3){
std::cout << "container size "< > >::type::value){
std::cout << "image!" << std::endl;
}
if(boost::is_same > >::type::value){
std::cout << "terrain!" << std::endl;
}
if(boost::is_same > >::type::value){
std::cout << "obstacle!" << std::endl;
}
if(boost::is_same > >::type::value){
std::cout << "motion!" << std::endl;
}
if(boost::is_same > >::type::value){
std::cout << "classification!" << std::endl;
}
}
if(container.size() > max_size)
{
std::cout << "[WW] lib3dv: dropping " << container.size() - max_size << " objects ";
if(boost::is_same > >::type::value){
std::cout << "image!" << std::endl;
}
if(boost::is_same > >::type::value){
std::cout << "terrain!" << std::endl;
}
if(boost::is_same > >::type::value){
std::cout << "obstacle!" << std::endl;
}
if(boost::is_same > >::type::value){
std::cout << "motion!" << std::endl;
}
container_iterator_type bb = container.begin();
container_iterator_type ee = container.begin();
std::advance(ee, container.size() - max_size);
container.erase(bb, ee);
}
if(fragments_container.size() > max_size)
{
std::cout << "[WW] lib3dv: dropping " << fragments_container.size() - max_size << " object fragments!" << std::endl;
fragments_container_iterator_type bb = fragments_container.begin();
fragments_container_iterator_type ee = fragments_container.begin();
std::advance(ee, fragments_container.size() - max_size);
fragments_container.erase(bb, ee);
}
}
#endif