/*
\brief Another sanity and demo test. Shows the user how to interface with a LaserScan type that contains a big array of data, as well as shows that a standard type that is generated is working.
*/
void test_scan()
{
// Building the object
::capnp::MallocMessageBuilder scan_b;
auto scan = scan_b.initRoot<gams::types::LaserScan>();
// Setting some scan fields
scan.setAngleMin(0);
scan.setAngleMax(M_PI);
// Initializing a scan data list and populating it
auto ranges = scan.initRanges(1000);
ranges.set(0, 1.0);
ranges.set(999, 1.0);
// Incorrect
//scan.setRanges(ranges.asReader());
// Setting to KB
k.set_any("scan", madara::knowledge::CapnObject<gams::types::LaserScan>(scan_b));
// Retrieving from KB
madara::knowledge::CapnObject<gams::types::LaserScan> k_scan = k.get("scan").to_any<madara::knowledge::CapnObject<gams::types::LaserScan> >();
TEST_EQ(k_scan.reader().getAngleMin(), 0);
TEST_GT((k_scan.reader().getAngleMax() - M_PI), 0);
TEST_EQ(k_scan.reader().getRanges()[0], 1.0);
TEST_EQ(k_scan.reader().getRanges()[999], 1.0);
}
/*
\brief Another sanity and demo test. Shows the user how to interface with a CapnObject that uses many fields as well as shows that a standard type that is generated is working.
*/
void test_imu()
{
::capnp::MallocMessageBuilder imu_b;
auto imu = imu_b.initRoot<gams::types::Imu>();
auto lin_acc = imu.initLinearAccelerationCovariance(9);
lin_acc.set(0, 0.01);
lin_acc.set(1, 0.03);
::capnp::MallocMessageBuilder header_b;
auto header = imu.initHeader();
header.setStamp(10);
header.setFrameId("world");
header.setSeq(100);
// Incorrect
//imu.setHeader(header);
//imu.setLinearAccelerationCovariance(lin_acc.asReader());
k.set_any("imu", madara::knowledge::CapnObject<gams::types::Imu>(imu_b));
madara::knowledge::CapnObject<gams::types::Imu> imu_ = k.get("imu").to_any<madara::knowledge::CapnObject<gams::types::Imu> >();
TEST_EQ(imu_.reader().getHeader().getStamp(), 10);
TEST_EQ(imu_.reader().getHeader().getFrameId().cStr(), std::string("world"));
TEST_EQ((signed)imu_.reader().getHeader().getSeq(), 100);
TEST_EQ(imu_.reader().getLinearAccelerationCovariance()[0], 0.01);
TEST_EQ(imu_.reader().getLinearAccelerationCovariance()[1], 0.03);
}