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(); }