static RVector<double> PoseValues(tf::Pose & pose) { RVector<double> values; values.push_back( pose.getOrigin().x()); values.push_back( pose.getOrigin().y()); values.push_back( pose.getOrigin().z()); double roll, pitch, yaw; tf::Quaternion q = pose.getRotation(); tf::Matrix3x3(q).getRPY(roll, pitch, yaw ); values.push_back( roll ); values.push_back( pitch ); values.push_back( yaw ); return values; }
TEST_F(RpcConainersTest, testRVector) { RVector<Int32> expected; expected.push_back(1); expected.push_back(3); expected.push_back(5); expected.serialize(*ostream_); RVector<Int32> actual; actual.serialize(*istream_); EXPECT_EQ(expected.size(), actual.size()); EXPECT_EQ(expected.back(), actual.back()); }
BOOST_FIXTURE_TEST_CASE( replace_test , F) { Registry r = createRegistry(); r.name = "/soa"; Registry r2 = createRegistry(); r2.name = "/soa2"; cache.add(r); const string name = r.name; RVector v; v.push_back(r2); cache.replace(r.name, v); BOOST_CHECK(cache.size() == 1); }