Exemple #1
0
 virtual void handle_stop()
 {
     if (acceptor_ && acceptor_->is_open())
     {
         acceptor_->close();
         std::cout << "Server stopped" << std::endl;
     }
     acceptor_.reset();
 }
Exemple #2
0
int main( int ac, char** av )
{
    try
    {
        comma::command_line_options options( ac, av, usage );
        if( options.exists( "--format" ) ) { std::cout << "t,2d,b" << std::endl; return 0; }
        if( options.exists( "--pan-limits,--tilt-limits,--debug" )) { comma::verbose.init( true, "" ); }
        debug = options.exists( "--debug" );
        std::vector< std::string > v = options.unnamed( "--diff,--help,-h,--verbose,-v,--debug,--format,--output-if-changed", "--binary,-b,--fields,-f,--delimiter,-d,--pan-limits,--tilt-limits,--camera" );
        if( v.empty() ) { std::cerr << comma::verbose.app_name() << ": please specify port name" << std::endl; exit( 1 ); }
        if( v.size() > 1 ) { std::cerr << comma::verbose.app_name() << ": expected one serial port name, got \"" << comma::join( v, ' ' ) << std::endl; exit( 1 ); }
        std::string name = v[0];
        output_if_changed = options.exists( "--output-if-changed" );
        differential = options.exists( "--diff" );
        comma::verbose << "connecting to " << name << "..." << std::endl;
        protocol.reset( new quickset::ptcr::protocol( name ) );
        comma::verbose << "connected to " << name << std::endl;
        comma::csv::options input_csv( options );
        comma::csv::options output_csv;
        if( input_csv.fields == "" )
        {
            input_csv.fields = "pan,tilt";
            if( input_csv.binary() ) { output_csv.format( "t,2d,20b" ); }
        }
        output.reset( new comma::csv::output_stream< status >( std::cout, output_csv ) );
        set_limits( options.optional< std::string >( "--pan-limits" ), options.optional< std::string >( "--tilt-limits" ) );
        if( options.exists( "--camera" ) ) { set_camera( options.value< std::string >( "--camera" ) == "on" ); }
        input.reset( new comma::csv::input_stream< position >( std::cin, input_csv ) );
        comma::io::select select;
        select.read().add( 0 );
        const boost::posix_time::time_duration timeout = boost::posix_time::millisec( 20 );
        boost::posix_time::ptime deadline = boost::posix_time::microsec_clock::universal_time();
        bool ok = false;
        comma::verbose << "synchronizing..." << std::endl;
        // Consume any messages already queued for reading
        while( protocol->receive( boost::posix_time::millisec( 500 ))) {}
        while( !is_shutdown && std::cin.good() && !std::cin.eof() && std::cout.good() )
        {
            boost::posix_time::ptime now = boost::posix_time::microsec_clock::universal_time();
            bool was_ok = ok;
            get_status.command = ok ? 0 : quickset::ptcr::commands::get_status::command::res;
            if( now > deadline || get_status.command() ) { ok = handle_status(); deadline = now + timeout; }
            if( was_ok != ok ) { std::cerr << comma::verbose.app_name() << ": " << ( ok ? "synchronized" : "lost sync, synchronizing..." ) << std::endl; }
            if( ok )
            {
                if( target || select.wait( timeout ) != 0 ) { ok = handle_move_to(); }
            }
            else
            {
                boost::thread::sleep( deadline );
            }
        }
        if( is_shutdown ) { std::cerr << comma::verbose.app_name() << ": interrupted by signal" << std::endl; }
        protocol->close();
        comma::verbose << "done" << std::endl;
        return 0;
    }
    catch( std::exception& ex )
    {
        std::cerr << comma::verbose.app_name() << ": " << ex.what() << std::endl;
    }
    catch( ... )
    {
        std::cerr << comma::verbose.app_name() << ": unknown exception" << std::endl;
    }
    if( protocol ) { protocol->close(); }
    exit( 0 );
}
Exemple #3
0
void run( S* stream )
{
    static const unsigned int timeSize = 12;
    boost::mt19937 generator;
    boost::uniform_real< float > distribution( 0, 1 );
    boost::variate_generator< boost::mt19937&, boost::uniform_real< float > > random( generator, distribution );
    comma::uint64 count = 0;
    comma::uint64 dropped_count = 0;
    double compression = 0;
    velodyne::packet packet;
    comma::signal_flag isShutdown;
    velodyne::scan_tick tick;
    comma::uint32 scan_id = 0;
    while( !isShutdown && std::cin.good() && !std::cin.eof() && std::cout.good() && !std::cout.eof() )
    {
        const char* p = velodyne::impl::stream_traits< S >::read( *stream, sizeof( velodyne::packet ) );
        if( p == NULL ) { break; }
        ::memcpy( &packet, p, velodyne::packet::size );
        if( tick.is_new_scan( packet ) ) { ++scan_id; } // quick and dirty
        boost::posix_time::ptime timestamp = stream->timestamp();
        if( scan_rate ) { scan.thin( packet, *scan_rate, angularSpeed( packet ) ); }
        if( !scan_rate || !scan.empty() )
        {
            if( focus ) { velodyne::thin::thin( packet, *focus, *db, angularSpeed( packet ), random ); }
            if( rate ) { velodyne::thin::thin( packet, *rate, random ); }
        }
        const boost::posix_time::ptime base( snark::timing::epoch );
        const boost::posix_time::time_duration d = timestamp - base;
        comma::int64 seconds = d.total_seconds();
        comma::int32 nanoseconds = static_cast< comma::int32 >( d.total_microseconds() % 1000000 ) * 1000;
        if( outputRaw ) // real quick and dirty
        {
            static boost::array< char, 16 + timeSize + velodyne::packet::size + 4 > buf;
            static const boost::array< char, 2 > start = {{ -78, 85 }}; // see QLib::Bytestreams::GetDefaultStartDelimiter()
            static const boost::array< char, 2 > end = {{ 117, -97 }}; // see QLib::Bytestreams::GetDefaultStartDelimiter()
            ::memcpy( &buf[0], &start[0], 2 );
            ::memcpy( &buf[0] + buf.size() - 2, &end[0], 2 );
            ::memcpy( &buf[0] + 16, &seconds, 8 );
            ::memcpy( &buf[0] + 16 + 8, &nanoseconds, 4 );
            ::memcpy( &buf[0] + 16 + 8 + 4, &packet, velodyne::packet::size );
            if( publisher ) { publisher->write( &buf[0], buf.size() ); }
            else if( publisher_udp_socket ) { publisher_udp_socket->send_to( boost::asio::buffer( &buf[0], buf.size() ), udp_destination ); }
            else { std::cout.write( &buf[0], buf.size() ); }
        }
        else
        {
            // todo: certainly rewrite with the proper header using comma::packed
            static char buf[ timeSize + sizeof( comma::uint16 ) + velodyne::thin::maxBufferSize ];
            comma::uint16 size = velodyne::thin::serialize( packet, buf + timeSize + sizeof( comma::uint16 ), scan_id );
            bool empty = size == ( sizeof( comma::uint32 ) + 1 ); // todo: atrocious... i.e. packet is not empty; refactor!!!
            if( !empty )
            {
                size += timeSize;
                ::memcpy( buf, &size, sizeof( comma::uint16 ) );
                size += sizeof( comma::uint16 );
                ::memcpy( buf + sizeof( comma::uint16 ), &seconds, sizeof( comma::int64 ) );
                ::memcpy( buf + sizeof( comma::uint16 ) + sizeof( comma::int64 ), &nanoseconds, sizeof( comma::int32 ) );
                if( publisher ) { publisher->write( buf, size ); }
                else if( publisher_udp_socket ) { publisher_udp_socket->send_to( boost::asio::buffer( buf, size ), udp_destination ); }
                else { std::cout.write( buf, size ); }
            }
            else
            {
                ++dropped_count;
            }
            if( verbose )
            {
                ++count;
                compression = 0.9 * compression + 0.1 * ( empty ? 0.0 : double( size + sizeof( comma::int16 ) ) / ( velodyne::packet::size + timeSize ) );
                if( count % 10000 == 0 ) { std::cerr << "velodyne-thin: processed " << count << " packets; dropped " << ( double( dropped_count ) * 100. / count ) << "% full packets; compression rate " << compression << std::endl; }
            }
        }
    }
    if( publisher ) { publisher->close(); }
    std::cerr << "velodyne-thin: " << ( isShutdown ? "signal received" : "no more data" ) << "; shutdown" << std::endl;
}