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