void output_points(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2) { if( csv.binary() ) { std::cout.write( reinterpret_cast< const char* >( &p1 ), sizeof( double ) * 3 ); std::cout.write( reinterpret_cast< const char* >( &p2 ), sizeof( double ) * 3 ); std::cout.flush(); } else { std::cout << ascii.put( p1 ) << csv.delimiter << ascii.put( p2 ) << std::endl; } }
template < typename T > void handle( const nmea::string& s ) { static comma::csv::ascii< T > ascii; handle( ascii.get( s.values() ) ); }
TEST( hokuyo_packed, scip_gd_response ) { hok::request_gd gd; gd.header.start_step = 0; gd.header.end_step = 10; gd.message_id.seq_num = 999; const std::string gd_request( "GD0000001000;GD00000999\n" ); // GD reponse for request with steps 0-10: 11 data points const std::string response = "GD0000001000;GD00000999\n00P\nG]\\VF\n06306J06_07407607106i06i06i075070n\n\n"; const hok::reply_gd< 11 >* gd_reply = reinterpret_cast< const hok::reply_gd< 11 >* >( &response[0] ); EXPECT_EQ( "00P", std::string( gd_reply->header.status.data(), hok::status_t::size ) ); EXPECT_TRUE( hok::verify_checksum( std::string( gd_reply->header.status.data(), hok::status_t::size ) ) ); EXPECT_EQ( "00", std::string( gd_reply->header.status.status.data(), 2 ) ); EXPECT_EQ( "G]\\VF", std::string( gd_reply->header.timestamp.data(), hok::timestamp_t::size ) ); // EXPECT_EQ( 0, gd_reply->header.status.status() ); EXPECT_EQ( 6216486, gd_reply->header.timestamp() ); EXPECT_EQ( 'F', gd_reply->header.timestamp.sum ); /// Line feeds are included in the data, remove it to verify checksum, as that value is the last 'n' EXPECT_EQ( "06306J06_07407607106i06i06i075070n", std::string( gd_reply->encoded.raw_data.data(), hok::distance_data< 11 >::value-1 ) ); hok::distance_data< 11 >::rays rays; gd_reply->encoded.get_values( rays ); { std::ostringstream ss; for( std::size_t i=0; i<rays.steps.size(); ++i ) { ss << rays.steps[i]() << ' '; } // distances in mm EXPECT_EQ( "387 410 431 452 454 449 441 441 441 453 448 ", ss.str() ); } { std::string line; static comma::csv::ascii< hok::distance_data< 11 >::points_t > ascii; EXPECT_EQ( "387,410,431,452,454,449,441,441,441,453,448", ascii.put( rays.steps, line ) ); } /// Why fails checksum // std::cerr << "11 values are : "; // comma::csv::output_stream< hok::distance_data< 11 > > oss( std::cerr ); // oss.write( gd_reply->data ); /// 100 steps/rays gd.header.end_step = 100; const char* reply101 = "GD0000010000;GD00000999\n00P\n\\0[Vm" "\n0Mm0MV0Jc0If0I30HX0HL0H;0G`0Gb0Gm0GV0GT0G;0FQ0FB0F<0F40Eo0En0E`0a" "\nEW0ER0ER0E40E90E40E80E<0E90E90E=0E>0E>0ED0EI0EQ0EX0EX0EW0EY0E_0Ef" "\ng0F80F>0FA0FC0FA0F?0FK0FN0F[0F^0F[0Fa0G50Gb?om?om?om?om?om?om?omi" "\n?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?f" "\nom?om0I\\0IR0I[0I\\0I`?om?om?om?om?om0J60J60In0Ie5\n\n"; const hok::reply_gd< 101 >* reply = reinterpret_cast< const hok::reply_gd< 101 >* >( reply101 ); EXPECT_EQ( "00P", std::string( reply->header.status.data(), hok::status_t::size ) ); EXPECT_TRUE( hok::verify_checksum( std::string( reply->header.status.data(), hok::status_t::size ) ) ); EXPECT_EQ( "00", std::string( reply->header.status.status.data(), 2 ) ); EXPECT_EQ( "\\0[Vm", std::string( reply->header.timestamp.data(), hok::timestamp_t::size ) ); EXPECT_TRUE( hok::verify_checksum( "0Mm0MV0Jc0If0I30HX0HL0H;0G`0Gb0Gm0GV0GT0G;0FQ0FB0F<0F40Eo0En0E`0a" ) ); EXPECT_TRUE( hok::verify_checksum( "EW0ER0ER0E40E90E40E80E<0E90E90E=0E>0E>0ED0EI0EQ0EX0EX0EW0EY0E_0Ef" ) ); EXPECT_TRUE( hok::verify_checksum( "g0F80F>0FA0FC0FA0F?0FK0FN0F[0F^0F[0Fa0G50Gb?om?om?om?om?om?om?omi" ) ); EXPECT_TRUE( hok::verify_checksum( "?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?om?f" ) ); EXPECT_TRUE( hok::verify_checksum( "om?om0I\\0IR0I[0I\\0I`?om?om?om?om?om0J60J60In0Ie5" ) ); hok::distance_data< 101 >::rays rays101; reply->encoded.get_values( rays101 ); // std::cerr << "11 values are : "; // for( std::size_t i=0; i<rays101.steps.size(); ++i ) // { // std::cerr << rays101.steps[i]() << ' '; // } // std::cerr << std::endl; }