virtual void handle_stop()
{
if (acceptor_ && acceptor_->is_open())
{
acceptor_->close();
std::cout << "Server stopped" << std::endl;
}
acceptor_.reset();
}
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 );
}
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;
}
