int main( int ac, char** av )
{
boost::scoped_ptr< boost::asio::ip::tcp::iostream > stream;
try
{
comma::command_line_options options( ac, av );
if( options.exists( "--help,-h" ) ) { usage(); }
verbose = options.exists( "--verbose,-v" );
std::vector< std::string > v = options.unnamed( "--help,-h,--get-status,--reset,--reset-dsp,--start,--stop,--verbose,-v,--no-discard,--no-flush", "--get,--set" );
std::vector< std::string > a = comma::split( ( v.empty() ? std::string( "192.168.0.1:12002" ) : v[0] ), ':' );
if( a.size() != 2 ) { std::cerr << "sick-ldmrs-stream: expected address, got \"" << v[0] << "\"" << std::endl; usage(); }
std::string address = a[0];
unsigned short port = boost::lexical_cast< unsigned short >( a[1] );
boost::asio::io_service service;
boost::asio::ip::tcp::resolver resolver( service );
boost::asio::ip::tcp::resolver::query query( a[0], a[1] );
boost::asio::ip::tcp::resolver::iterator it = resolver.resolve( query );
if( verbose ) { std::cerr << "sick-ldmrs-stream: connecting to " << it->endpoint() << "..." << std::endl; }
stream.reset( new boost::asio::ip::tcp::iostream( it->endpoint() ) );
if( !( *stream ) ) { COMMA_THROW( comma::exception, "failed to connect to " << address << ":" << port ); }
if( verbose ) { std::cerr << "sick-ldmrs-stream: connected to " << address << ":" << port << std::endl; }
protocol.reset( new sick::ibeo::protocol( *stream ) );
options.assert_mutually_exclusive( "--get,--get-status,--reset,--set,--reset-dsp,--start,--stop,--publish" );
bool ok = true;
if( options.exists( "--reset" ) )
{
sick::ibeo::commands::reset::response response = protocol->write( sick::ibeo::commands::reset() );
ok = response.ok();
if( ok ) { if( verbose ) { std::cerr << "sick-ldmrs-stream: reset to factory settings; next time connect to the default ip address" << std::endl; } }
clear_fault();
}
else if( options.exists( "--reset-dsp" ) )
{
protocol->reset_dsp();
clear_fault();
}
else if( options.exists( "--set" ) )
{
comma::name_value::map m( options.value< std::string >( "--set" ), ',' );
sick::ibeo::commands::set command;
for( comma::name_value::map::map_type::const_iterator it = m.get().begin(); ok && it != m.get().end(); ++it )
{
if( it->first == "address" )
{
command.index = sick::ibeo::commands::set::ip_address;
std::vector< std::string > bytes = comma::split( it->second, '.' );
if( bytes.size() != 4 ) { COMMA_THROW( comma::exception, "expected ip address, got " << it->second ); }
command.value.data()[0] = boost::lexical_cast< unsigned int >( bytes[3] ); // little endian
command.value.data()[1] = boost::lexical_cast< unsigned int >( bytes[2] );
command.value.data()[2] = boost::lexical_cast< unsigned int >( bytes[1] );
command.value.data()[3] = boost::lexical_cast< unsigned int >( bytes[0] );
}
else if( it->first == "port" )
{
command.index = sick::ibeo::commands::set::tcp_port;
command.value = boost::lexical_cast< unsigned short >( it->second ); // todo: sort out endianness!
}
else
{
COMMA_THROW( comma::exception, "expected parameter, got " << it->first );
}
ok = protocol->write( command ).ok();
if( !ok ) { std::cerr << "sick-ldmrs-stream: failed to set " << it->first << " to " << it->second << std::endl; }
clear_fault();
}
if( ok )
{
ok = protocol->write( sick::ibeo::commands::save_configuration() ).ok();
if( !ok ) { std::cerr << "sick-ldmrs-stream: failed to save configuration" << std::endl; }
else if( verbose ) { std::cerr << "sick-ldmrs-stream: saved configuration for " << options.value< std::string >( "--set" ) << std::endl; }
}
}
else if( options.exists( "--get" ) )
{
std::vector< std::string > v = comma::split( options.value< std::string >( "--get" ), ',' );
sick::ibeo::commands::get command;
sick::ibeo::commands::get::response response;
std::string comma;
for( std::size_t i = 0; ok && i < v.size(); ++i, ok = ok && response.ok() )
{
if( v[i] == "address" ) { command.index = sick::ibeo::commands::set::ip_address; }
else if( v[i] == "port" ) { command.index = sick::ibeo::commands::set::tcp_port; }
else { COMMA_THROW( comma::exception, "expected parameter, got " << v[i] ); }
response = protocol->write( command );
if( response.ok() ) { std::cout << comma << response << std::endl; }
comma = ",";
clear_fault();
}
}
else if( options.exists( "--get-status" ) )
{
sick::ibeo::commands::get_status::response response = protocol->write( sick::ibeo::commands::get_status() );
ok = response.ok();
if( ok ) { std::cout << response << std::endl; }
clear_fault();
}
else if( options.exists( "--start" ) )
{
ok = protocol->write( sick::ibeo::commands::start() ).ok();
clear_fault();
}
else if( options.exists( "--stop" ) )
{
ok = protocol->write( sick::ibeo::commands::stop() ).ok();
clear_fault();
}
else
{
std::string publisher_name;
boost::shared_ptr< comma::io::publisher > publisher;
if( options.exists( "--publish" ) )
{
publisher_name = options.value< std::string >( "--publish" );
bool blocking = options.exists( "--no-discard" );
bool flush = !options.exists( "--no-flush" );
publisher = boost::shared_ptr< comma::io::publisher >( new comma::io::publisher( publisher_name, comma::io::mode::binary, blocking, flush ) );
if( verbose ) {
std::cerr << "sick-ldmrs-stream: will be publishing on " << publisher_name << ( blocking? ", blocking enabled": "" ) << ( flush? ", flush enabled": "" ) << std::endl;
}
}
update_timestamp( true );
if( verbose ) { std::cerr << "sick-ldmrs-stream: starting scanning..." << std::endl; }
if( !protocol->write( sick::ibeo::commands::start() ).ok() ) { COMMA_THROW( comma::exception, "failed to start scanning" ); }
if( verbose ) { std::cerr << "sick-ldmrs-stream: started scanning" << std::endl; }
bool first = true;
comma::signal_flag is_shutdown;
#ifdef WIN32
_setmode( _fileno( stdout ), _O_BINARY );
#endif
while( !is_shutdown )
{
update_timestamp();
clear_fault();
const sick::ibeo::scan_packet* scan;
try { scan = protocol->readscan(); }
catch( sick::ibeo::protocol::faultException& ex ) { continue; }
if( scan == NULL ) { break; }
if( verbose && first ) { std::cerr << "sick-ldmrs-stream: got first scan" << std::endl; first = false; }
if( !scan->packet_header.valid() ) { COMMA_THROW( comma::exception, "invalid scan" ); }
if( options.exists( "--publish" ) )
{
publisher->write( scan->data(), sick::ibeo::header::size + scan->packet_header.payload_size() );
}
else
{
std::cout.write( scan->data(), sick::ibeo::header::size + scan->packet_header.payload_size() );
}
if( verbose && ( scan->packet_scan.scan_header.measurement_number() % 10 == 0 ) )
{
std::cerr << "sick-ldmrs-stream: got " << scan->packet_scan.scan_header.measurement_number() << " scans \r";
}
}
if( is_shutdown ) { std::cerr << "sick-ldmrs-stream: caught signal" << std::endl; }
}
if( ok ) { if( verbose ) { std::cerr << "sick-ldmrs-stream: done" << std::endl; } }
else { std::cerr << "sick-ldmrs-stream: failed" << std::endl; }
if( stream ) { stream->close(); }
return ok ? 0 : -1;
}
catch( std::exception& ex )
{
std::cerr << "sick-ldmrs-stream: " << ex.what() << std::endl;
}
catch( ... )
{
std::cerr << "sick-ldmrs-stream: unknown exception" << std::endl;
}
if( stream ) { stream->close(); }
usage();
}
