velodyne_stream.h

// This file is part of snark, a generic and flexible library
// for robotics research.
//
// Copyright (C) 2011 The University of Sydney
//
// snark is free software; 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.
//
// snark 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 Lesser General Public License
// for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with snark. If not, see <http://www.gnu.org/licenses/>.

#ifndef SNARK_SENSORS_VELODYNE_VELODYNESTREAM_H_
#define SNARK_SENSORS_VELODYNE_VELODYNESTREAM_H_

#ifndef WIN32
#include <stdlib.h>
#endif
#include <snark/sensors/velodyne/stream.h>
#include <snark/visiting/eigen.h>

namespace snark {

struct velodyne_point
{
    boost::posix_time::ptime timestamp;
    comma::uint32 id;
    comma::uint32 intensity;
    std::pair< ::Eigen::Vector3d, ::Eigen::Vector3d > ray;
    double azimuth;
    double range;
    bool valid;
    comma::uint32 scan;
};

template < typename S >
class velodyne_stream
{   
public:    
    velodyne_stream( const velodyne::db& db
                  , bool outputInvalidpoints
                  , boost::optional< std::size_t > from = boost::optional< std::size_t >(), boost::optional< std::size_t > to = boost::optional< std::size_t >() );

    template < typename P >
    velodyne_stream( const P& p
                  , const velodyne::db& db
                  , bool outputInvalidpoints
                  , boost::optional< std::size_t > from = boost::optional< std::size_t >(), boost::optional< std::size_t > to = boost::optional< std::size_t >() );

    bool read();
    const velodyne_point& point() const { return m_point; }

private:
    velodyne::stream< S > m_stream;
    velodyne::db m_db;
    velodyne_point m_point;
    boost::optional< std::size_t > m_to;
};

template < typename S >
velodyne_stream< S >::velodyne_stream ( const velodyne::db& db, bool outputInvalidpoints
                    , boost::optional< std::size_t > from
                    , boost::optional< std::size_t > to ):
    m_stream( new S, outputInvalidpoints ),
    m_db( db ),
    m_to( to )
{
    if( from ) { while( m_stream.scan() < *from ) { m_stream.skipscan(); } }
}

template < typename S >
template < typename P >
velodyne_stream< S >::velodyne_stream ( const P& p, const velodyne::db& db, bool outputInvalidpoints
                    , boost::optional< std::size_t > from
                    , boost::optional< std::size_t > to ):
    m_stream( new S( p ), outputInvalidpoints ),
    m_db( db ),
    m_to( to )
{
    if( from ) { while( m_stream.scan() < *from ) { m_stream.skipscan(); } }
}

template < typename S >
bool velodyne_stream< S >::read()
{
    if( m_to && m_stream.scan() > *m_to )
    {
        return false;
    }

    const velodyne::laser_return* r = m_stream.read();

    if( r == NULL ) { return false; }
    m_point.timestamp = r->timestamp;
    m_point.id = r->id;
    m_point.intensity = r->intensity;
    m_point.valid = !comma::math::equal( r->range, 0 ); // quick and dirty
    m_point.ray = m_db.lasers[ m_point.id ].ray( r->range, r->azimuth );
    m_point.range = m_db.lasers[ m_point.id ].range( r->range );
    m_point.scan = m_stream.scan();
    m_point.azimuth = m_db.lasers[ m_point.id ].azimuth( r->azimuth );
    return true;
}

template <>
class velodyne_stream< comma::csv::input_stream< velodyne_point> >
{
public:

    velodyne_stream( const comma::csv::options& options ): m_stream( std::cin, options ) {}

    bool read();
    const velodyne_point& point() const { return m_point; }

private:
    comma::csv::input_stream< velodyne_point > m_stream;
    velodyne_point m_point;
};

inline bool velodyne_stream< comma::csv::input_stream< velodyne_point> >::read()
{
    const velodyne_point* point = m_stream.read();
    if( point == NULL )
    {
        return false;
    }
    m_point = *point;
    return true;
}


} 


namespace comma { namespace visiting {

template <> struct traits< snark::velodyne_point >
{    
    template < typename K, typename V > static void visit( const K&, snark::velodyne_point& p, V& v )
    {
        v.apply( "t", p.timestamp );
        v.apply( "id", p.id );
        v.apply( "intensity", p.intensity );
        v.apply( "ray", p.ray );
        v.apply( "azimuth", p.azimuth );
        v.apply( "range", p.range );
        v.apply( "valid", p.valid );
        v.apply( "scan", p.scan );
    }
    
    template < typename K, typename V > static void visit( const K&, const snark::velodyne_point& p, V& v )
    {
        v.apply( "t", p.timestamp );
        v.apply( "id", p.id );
        v.apply( "intensity", p.intensity );
        v.apply( "ray", p.ray );
        v.apply( "azimuth", p.azimuth );
        v.apply( "range", p.range );
        v.apply( "valid", p.valid );
        v.apply( "scan", p.scan );
    }
};

} } // namespace comma { namespace visiting {

#endif // SNARK_SENSORS_VELODYNE_VELODYNESTREAM_H_