TEST(RigidBody, Integration) {
float h = 0.01f;
const int N = 120;
float thresh_theta = 0.5f;
float thresh_vel = 0.1f;
EulerIntegration integratorE;
AdamsBashforthIntegration integratorAB;
AdamsBashforthIntegration5 integratorAB5;
int schedule[] = {10, 30, 60, 80, 100, 1000};
float torques[] = {1.0f, 0.0f, -0.5f, 2.5f, -0.1f, 0.0f};
int ptr = 0;
for (int i = 0; i < N; i++) {
for (; i > schedule[ptr] && schedule[ptr] < 1000; ptr++) { }
integratorE.torque = integratorAB.torque = integratorAB5.torque = torques[ptr];
integratorE.update(h);
integratorAB.update(h);
integratorAB5.update(h);
}
bool passed = true;
math::matrix2x1<float> states[] = { integratorE.state, integratorAB.state, integratorAB5.state };
float thresh[] = { thresh_theta, thresh_vel };
float math::matrix2x1<float>:: *elements[] = { & math::matrix2x1<float>::_0, & math::matrix2x1<float>::_1 };
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 2; j++) {
EXPECT_LT(std::abs((states[i]).*(elements[j]) - (states[i+1]).*(elements[j])), thresh[j]);
}
}
}
TEST(InterfacesTest, OdomPoseBasicIO)
{
stateUpdated_ = false;
ros::NodeHandle nh;
ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("/odom_input0", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
nav_msgs::Odometry odom;
odom.pose.pose.position.x = 20.0;
odom.pose.pose.position.y = 10.0;
odom.pose.pose.position.z = -40.0;
odom.pose.covariance[0] = 2.0;
odom.pose.covariance[7] = 2.0;
odom.pose.covariance[14] = 2.0;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
ros::Rate loopRate(50);
for (size_t i = 0; i < 50; ++i)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
// Now check the values from the callback
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - odom.pose.pose.position.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); // Configuration for this variable for this sensor is false
EXPECT_LT(::fabs(filtered_.pose.pose.position.z - odom.pose.pose.position.z), 0.01);
EXPECT_LT(filtered_.pose.covariance[0], 0.5);
EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Configuration for this variable for this sensor is false
EXPECT_LT(filtered_.pose.covariance[14], 0.6);
resetFilter();
}
TEST_F(QuotaTest, usage)
{
// put quota
MojObject obj;
MojAssertNoErr( obj.fromJson(_T("{\"owner\":\"com.foo.bar\",\"size\":1000}")) );
MojAssertNoErr( db.putQuotas(&obj, &obj + 1) );
// empty
MojInt64 kindUsage = -1;
EXPECT_NO_FATAL_FAILURE( getKindUsage(db, _T("Test:1"), kindUsage) );
EXPECT_EQ( 0, kindUsage )
<< "Kind without objects should have zero usage";
MojInt64 quotaUsage = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage) );
EXPECT_EQ( 0, quotaUsage )
<< "Quota without matching objects should have zero usage";
// new obj
EXPECT_NO_FATAL_FAILURE( put(db, MojTestKind1Objects[0]) );
MojInt64 kindUsage1 = -1;
EXPECT_NO_FATAL_FAILURE( getKindUsage(db, _T("Test:1"), kindUsage1) );
EXPECT_LT( 0, kindUsage1 )
<< "Adding new object into kind should increase kind usage";
MojInt64 quotaUsage1 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage1) );
EXPECT_LT( 0, quotaUsage1 )
<< "Adding new object matching quota should increase quota usage";
// add prop to existing obj
MojAssertNoErr( obj.fromJson(MojTestKind1Objects[0]) );
MojAssertNoErr( obj.put(_T("bar"), 2) );
MojAssertNoErr( db.put(obj, MojDb::FlagForce) );
MojInt64 kindUsage2 = -1;
EXPECT_NO_FATAL_FAILURE( getKindUsage(db, _T("Test:1"), kindUsage2) );
EXPECT_LE( 0, kindUsage2 );
EXPECT_LT( kindUsage1, kindUsage2 )
<< "Adding property to existing object should increase kind usage";
MojInt64 quotaUsage2 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage2) );
EXPECT_LE( 0, quotaUsage2 );
EXPECT_LT( quotaUsage1, quotaUsage2 )
<< "Adding property to existing object that matches quota should increase usage";
// add 2nd obj
EXPECT_NO_FATAL_FAILURE( put(db, MojTestKind1Objects[1]) );
MojInt64 kindUsage3 = -1;
EXPECT_NO_FATAL_FAILURE( getKindUsage(db, _T("Test:1"), kindUsage3) );
EXPECT_LE( 0, kindUsage3 );
EXPECT_LT( kindUsage2, kindUsage3 )
<< "Adding another object should increase kind usage";
MojInt64 quotaUsage3 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage3) );
EXPECT_LE( 0, quotaUsage3 );
EXPECT_LT( quotaUsage2, quotaUsage3 )
<< "Adding another object matching to quota should increase usage";
// del first obj
bool found = false;
MojExpectNoErr( db.del(1, found, MojDb::FlagPurge) );
EXPECT_TRUE( found ) << "Object should be deleted";
MojInt64 kindUsage4 = -1;
EXPECT_NO_FATAL_FAILURE( getKindUsage(db, _T("Test:1"), kindUsage4) );
EXPECT_LE( 0, kindUsage4 );
EXPECT_EQ( kindUsage3 - kindUsage2, kindUsage4 )
<< "Deletion of object should bring kind usage to expected value";
MojInt64 quotaUsage4 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage4) );
EXPECT_LE( 0, quotaUsage4 );
EXPECT_EQ( quotaUsage3 - quotaUsage2, quotaUsage4 )
<< "Deletion of object should bring quota usage to expected value";
// add index
MojAssertNoErr( obj.fromJson(MojTestKind1Str2) );
MojExpectNoErr( db.putKind(obj) );
MojInt64 kindUsage5 = -1;
EXPECT_NO_FATAL_FAILURE( getKindUsage(db, _T("Test:1"), kindUsage5) );
EXPECT_LE( 0, kindUsage5 );
EXPECT_LT( kindUsage4, kindUsage5 )
<< "Adding new index should increase kind usage";
MojInt64 quotaUsage5 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage5) );
EXPECT_LE( 0, quotaUsage5 );
EXPECT_LT( quotaUsage4, quotaUsage5 )
<< "Adding new index should increase quota usage";
// update locale
MojExpectNoErr( db.updateLocale(_T("FR_fr")) );
MojExpectNoErr( db.updateLocale(_T("EN_us")) );
MojInt64 kindUsage6 = -1;
EXPECT_NO_FATAL_FAILURE( getKindUsage(db, _T("Test:1"), kindUsage6) );
EXPECT_LE( 0, kindUsage6 );
EXPECT_EQ( kindUsage5, kindUsage6 )
<< "Switching locale forth and back shouldn't affect kind usage";
MojInt64 quotaUsage6 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage6) );
EXPECT_LE( 0, kindUsage6 );
EXPECT_EQ( quotaUsage5, quotaUsage6 )
<< "Switching locale forth and back shouldn't affect quota usage";
// drop index
MojAssertNoErr( obj.fromJson(MojTestKind1Str1) );
MojExpectNoErr( db.putKind(obj) );
MojInt64 kindUsage7 = -1;
EXPECT_NO_FATAL_FAILURE( getKindUsage(db, _T("Test:1"), kindUsage7) );
EXPECT_LE( 0, kindUsage7 );
EXPECT_EQ( kindUsage4, kindUsage7 )
<< "Dropping of index should bring kind usage to expected value";
MojInt64 quotaUsage7 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage7) );
EXPECT_LE( 0, quotaUsage7 );
EXPECT_EQ( quotaUsage4, quotaUsage7 )
<< "Dropping of index should bring quota usage to expected value";
// drop kind
MojString kindStr;
MojAssertNoErr( kindStr.assign(_T("Test:1")) );
MojExpectNoErr( db.delKind(kindStr, found) );
EXPECT_TRUE( found ) << "Kind should be deleted";
MojInt64 kindUsage8 = -1;
EXPECT_NO_FATAL_FAILURE( getKindUsage(db, _T("Test:1"), kindUsage8) );
EXPECT_EQ( 0, kindUsage8 )
<< "Dropping of kind should bring its usage to zero";
MojInt64 quotaUsage8 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage8) );
EXPECT_EQ( 0, quotaUsage8 )
<< "Dropping of kind that matches quota should bring quota usage to zero";
MojExpectNoErr( db.quotaStats(obj) );
MojString statStr;
MojExpectNoErr( obj.toJson(statStr) );
std::cerr << "quotaStats: " << statStr.data() << std::endl;
}
TEST(String, LessOnDiffCase)
{
String s1("Hello"), s2("hello");
EXPECT_LT(s1, s2);
EXPECT_FALSE(s2 < s1);
}
TEST(InterfacesTest, PoseDifferentialIO)
{
ros::NodeHandle nh;
ros::Publisher posePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/pose_input1", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
geometry_msgs::PoseWithCovarianceStamped pose;
pose.pose.pose.position.x = 20.0;
pose.pose.pose.position.y = 10.0;
pose.pose.pose.position.z = -40.0;
pose.pose.pose.orientation.w = 1;
pose.pose.covariance[0] = 2.0;
pose.pose.covariance[7] = 2.0;
pose.pose.covariance[14] = 2.0;
pose.pose.covariance[21] = 0.2;
pose.pose.covariance[28] = 0.2;
pose.pose.covariance[35] = 0.2;
pose.header.frame_id = "odom";
// No guaranteeing that the zero state
// we're expecting to see here isn't just
// a result of zeroing it out previously,
// so check 10 times in succession.
size_t zeroCount = 0;
while (zeroCount++ < 10)
{
pose.header.stamp = ros::Time::now();
posePub.publish(pose);
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01);
ros::Duration(0.1).sleep();
pose.header.seq++;
}
// ...but only if we give the measurement a tiny covariance
for (size_t ind = 0; ind < 36; ind+=7)
{
pose.pose.covariance[ind] = 1e-6;
}
// Issue this location repeatedly, and see if we get
// a final reported position of (1, 2, -3)
ros::Rate loopRate(20);
for (size_t i = 0; i < 100; ++i)
{
pose.pose.pose.position.x += 0.01;
pose.pose.pose.position.y += 0.02;
pose.pose.pose.position.z -= 0.03;
pose.header.stamp = ros::Time::now();
posePub.publish(pose);
ros::spinOnce();
loopRate.sleep();
pose.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6);
resetFilter();
}
// Tests that the Variant::operator< method works for string-float comparison
TEST_F(TestVariant, OperatorLTStringFloat) {
Variant v1("124.08");
Variant v2((float) 124.09);
EXPECT_LT(v1, v2);
}
TEST(ControlSystem, depth)
{
double test_depth = -3;
double overshoot_allowed = 0.3;
double average_threshold = 0.1;
double std_dev_allowed = 0.05;
rs::SubscriberAnalyzer<robosub_msgs::Float32Stamped> analyzer;
analyzer.Init("depth", &get_depth_data);
robosub_msgs::control msg;
//keep the sub steady
msg.forward_state = robosub_msgs::control::STATE_ERROR;
msg.forward = 0;
msg.strafe_state = robosub_msgs::control::STATE_ERROR;
msg.strafe_left = 0;
msg.yaw_state = robosub_msgs::control::STATE_RELATIVE;
msg.yaw_left = 0;
msg.roll_state = robosub_msgs::control::STATE_ABSOLUTE;
msg.roll_right = 0;
msg.pitch_state = robosub_msgs::control::STATE_ABSOLUTE;
msg.pitch_down = 0;
//just go to depth
msg.dive_state = robosub_msgs::control::STATE_ABSOLUTE;
msg.dive = test_depth;
//fill out a control message to stay level and go to depth
pub.publish(msg);
analyzer.Start();
ROS_INFO("diving to depth");
//wait for the sub to reach its depth
ros::Time exit_time = ros::Time::now() + ros::Duration(10);
while (ros::Time::now() < exit_time)
{
ros::spinOnce();
ros::Duration(0.01).sleep();
}
analyzer.Stop();
//confirm we didn't dive too deep
EXPECT_LT(test_depth - overshoot_allowed, analyzer.GetMin());
ROS_INFO("maintaining depth to check steady-state oscillation...");
analyzer.ClearData();
analyzer.Start();
//wait for 10 seconds to measure wiggle
exit_time = ros::Time::now() + ros::Duration(10);
while (ros::Time::now() < exit_time)
{
ros::spinOnce();
ros::Duration(0.01).sleep();
}
analyzer.Stop();
//confirm depth is stable
EXPECT_NEAR(test_depth, analyzer.GetAverage(), average_threshold);
EXPECT_LT(analyzer.GetStandardDeviation(), std_dev_allowed);
}
TEST(RealWordTest, XORLinear)
{
typedef NeuralNetwork<double, LinearActivationFunction<double >> network;
network nn;
//nn.setActivationfunction(LinearActivationFunction<double>(10));
nn.setEntries(2);
nn.setExits(1);
nn.setLayersCount(2);
nn.setNeurons(1, 2);
nn.init();
std::vector<double> in1(2);
std::vector<double> in2(2);
std::vector<double> in3(2);
std::vector<double> in4(2);
in1[0] = 0;
in1[1] = 0;
in2[0] = 1;
in2[1] = 0;
in3[0] = 0;
in3[1] = 1;
in4[0] = 1;
in4[1] = 1;
std::vector<double> out0(1);
std::vector<double> out1(1);
out0[0] = 0;
out1[0] = 1;
//uczenie
for (int i = 0; i < 1000; ++i)
{
std::vector<double> o(1);
nn.setInput(in1.begin(), in1.end());
o = nn.calcOutput();
nn.learn(out0.begin(), out0.end());
//
// std::cout << "Wynik0: " << o[0] << "\n";
// nn.printWages();
nn.setInput(in2.begin(), in2.end());
o = nn.calcOutput();
nn.learn(out1.begin(), out1.end());
//
// std::cout << "Wynik1: " << o[0] << "\n";
// nn.printWages();
nn.setInput(in3.begin(), in3.end());
o = nn.calcOutput();
nn.learn(out1.begin(), out1.end());
//
// std::cout << "Wynik1: " << o[0] << "\n";
// nn.printWages();
nn.setInput(in4.begin(), in4.end());
o = nn.calcOutput();
nn.learn(out0.begin(), out0.end());
//
// std::cout << "Wynik0: " << o[0] << "\n";
// nn.printWages();
}
//test
std::vector<double> out;
std::vector<double> o(4);
nn.setInput(in1.begin(), in1.end());
out = nn.calcOutput();
o[0] = out[0];
EXPECT_LT(out[0], 0.1);
nn.setInput(in2.begin(), in2.end());
out = nn.calcOutput();
o[1] = out[0];
EXPECT_GT(out[0], 0.9);
nn.setInput(in3.begin(), in3.end());
out = nn.calcOutput();
o[2] = out[0];
EXPECT_GT(out[0], 0.9);
nn.setInput(in4.begin(), in4.end());
out = nn.calcOutput();
o[3] = out[0];
EXPECT_LT(out[0], 0.1);
//std::cout << o[0] << " " << o[1] << " " << o[2] << " " << o[3] << "\n";
}
// Tests that the Variant::operator< method works for float-float comparison
TEST_F(TestVariant, OperatorLTFloatFloatP1) {
Variant v1((float) 124.8);
Variant v2((float) 124.9);
EXPECT_LT(v1, v2);
}
TEST(InterfacesTest, ImuDifferentialIO)
{
ros::NodeHandle nh;
ros::Publisher imu0Pub = nh.advertise<sensor_msgs::Imu>("/imu_input0", 5);
ros::Publisher imu1Pub = nh.advertise<sensor_msgs::Imu>("/imu_input1", 5);
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input3", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
imu.header.frame_id = "base_link";
tf2::Quaternion quat;
const double roll = M_PI/2.0;
const double pitch = -M_PI;
const double yaw = -M_PI/4.0;
quat.setRPY(roll, pitch, yaw);
imu.orientation = tf2::toMsg(quat);
imu.orientation_covariance[0] = 0.02;
imu.orientation_covariance[4] = 0.02;
imu.orientation_covariance[8] = 0.02;
imu.angular_velocity_covariance[0] = 0.02;
imu.angular_velocity_covariance[4] = 0.02;
imu.angular_velocity_covariance[8] = 0.02;
size_t setCount = 0;
while (setCount++ < 10)
{
imu.header.stamp = ros::Time::now();
imu0Pub.publish(imu); // Use this to move the absolute orientation
imu1Pub.publish(imu); // Use this to keep velocities at 0
ros::spinOnce();
ros::Duration(0.1).sleep();
imu.header.seq++;
}
size_t zeroCount = 0;
while (zeroCount++ < 10)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
ros::Duration(0.1).sleep();
imu.header.seq++;
}
double rollFinal = roll;
double pitchFinal = pitch;
double yawFinal = yaw;
// Move the orientation slowly, and see if we
// can push it to 0
ros::Rate loopRate(20);
for (size_t i = 0; i < 100; ++i)
{
yawFinal -= 0.01 * (3.0 * M_PI/4.0);
quat.setRPY(rollFinal, pitchFinal, yawFinal);
imu.orientation = tf2::toMsg(quat);
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
loopRate.sleep();
imu.header.seq++;
}
ros::spinOnce();
// Move the orientation slowly, and see if we
// can push it to 0
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
rollFinal += 0.01 * (M_PI/2.0);
quat.setRPY(rollFinal, pitchFinal, yawFinal);
imu.orientation = tf2::toMsg(quat);
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
loopRate.sleep();
imu.header.seq++;
}
ros::spinOnce();
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
tf2::Matrix3x3 mat(quat);
mat.getRPY(rollFinal, pitchFinal, yawFinal);
EXPECT_LT(::fabs(rollFinal), 0.2);
EXPECT_LT(::fabs(pitchFinal), 0.2);
EXPECT_LT(::fabs(yawFinal), 0.2);
resetFilter();
}
TEST_F(MatMultTests, SIFT) {
string fileName("mat-sift");
int m = 898790;
int n = 128;
int k = 256;
/* allocate data */
float * data = (float *)malloc(m*n*sizeof(float));
float * centers = (float *)malloc(k*n*sizeof(float));
float * result = (float *)malloc(m*k*sizeof(float));
float * resultCublas = (float *)malloc(m*k*sizeof(float));
/* read matrix from file */
FILE * fid = fopen(fileName.c_str(), "rb");
int nread = fread(data, sizeof(float), m*n, fid);
ASSERT_EQ(nread, m*n);
fclose(fid);
/* initialize centers to 1 */
for (int i = 0; i<k*n; ++i) centers[i] = (float)1;
/* allocate device space for the various arrays */
float * dev_data, *dev_centers, *dev_result;
int factor = TILESIZE*N_UNROLL_FLOAT;
int m_padded = ((m + factor - 1) / factor)*factor;
int nBytes = m_padded*n*sizeof(float);
cudaMalloc((void**)&dev_data, nBytes);
cudaMemset(dev_data, 0, nBytes);
cudaMemcpy(dev_data, data, m*n*sizeof(float), cudaMemcpyHostToDevice);
nBytes = n*k*sizeof(float);
cudaMalloc((void**)&dev_centers, nBytes);
cudaMemcpy(dev_centers, centers, nBytes, cudaMemcpyHostToDevice);
nBytes = m*k*sizeof(float);
cudaMalloc((void**)&dev_result, nBytes);
cudaMemset(dev_result, 0, nBytes);
/* run MatMatMultF */
int err = MatMatMultF(m, n, dev_data, k, dev_centers, dev_result);
if (err) printf("Error int MatMatMultF for mat-sift\n");
cudaMemcpy(result, dev_result, nBytes, cudaMemcpyDeviceToHost);
/* run CUBLAS SGEMM */
float one = 1.f;
float zero = 0.f;
cublasHandle_t handle;
cublasCreate(&handle);
cublasSgemm(handle, CUBLAS_OP_N, CUBLAS_OP_N,
k, m, n, (const float *)&one,
(const float *)dev_centers, k,
(const float *)dev_data, n,
(const float *)&zero, (float *)dev_result, k);
cudaMemcpy(resultCublas, dev_result, nBytes, cudaMemcpyDeviceToHost);
#if 1
/* check results */
int maxPrintErrors=10;
int numPrintErrors=0;
for (int i = 0; i < m; ++i) {
for (int j = 0; j < k; ++j) {
int index = i*k + j;
if (result[index] == 0 && resultCublas[index] == 0) continue;
else {
float err = fabs(result[index] - resultCublas[index]) / fabs(result[index]);
if (err >= 1.e-6 || result[index] == 0) {
printf("i=%d, j=%d : %1.5g, %1.5g, err=%1.5g\n", i, j, result[index], resultCublas[index], err);
if (numPrintErrors<maxPrintErrors) {
numPrintErrors++;
EXPECT_LT(err, 1.e-6);
} else {
ASSERT_LT(err, 1.e-6);
}
}
}
}
}
#endif
/* free data */
if (dev_data) cudaFree(dev_data);
if (dev_centers) cudaFree(dev_centers);
if (dev_result) cudaFree(dev_result);
if (data) free(data);
if (centers) free(centers);
if (result) free(result);
if (resultCublas) free(resultCublas);
cublasDestroy(handle);
}
TEST(PlayerControlSystem, Process)
{
ECS::World* world = CreateWorld();
world->SetDelta(1.0f);
g_world = world;
g_networkEntityMap.clear();
// Initialize the keybindings we need for the test(not the complete list that is normally used)
std::vector<RootForce::Keybinding> keybindings(6);
keybindings[0].Bindings.push_back(SDL_SCANCODE_UP);
keybindings[0].Bindings.push_back(SDL_SCANCODE_W);
keybindings[0].Action = RootForce::PlayerAction::MOVE_FORWARDS;
keybindings[0].Edge = true;
keybindings[1].Bindings.push_back(SDL_SCANCODE_DOWN);
keybindings[1].Bindings.push_back(SDL_SCANCODE_S);
keybindings[1].Action = RootForce::PlayerAction::MOVE_BACKWARDS;
keybindings[1].Edge = true;
keybindings[4].Bindings.push_back(SDL_SCANCODE_SPACE);
keybindings[4].Action = RootForce::PlayerAction::JUMP_PRESSED;
keybindings[4].ActionUp = RootForce::PlayerAction::JUMP_RELEASED;
keybindings[4].Edge = true;
keybindings[5].Bindings.push_back((SDL_Scancode)RootEngine::InputManager::MouseButton::LEFT);
keybindings[5].Action = RootForce::PlayerAction::ACTIVATE_ABILITY_PRESSED;
keybindings[5].ActionUp = RootForce::PlayerAction::ACTIVATE_ABILITY_RELEASED;
keybindings[5].Edge = true;
keybindings.push_back(RootForce::Keybinding());
keybindings[keybindings.size()-1].Bindings.push_back(SDL_SCANCODE_1);
keybindings[keybindings.size()-1].Action = RootForce::PlayerAction::SELECT_ABILITY1;
keybindings[keybindings.size()-1].Edge = true;
keybindings.push_back(RootForce::Keybinding());
keybindings[keybindings.size()-1].Bindings.push_back(SDL_SCANCODE_2);
keybindings[keybindings.size()-1].Action = RootForce::PlayerAction::SELECT_ABILITY2;
keybindings[keybindings.size()-1].Edge = true;
keybindings.push_back(RootForce::Keybinding());
keybindings[keybindings.size()-1].Bindings.push_back(SDL_SCANCODE_3);
keybindings[keybindings.size()-1].Action = RootForce::PlayerAction::SELECT_ABILITY3;
keybindings[keybindings.size()-1].Edge = true;
RootForce::PlayerControlSystem* system = new RootForce::PlayerControlSystem(world);
system->SetInputInterface(g_engineContext.m_inputSys);
system->SetLoggingInterface(g_engineContext.m_logger);
system->SetPhysicsInterface(g_engineContext.m_physics);
system->SetKeybindings(keybindings);
// Call the OnCreate script
g_engineContext.m_script->SetGlobalNumber("UserID", 0);
g_engineContext.m_script->SetGlobalNumber("IsClient", true);
g_engineContext.m_script->SetFunction(g_engineContext.m_resourceManager->LoadScript("Player"), "OnCreate");
//g_engineContext.m_script->AddParameterUserData(testity, sizeof(ECS::Entity*), "Entity");
g_engineContext.m_script->AddParameterNumber(0);
g_engineContext.m_script->AddParameterNumber(RootForce::Network::ReservedActionID::CONNECT);
g_engineContext.m_script->ExecuteScript();
ECS::Entity* testity = world->GetTagManager()->GetEntityByTag("Player");
RootEngine::InputManager::InputInterface* ii = g_engineContext.m_inputSys;
RootForce::PlayerActionComponent* action = world->GetEntityManager()->GetComponent<RootForce::PlayerActionComponent>(testity);
//Fake event to test different keys
SDL_Event falseevent;
falseevent.type = SDL_KEYDOWN;
falseevent.key.repeat = false;
//Movement test
{
falseevent.key.keysym.scancode = SDL_SCANCODE_UP;
ii->HandleInput(falseevent);
system->Process();
//Pushing just forward should set movepower to 1
EXPECT_EQ(action->MovePower, 1);
falseevent.key.keysym.scancode = SDL_SCANCODE_DOWN;
ii->HandleInput(falseevent);
falseevent.key.keysym.scancode = SDL_SCANCODE_UP;
ii->HandleInput(falseevent);
system->Process();
//Pushing back and forth at the same time should cancel each other out
EXPECT_EQ(action->MovePower, 0);
falseevent.key.keysym.scancode = SDL_SCANCODE_DOWN;
ii->HandleInput(falseevent);
system->Process();
//We have now released the up key and should be moving backwards
EXPECT_EQ(action->MovePower, -1);
}
//Jump test
{
falseevent.key.keysym.scancode = SDL_SCANCODE_SPACE;
falseevent.type = SDL_KEYDOWN;
ii->HandleInput(falseevent);
system->Process();
EXPECT_GT(action->JumpTime, 0);
}
//Orientation test
{
SDL_Event falseMouseEvent;
falseMouseEvent.type = SDL_MOUSEMOTION;
falseMouseEvent.motion.x = 5;
falseMouseEvent.motion.y = 140;
falseMouseEvent.motion.xrel = 5;
falseMouseEvent.motion.yrel = 140;
ii->HandleInput(falseMouseEvent);
system->Process();
EXPECT_LT(action->Angle.y, 0);
EXPECT_GT(action->Angle.x, 0);
}
world->GetEntityManager()->RemoveAllEntitiesAndComponents();
world->GetTagManager()->UnregisterAll();
world->GetGroupManager()->UnregisterAll();
g_engineContext.m_physics->RemoveAll();
delete world;
}
void checkMonotone(const T *ary, size_t size)
{
for (size_t i = 1; i < size; ++i) {
EXPECT_LT(ary[i-1], ary[i]);
}
}
TEST_F(MidiControllerTest, ReceiveMessage_PotMeterCO_14BitCC) {
ConfigKey key("[Channel1]", "playposition");
const double kMinValue = -1234.5;
const double kMaxValue = 678.9;
const double kMiddleValue = (kMinValue + kMaxValue) * 0.5;
ControlPotmeter potmeter(key, kMinValue, kMaxValue);
potmeter.set(0);
unsigned char channel = 0x01;
unsigned char lsb_control = 0x10;
unsigned char msb_control = 0x11;
MidiOptions lsb;
lsb.fourteen_bit_lsb = true;
MidiOptions msb;
msb.fourteen_bit_msb = true;
addMapping(MidiInputMapping(MidiKey(MIDI_CC | channel, lsb_control),
lsb, key));
addMapping(MidiInputMapping(MidiKey(MIDI_CC | channel, msb_control),
msb, key));
loadPreset(m_preset);
// If kMinValue or kMaxValue are such that the middle value is 0 then the
// set(0) commands below allow us to hide failures.
ASSERT_NE(0.0, kMiddleValue);
// Receive a 0x0000 (lsb-first), MIDI parameter should map to the min value.
potmeter.set(0);
receive(MIDI_CC | channel, lsb_control, 0x00);
receive(MIDI_CC | channel, msb_control, 0x00);
EXPECT_DOUBLE_EQ(kMinValue, potmeter.get());
// Receive a 0x0000 (msb-first), MIDI parameter should map to the min value.
potmeter.set(0);
receive(MIDI_CC | channel, msb_control, 0x00);
receive(MIDI_CC | channel, lsb_control, 0x00);
EXPECT_DOUBLE_EQ(kMinValue, potmeter.get());
// Receive a 0x3FFF (lsb-first), MIDI parameter should map to the max value.
potmeter.set(0);
receive(MIDI_CC | channel, lsb_control, 0x7F);
receive(MIDI_CC | channel, msb_control, 0x7F);
EXPECT_DOUBLE_EQ(kMaxValue, potmeter.get());
// Receive a 0x3FFF (msb-first), MIDI parameter should map to the max value.
potmeter.set(0);
receive(MIDI_CC | channel, msb_control, 0x7F);
receive(MIDI_CC | channel, lsb_control, 0x7F);
EXPECT_DOUBLE_EQ(kMaxValue, potmeter.get());
// Receive a 0x2000 (lsb-first), MIDI parameter should map to the middle
// value.
potmeter.set(0);
receive(MIDI_CC | channel, lsb_control, 0x00);
receive(MIDI_CC | channel, msb_control, 0x40);
EXPECT_DOUBLE_EQ(kMiddleValue, potmeter.get());
// Receive a 0x2000 (msb-first), MIDI parameter should map to the middle
// value.
potmeter.set(0);
receive(MIDI_CC | channel, msb_control, 0x40);
receive(MIDI_CC | channel, lsb_control, 0x00);
EXPECT_DOUBLE_EQ(kMiddleValue, potmeter.get());
// Check the 14-bit resolution is actually present. Receive a 0x2001
// (msb-first), MIDI parameter should map to the middle value plus a tiny
// amount. Scaling is not quite linear for MIDI parameters so just check
// that incrementing the LSB by 1 is greater than the middle value.
potmeter.set(0);
receive(MIDI_CC | channel, msb_control, 0x40);
receive(MIDI_CC | channel, lsb_control, 0x01);
EXPECT_LT(kMiddleValue, potmeter.get());
// Check the 14-bit resolution is actually present. Receive a 0x2001
// (lsb-first), MIDI parameter should map to the middle value plus a tiny
// amount. Scaling is not quite linear for MIDI parameters so just check
// that incrementing the LSB by 1 is greater than the middle value.
potmeter.set(0);
receive(MIDI_CC | channel, lsb_control, 0x01);
receive(MIDI_CC | channel, msb_control, 0x40);
EXPECT_LT(kMiddleValue, potmeter.get());
}
TEST(ErrorBlockTests, error_blocks_maxwell)
{
ros::NodeHandle nh("~");
robot_calibration::Optimizer opt(robot_description);
std::vector<robot_calibration_msgs::CalibrationData> data;
robot_calibration_msgs::CalibrationData msg;
// Match expected output from chain manager
msg.joint_states.name.resize(10);
msg.joint_states.name[0] = "arm_lift_joint";
msg.joint_states.name[1] = "arm_shoulder_pan_joint";
msg.joint_states.name[2] = "arm_shoulder_lift_joint";
msg.joint_states.name[3] = "arm_upperarm_roll_joint";
msg.joint_states.name[4] = "arm_elbow_flex_joint";
msg.joint_states.name[5] = "arm_wrist_flex_joint";
msg.joint_states.name[6] = "arm_wrist_roll_joint";
msg.joint_states.name[7] = "head_pan_joint";
msg.joint_states.name[8] = "head_tilt_joint";
msg.joint_states.name[9] = "arm_lift_joint";
msg.joint_states.position.resize(10);
msg.joint_states.position[0] = -0.05; // Add some error
msg.joint_states.position[1] = -0.814830;
msg.joint_states.position[2] = -0.00022290000000002586;
msg.joint_states.position[3] = 0.0;
msg.joint_states.position[4] = -0.7087341;
msg.joint_states.position[5] = 0.0;
msg.joint_states.position[6] = 0.0;
msg.joint_states.position[7] = -0.8280187999999999;
msg.joint_states.position[8] = 0.6358500000000002;
msg.joint_states.position[9] = 0.0;
// Expectect output from led finder
msg.observations.resize(2);
msg.observations[0].sensor_name = "camera";
msg.observations[1].sensor_name = "arm";
msg.observations[0].features.resize(1);
msg.observations[0].features[0].header.frame_id = "head_camera_rgb_optical_frame";
msg.observations[0].features[0].point.x = -0.0143163670728;
msg.observations[0].features[0].point.y = 0.111304592065;
msg.observations[0].features[0].point.z = 0.522079317365;
msg.observations[0].ext_camera_info.camera_info.P[0] = 100.0; // fx
msg.observations[0].ext_camera_info.camera_info.P[5] = 100.0; // fy
msg.observations[0].ext_camera_info.camera_info.P[2] = 320.0; // cx
msg.observations[0].ext_camera_info.camera_info.P[6] = 240.0; // cy
msg.observations[0].ext_camera_info.parameters.resize(2);
msg.observations[0].ext_camera_info.parameters[0].name = "z_offset";
msg.observations[0].ext_camera_info.parameters[0].value = 0.0;
msg.observations[0].ext_camera_info.parameters[1].name = "z_scaling";
msg.observations[0].ext_camera_info.parameters[1].value = 1.0;
msg.observations[1].features.resize(1);
msg.observations[1].features[0].header.frame_id = "gripper_led_frame";
msg.observations[1].features[0].point.x = 0.0;
msg.observations[1].features[0].point.y = 0.0;
msg.observations[1].features[0].point.z = 0.0;
// Add first data point
data.push_back(msg);
// Add a second data point that is just a little different
msg.joint_states.position[1] = -0.019781999999999966;
msg.joint_states.position[7] = 0.0;
msg.observations[0].features[0].point.x = 0.0365330705881;
msg.observations[0].features[0].point.y = 0.102609552493;
msg.observations[0].features[0].point.z = 0.536061220027;
data.push_back(msg);
// And a third data point
msg.joint_states.position[1] = 0.883596;
msg.joint_states.position[7] = 0.9442135999999999;
msg.observations[0].features[0].point.x = 0.0942445346646;
msg.observations[0].features[0].point.y = 0.11409172323;
msg.observations[0].features[0].point.z = 0.517497963716;
data.push_back(msg);
// Setup params
robot_calibration::OptimizationParams params;
params.LoadFromROS(nh);
// Optimize
opt.optimize(params, data, false);
EXPECT_GT(opt.summary()->initial_cost, 0.001);
EXPECT_LT(opt.summary()->final_cost, 1e-18);
EXPECT_GT(opt.summary()->iterations.size(), static_cast<size_t>(1)); // expect more than 1 iteration
// The -0.05 we added above should be calibrated off
EXPECT_LT(fabs(0.05 - opt.getOffsets()->get("arm_lift_joint")), 0.001);
// 1 joint
EXPECT_EQ(1, opt.getNumParameters());
// 3 CalibrationData, each with chain3d with a single observed point (3 residuals)
EXPECT_EQ(9, opt.getNumResiduals());
}
std::pair<double, double>
run_test(barrier_inserter& insert_barrier,
bool prefill,
uint64_t tasks_per_queue,
unsigned num_queues,
unsigned num_threads,
uint64_t delay_us,
unsigned idle_queues)
{
EXPECT_LT(0U, tasks_per_queue);
EXPECT_LT(0U, num_queues);
EXPECT_LE(0U, idle_queues);
boost::property_tree::ptree pt;
PARAMETER_TYPE(ip::perf_threadpool_test_threads)(num_threads).persist(pt);
pt.put("version", 1);
std::unique_ptr<threadpool_type> tp(new threadpool_type(pt));
BOOST_SCOPE_EXIT_TPL((&tp))
{
EXPECT_NO_THROW(tp->stop()) << "Failed to stop threadpool";
}
BOOST_SCOPE_EXIT_END;
{
blocker_ptr_vec blockers(idle_queues);
for (size_t i = 0; i < idle_queues; ++i)
{
blockers[i] = blocker_ptr(new Blocker(*tp, num_queues + i));
}
}
callback_ptr_vec callbacks(num_queues);
for (size_t i = 0; i < callbacks.size(); ++i)
{
callbacks[i] = callback_ptr(new Callback(tasks_per_queue, delay_us));
}
youtils::wall_timer t;
double post_time;
if (prefill)
{
blocker_ptr_vec blockers(num_queues);
for (size_t i = 0; i < blockers.size(); ++i)
{
blockers[i] = blocker_ptr(new Blocker(*tp, i));
}
post_time = post_tasks_(insert_barrier, *tp, callbacks, tasks_per_queue);
t.restart();
}
else
{
post_time = post_tasks_(insert_barrier, *tp, callbacks, tasks_per_queue);
}
for (size_t i = 0; i < callbacks.size(); ++i)
{
callback_ptr cb = callbacks[i];
std::unique_lock<Callback::lock_type> u(cb->lock_);
while (cb->count_ > 0)
{
ASSERT(cb->count_ <= tasks_per_queue);
cb->cond_.wait(u);
}
}
const double proc_time = t.elapsed();
std::cout <<
"# queues: " << num_queues <<
", tasks per queue: " << tasks_per_queue <<
", # idle queues: " << idle_queues <<
", threads in pool: " << tp->getNumThreads() <<
", delay per task (us): " << delay_us <<
", processing duration (s): " << proc_time <<
std::endl;
return std::make_pair(post_time, proc_time);
}
/** Verify TPC-B results. */
ErrorStack verify_tpcb_task(const proc::ProcArguments& args) {
thread::Thread* context = args.context_;
xct::XctManager* xct_manager = context->get_engine()->get_xct_manager();
CHECK_ERROR(xct_manager->begin_xct(context, xct::kSerializable));
int64_t expected_branch[kBranches];
int64_t expected_teller[kBranches * kTellers];
int64_t expected_account[kBranches * kAccounts];
for (int i = 0; i < kBranches; ++i) {
expected_branch[i] = kInitialAccountBalance * kAccounts;
}
for (int i = 0; i < kBranches * kTellers; ++i) {
expected_teller[i] = kInitialAccountBalance * kAccountsPerTellers;
}
for (int i = 0; i < kBranches * kAccounts; ++i) {
expected_account[i] = kInitialAccountBalance;
}
// we don't have scanning API yet, so manually do it.
std::set<uint64_t> observed_history_ids;
WRAP_ERROR_CODE(sequential::SequentialStoragePimpl(
context->get_engine(), histories.get_control_block()).for_every_page(
[&](SequentialPage* page){
uint16_t record_count = page->get_record_count();
const char* record_pointers[kMaxSlots];
uint16_t payload_lengthes[kMaxSlots];
page->get_all_records_nosync(&record_count, record_pointers, payload_lengthes);
for (uint16_t rec = 0; rec < record_count; ++rec) {
EXPECT_EQ(payload_lengthes[rec], sizeof(HistoryData));
const HistoryData& history = *reinterpret_cast<const HistoryData*>(
record_pointers[rec] + kRecordOverhead);
EXPECT_GE(history.amount_, kAmountRangeFrom);
EXPECT_LE(history.amount_, kAmountRangeTo);
EXPECT_LT(history.branch_id_, kBranches);
EXPECT_LT(history.teller_id_, kBranches * kTellers);
EXPECT_LT(history.account_id_, kBranches * kAccounts);
EXPECT_EQ(history.branch_id_, history.teller_id_ / kTellers);
EXPECT_EQ(history.branch_id_, history.account_id_ / kAccounts);
EXPECT_EQ(history.teller_id_, history.account_id_ / kAccountsPerTellers);
expected_branch[history.branch_id_] += history.amount_;
expected_teller[history.teller_id_] += history.amount_;
expected_account[history.account_id_] += history.amount_;
EXPECT_EQ(observed_history_ids.end(), observed_history_ids.find(history.history_id_))
<< history.history_id_;
observed_history_ids.insert(history.history_id_);
}
return kErrorCodeOk;
}));
EXPECT_EQ(kXctsPerThread * thread_count, observed_history_ids.size());
for (int i = 0; i < kXctsPerThread * thread_count; ++i) {
EXPECT_NE(observed_history_ids.end(), observed_history_ids.find(i)) << i;
}
for (int i = 0; i < kBranches; ++i) {
BranchData data;
CHECK_ERROR(branches.get_record(context, i, &data));
EXPECT_EQ(expected_branch[i], data.branch_balance_) << "branch-" << i;
}
for (int i = 0; i < kBranches * kTellers; ++i) {
TellerData data;
CHECK_ERROR(tellers.get_record(context, i, &data));
EXPECT_EQ(i / kTellers, data.branch_id_) << i;
EXPECT_EQ(expected_teller[i], data.teller_balance_) << "teller-" << i;
}
for (int i = 0; i < kBranches * kAccounts; ++i) {
AccountData data;
CHECK_ERROR(accounts.get_record(context, i, &data));
EXPECT_EQ(i / kAccounts, data.branch_id_) << i;
EXPECT_EQ(expected_account[i], data.account_balance_) << "account-" << i;
}
for (uint32_t i = 0; i < context->get_current_xct().get_read_set_size(); ++i) {
xct::ReadXctAccess& access = context->get_current_xct().get_read_set()[i];
EXPECT_FALSE(access.observed_owner_id_.is_being_written()) << i;
EXPECT_FALSE(access.observed_owner_id_.is_deleted()) << i;
EXPECT_FALSE(access.observed_owner_id_.is_moved()) << i;
}
CHECK_ERROR(xct_manager->abort_xct(context));
return foedus::kRetOk;
}
TEST(logd, statistics) {
size_t len;
char *buf;
alloc_statistics(&buf, &len);
#ifdef TARGET_USES_LOGD
ASSERT_TRUE(NULL != buf);
#else
if (!buf) {
return;
}
#endif
// remove trailing FF
char *cp = buf + len - 1;
*cp = '\0';
bool truncated = *--cp != '\f';
if (!truncated) {
*cp = '\0';
}
// squash out the byte count
cp = buf;
if (!truncated) {
while (isdigit(*cp) || (*cp == '\n')) {
++cp;
}
}
fprintf(stderr, "%s", cp);
EXPECT_LT((size_t)64, strlen(cp));
EXPECT_EQ(0, truncated);
#ifdef TARGET_USES_LOGD
char *main_logs = strstr(cp, "\nmain:");
EXPECT_TRUE(NULL != main_logs);
char *radio_logs = strstr(cp, "\nradio:");
EXPECT_TRUE(NULL != radio_logs);
char *system_logs = strstr(cp, "\nsystem:");
EXPECT_TRUE(NULL != system_logs);
char *events_logs = strstr(cp, "\nevents:");
EXPECT_TRUE(NULL != events_logs);
#endif
// Parse timing stats
cp = strstr(cp, "Minimum time between log events per dgram_qlen:");
if (cp) {
while (*cp && (*cp != '\n')) {
++cp;
}
if (*cp == '\n') {
++cp;
}
char *list_of_spans = cp;
EXPECT_NE('\0', *list_of_spans);
unsigned short number_of_buckets = 0;
unsigned short *dgram_qlen = NULL;
unsigned short bucket = 0;
while (*cp && (*cp != '\n')) {
bucket = 0;
while (isdigit(*cp)) {
bucket = bucket * 10 + *cp - '0';
++cp;
}
while (*cp == ' ') {
++cp;
}
if (!bucket) {
break;
}
unsigned short *new_dgram_qlen = new unsigned short[number_of_buckets + 1];
EXPECT_TRUE(new_dgram_qlen != NULL);
if (dgram_qlen) {
memcpy(new_dgram_qlen, dgram_qlen, sizeof(*dgram_qlen) * number_of_buckets);
delete [] dgram_qlen;
}
dgram_qlen = new_dgram_qlen;
dgram_qlen[number_of_buckets++] = bucket;
}
char *end_of_spans = cp;
EXPECT_NE('\0', *end_of_spans);
EXPECT_LT(5, number_of_buckets);
unsigned long long *times = new unsigned long long [number_of_buckets];
ASSERT_TRUE(times != NULL);
memset(times, 0, sizeof(*times) * number_of_buckets);
while (*cp == '\n') {
++cp;
}
unsigned short number_of_values = 0;
unsigned long long value;
while (*cp && (*cp != '\n')) {
EXPECT_GE(number_of_buckets, number_of_values);
value = 0;
while (isdigit(*cp)) {
value = value * 10ULL + *cp - '0';
++cp;
}
switch(*cp) {
case ' ':
case '\n':
value *= 1000ULL;
/* FALLTHRU */
case 'm':
value *= 1000ULL;
/* FALLTHRU */
case 'u':
value *= 1000ULL;
/* FALLTHRU */
case 'n':
default:
break;
}
while (*++cp == ' ');
if (!value) {
break;
}
times[number_of_values] = value;
++number_of_values;
}
#ifdef TARGET_USES_LOGD
EXPECT_EQ(number_of_values, number_of_buckets);
#endif
FILE *fp;
ASSERT_TRUE(NULL != (fp = fopen("/proc/sys/net/unix/max_dgram_qlen", "r")));
unsigned max_dgram_qlen = 0;
fscanf(fp, "%u", &max_dgram_qlen);
fclose(fp);
// Find launch point
unsigned short launch = 0;
unsigned long long total = 0;
do {
total += times[launch];
} while (((++launch < number_of_buckets)
&& ((total / launch) >= (times[launch] / 8ULL)))
|| (launch == 1)); // too soon
bool failure = number_of_buckets <= launch;
if (!failure) {
unsigned short l = launch;
if (l >= number_of_buckets) {
l = number_of_buckets - 1;
}
failure = max_dgram_qlen < dgram_qlen[l];
}
// We can get failure if at any time liblog_benchmarks has been run
// because designed to overload /proc/sys/net/unix/max_dgram_qlen even
// at excessive values like 20000. It does so to measure the raw processing
// performance of logd.
if (failure) {
cp = find_benchmark_spam(cp);
}
if (cp) {
// Fake a failure, but without the failure code
if (number_of_buckets <= launch) {
printf ("Expected: number_of_buckets > launch, actual: %u vs %u\n",
number_of_buckets, launch);
}
if (launch >= number_of_buckets) {
launch = number_of_buckets - 1;
}
if (max_dgram_qlen < dgram_qlen[launch]) {
printf ("Expected: max_dgram_qlen >= dgram_qlen[%d],"
" actual: %u vs %u\n",
launch, max_dgram_qlen, dgram_qlen[launch]);
}
} else
#ifndef TARGET_USES_LOGD
if (total)
#endif
{
EXPECT_GT(number_of_buckets, launch);
if (launch >= number_of_buckets) {
launch = number_of_buckets - 1;
}
EXPECT_GE(max_dgram_qlen, dgram_qlen[launch]);
}
delete [] dgram_qlen;
delete [] times;
}
delete [] buf;
}
TEST_F(PIDControllerTest, basicVerificationTest)
{
int loops = 0;
int onTargetLoops = 0;
// Check that position mode works as expected
printf("Setting PIDF values to [ 1 0.001 0.01 0 ]\n");
pidController->setConstants(1, 0.001, .01, 0);
printf("Setting setpoint to 10 revolutions\n");
pidController->setSetpoint(10);
pidController->setMode(POSITION_REV);
pidController->enable();
while(onTargetLoops < 10)
{
pidController->update();
if(pidController->onTarget())
{
onTargetLoops++;
}
else if(onTargetLoops > 0)
{
onTargetLoops--;
}
loops++;
}
printf("Verifying that PID took 97 loops to settle\n");
EXPECT_EQ(97, loops);
printf("Verifying that encoder position is within tolerance\n");
EXPECT_LT(10 - POSITION_REV_TOLERANCE, encoder->getPosition());
EXPECT_GT(10 + POSITION_REV_TOLERANCE, encoder->getPosition());
printf("Resetting PIDController and encoder for next test\n");
pidController->reset();
encoder->reset(true);
encoder->setRandSeed(0);
loops = 0;
onTargetLoops = 0;
// Check that raw position mode works as expected
printf("Setting PIDF values to [ 0.001 0.000001 0.001 0 ]\n");
pidController->setConstants(.001, 0.000001, .001, 0);
printf("Setting setpoint to 10240 ticks\n");
pidController->setSetpoint(10240);
pidController->setMode(POSITION_RAW);
pidController->enable();
while(onTargetLoops < 10)
{
pidController->update();
if(pidController->onTarget())
{
onTargetLoops++;
}
else if(onTargetLoops > 0)
{
onTargetLoops--;
}
loops++;
}
printf("Verifying that PID took 104 loops to settle\n");
EXPECT_EQ(104, loops);
printf("Verifying that encoder position is within tolerance\n");
EXPECT_LT(10240 - POSITION_RAW_TOLERANCE, encoder->getRaw());
EXPECT_GT(10240 + POSITION_RAW_TOLERANCE, encoder->getRaw());
printf("Resetting PIDController and encoder for next test\n");
pidController->reset();
encoder->reset(true);
encoder->setRandSeed(0);
loops = 0;
onTargetLoops = 0;
// Check that speed mode works as expected
printf("Setting PIDF values to [ 0.01 0.1 0.00001 0.01 ]\n");
pidController->setConstants(.01, 0.1, .00001, 0.01);
printf("Setting setpoint to 10 revolutions per second\n");
pidController->setSetpoint(10);
pidController->setMode(SPEED);
pidController->enable();
while(onTargetLoops < 10)
{
pidController->update();
if(pidController->onTarget())
{
onTargetLoops++;
}
else if(onTargetLoops > 0)
{
onTargetLoops--;
}
loops++;
}
printf("Verifying that PID took 32 loops to settle\n");
EXPECT_EQ(32, loops);
printf("Verifying that encoder position is within tolerance\n");
EXPECT_LT(10 - SPEED_TOLERANCE, encoder->getSpeed());
EXPECT_GT(10 + SPEED_TOLERANCE, encoder->getSpeed());
printf("Resetting PIDController and encoder for next test\n");
pidController->reset();
encoder->reset(true);
encoder->setRandSeed(0);
loops = 0;
onTargetLoops = 0;
// Check that PID doesn't run when disabled
printf("Verifying that PID can't run while disabled\n");
pidController->update();
EXPECT_EQ(0, motor->getOutput());
// Check that mode can't be changed when PID is enabled
printf("Verifying that control mode can't be changed while PID is running\n");
pidController->enable();
pidController->setMode(POSITION_RAW);
ASSERT_EQ(SPEED, pidController->getMode());
}
TEST_F(QuotaTest, multipleQuotas)
{
MojObject obj;
// put quota (from testUsage)
MojAssertNoErr( obj.fromJson(_T("{\"owner\":\"com.foo.bar\",\"size\":1000}")) );
MojAssertNoErr( db.putQuotas(&obj, &obj + 1) );
// quota for com.foo.baz
MojAssertNoErr( obj.fromJson(_T("{\"owner\":\"com.foo.baz\",\"size\":1000}")) );
MojAssertNoErr( db.putQuotas(&obj, &obj + 1) );
// register kinds
MojAssertNoErr( obj.fromJson(MojTestKind1Str1) );
MojExpectNoErr( db.putKind(obj) );
MojAssertNoErr( obj.fromJson(MojTestKind2Str1) );
MojExpectNoErr( db.putKind(obj) );
// put object of kind1 and kind2
EXPECT_NO_FATAL_FAILURE( put(db, MojTestKind1Objects[0]) );
EXPECT_NO_FATAL_FAILURE( put(db, MojTestKind2Objects[0]) );
MojInt64 quotaUsage1 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage1) );
EXPECT_LE( 0, quotaUsage1 );
MojInt64 quotaUsage2 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.baz"), quotaUsage2) );
EXPECT_LE( 0, quotaUsage2 );
EXPECT_LT( 0, quotaUsage1 );
EXPECT_EQ( 0, quotaUsage2 );
// change owner of kind2 to com.foo.baz
MojAssertNoErr( obj.fromJson(MojTestKind2Str2) );
MojExpectNoErr( db.putKind(obj) );
MojInt64 quotaUsage3 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage3) );
EXPECT_LE( 0, quotaUsage3 );
MojInt64 quotaUsage4 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.baz"), quotaUsage4) );
EXPECT_LE( 0, quotaUsage4 );
EXPECT_LT( 0, quotaUsage3 );
EXPECT_EQ( quotaUsage3, quotaUsage4);
// make kind2 inherit from kind1
MojAssertNoErr( obj.fromJson(MojTestKind2Str3) );
MojExpectNoErr( db.putKind(obj) );
MojInt64 quotaUsage5 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage5) );
MojInt64 quotaUsage6 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.baz"), quotaUsage6) );
EXPECT_GT( quotaUsage5, quotaUsage1 );
EXPECT_EQ( 0, quotaUsage6 );
// kind3 and object
MojAssertNoErr( obj.fromJson(MojTestKind3Str1) );
MojExpectNoErr( db.putKind(obj) );
EXPECT_NO_FATAL_FAILURE( put(db, MojTestKind3Objects[0]) );
MojInt64 quotaUsage7 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage7) );
EXPECT_EQ( quotaUsage7, quotaUsage5 );
// wildcard
MojAssertNoErr( obj.fromJson(_T("{\"owner\":\"com.foo.*\",\"size\":1000}")) );
MojExpectNoErr( db.putQuotas(&obj, &obj + 1) );
MojInt64 quotaUsage8 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.bar"), quotaUsage8) );
MojInt64 quotaUsage9 = -1;
EXPECT_NO_FATAL_FAILURE( getQuotaUsage(db, _T("com.foo.*"), quotaUsage9) );
EXPECT_EQ( quotaUsage5, quotaUsage8 );
EXPECT_LT( 0, quotaUsage9 );
}
TEST(logcat, sorted_order) {
FILE *fp;
ASSERT_TRUE(NULL != (fp = popen(
"logcat -v time -b radio -b events -b system -b main -d 2>/dev/null",
"r")));
class timestamp {
private:
int month;
int day;
int hour;
int minute;
int second;
int millisecond;
bool ok;
public:
void init(const char *buffer)
{
ok = false;
if (buffer != NULL) {
ok = sscanf(buffer, "%d-%d %d:%d:%d.%d ",
&month, &day, &hour, &minute, &second, &millisecond) == 6;
}
}
timestamp(const char *buffer)
{
init(buffer);
}
bool operator< (timestamp &T)
{
return !ok || !T.ok
|| (month < T.month)
|| ((month == T.month)
&& ((day < T.day)
|| ((day == T.day)
&& ((hour < T.hour)
|| ((hour == T.hour)
&& ((minute < T.minute)
|| ((minute == T.minute)
&& ((second < T.second)
|| ((second == T.second)
&& (millisecond < T.millisecond))))))))));
}
bool valid(void)
{
return ok;
}
} last(NULL);
char *last_buffer = NULL;
char buffer[5120];
int count = 0;
int next_lt_last = 0;
while (fgets(buffer, sizeof(buffer), fp)) {
if (!strncmp(begin, buffer, sizeof(begin) - 1)) {
continue;
}
if (!last.valid()) {
free(last_buffer);
last_buffer = strdup(buffer);
last.init(buffer);
}
timestamp next(buffer);
if (next < last) {
if (last_buffer) {
fprintf(stderr, "<%s", last_buffer);
}
fprintf(stderr, ">%s", buffer);
++next_lt_last;
}
if (next.valid()) {
free(last_buffer);
last_buffer = strdup(buffer);
last.init(buffer);
}
++count;
}
free(last_buffer);
pclose(fp);
static const int max_ok = 2;
// Allow few fails, happens with readers active
fprintf(stderr, "%s: %d/%d out of order entries\n",
(next_lt_last)
? ((next_lt_last <= max_ok)
? "WARNING"
: "ERROR")
: "INFO",
next_lt_last, count);
EXPECT_GE(max_ok, next_lt_last);
// sample statistically too small
EXPECT_LT(100, count);
}
TEST(InterfacesTest, TwistBasicIO)
{
ros::NodeHandle nh;
ros::Publisher twistPub = nh.advertise<geometry_msgs::TwistWithCovarianceStamped>("/twist_input0", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
geometry_msgs::TwistWithCovarianceStamped twist;
twist.twist.twist.linear.x = 5.0;
twist.twist.twist.linear.y = 0;
twist.twist.twist.linear.z = 0;
twist.twist.twist.angular.x = 0;
twist.twist.twist.angular.y = 0;
twist.twist.twist.angular.z = 0;
for (size_t ind = 0; ind < 36; ind+=7)
{
twist.twist.covariance[ind] = 1e-6;
}
twist.header.frame_id = "base_link";
ros::Rate loopRate = ros::Rate(20);
for (size_t i = 0; i < 400; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0);
resetFilter();
twist.twist.twist.linear.x = 0.0;
twist.twist.twist.linear.y = 5.0;
loopRate = ros::Rate(20);
for (size_t i = 0; i < 200; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.y - twist.twist.twist.linear.y), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 50.0), 1.0);
resetFilter();
twist.twist.twist.linear.y = 0.0;
twist.twist.twist.linear.z = 5.0;
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - twist.twist.twist.linear.z), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 25.0), 1.0);
resetFilter();
twist.twist.twist.linear.z = 0.0;
twist.twist.twist.linear.x = 1.0;
twist.twist.twist.angular.z = (M_PI/2) / (100.0 * 0.05);
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1);
EXPECT_LT(::fabs(filtered_.twist.twist.angular.z - twist.twist.twist.angular.z), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5);
resetFilter();
twist.twist.twist.linear.x = 0.0;
twist.twist.twist.angular.z = 0.0;
twist.twist.twist.angular.x = -(M_PI/2) / (100.0 * 0.05);
// First, roll the vehicle on its side
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
twist.twist.twist.angular.x = 0.0;
twist.twist.twist.angular.y = (M_PI/2) / (100.0 * 0.05);
// Now, pitch it down (positive pitch velocity in vehicle frame)
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
twist.twist.twist.angular.y = 0.0;
twist.twist.twist.linear.x = 3.0;
// We should now be on our side and facing -Y. Move forward in
// the vehicle frame X direction, and make sure Y decreases in
// the world frame.
loopRate = ros::Rate(20);
for (size_t i = 0; i < 100; ++i)
{
twist.header.stamp = ros::Time::now();
twistPub.publish(twist);
ros::spinOnce();
loopRate.sleep();
twist.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0);
resetFilter();
}
TEST(Type, Const) {
auto five = Type::cns(5);
EXPECT_LT(five, Type::Int);
EXPECT_NE(five, Type::Int);
EXPECT_TRUE(five.isConst());
EXPECT_EQ(5, five.intVal());
EXPECT_TRUE(five.isConst(Type::Int));
EXPECT_TRUE(five.isConst(5));
EXPECT_FALSE(five.isConst(5.0));
EXPECT_TRUE(Type::Gen.maybe(five));
EXPECT_EQ(Type::Int, five | Type::Int);
EXPECT_EQ(Type::Int, five | Type::cns(10));
EXPECT_EQ(five, five | Type::cns(5));
EXPECT_EQ(five, Type::cns(5) & five);
EXPECT_EQ(five, five & Type::Int);
EXPECT_EQ(five, Type::Gen & five);
EXPECT_EQ("Int<5>", five.toString());
EXPECT_EQ(five, five - Type::Arr);
EXPECT_EQ(five, five - Type::cns(1));
EXPECT_EQ(Type::Int, Type::Int - five); // conservative
EXPECT_EQ(Type::Bottom, five - Type::Int);
EXPECT_EQ(Type::Bottom, five - five);
EXPECT_EQ(Type::PtrToGen,
(Type::PtrToGen|Type::Nullptr) - Type::Nullptr);
EXPECT_EQ(Type::Int, five.dropConstVal());
EXPECT_TRUE(five.not(Type::cns(2)));
auto True = Type::cns(true);
EXPECT_EQ("Bool<true>", True.toString());
EXPECT_LT(True, Type::Bool);
EXPECT_NE(True, Type::Bool);
EXPECT_TRUE(True.isConst());
EXPECT_EQ(true, True.boolVal());
EXPECT_TRUE(Type::Uncounted.maybe(True));
EXPECT_FALSE(five <= True);
EXPECT_FALSE(five > True);
EXPECT_TRUE(five.not(True));
EXPECT_EQ(Type::Int | Type::Bool, five | True);
EXPECT_EQ(Type::Bottom, five & True);
EXPECT_TRUE(Type::Uninit.isConst());
EXPECT_TRUE(Type::InitNull.isConst());
EXPECT_FALSE(Type::Null.isConst());
EXPECT_FALSE((Type::Uninit | Type::Bool).isConst());
EXPECT_FALSE(Type::Int.isConst());
auto array = make_packed_array(1, 2, 3, 4);
auto arrData = ArrayData::GetScalarArray(array.get());
auto constArray = Type::cns(arrData);
auto packedArray = Type::Arr.specialize(ArrayData::kPackedKind);
auto mixedArray = Type::Arr.specialize(ArrayData::kMixedKind);
EXPECT_TRUE(constArray <= packedArray);
EXPECT_TRUE(constArray < packedArray);
EXPECT_FALSE(packedArray <= constArray);
EXPECT_TRUE(constArray <= constArray);
EXPECT_FALSE(packedArray <= mixedArray);
EXPECT_FALSE(mixedArray <= packedArray);
EXPECT_FALSE(constArray <= mixedArray);
EXPECT_EQ(constArray, constArray & packedArray);
ArrayTypeTable::Builder ratBuilder;
auto rat1 = ratBuilder.packedn(RepoAuthType::Array::Empty::No,
RepoAuthType(RepoAuthType::Tag::Str));
auto ratArray1 = Type::Arr.specialize(rat1);
auto rat2 = ratBuilder.packedn(RepoAuthType::Array::Empty::No,
RepoAuthType(RepoAuthType::Tag::Int));
auto ratArray2 = Type::Arr.specialize(rat2);
EXPECT_EQ(Type::Arr, ratArray1 & ratArray2);
EXPECT_TRUE(ratArray1 < Type::Arr);
EXPECT_TRUE(ratArray1 <= ratArray1);
EXPECT_TRUE(ratArray1 < (Type::Arr|Type::Obj));
EXPECT_FALSE(ratArray1 < ratArray2);
EXPECT_NE(ratArray1, ratArray2);
auto packedRat = packedArray & ratArray1;
EXPECT_EQ("Arr=PackedKind:N([Str])", packedRat.toString());
EXPECT_TRUE(packedRat <= packedArray);
EXPECT_TRUE(packedRat < packedArray);
EXPECT_TRUE(packedRat <= ratArray1);
EXPECT_TRUE(packedRat < ratArray1);
EXPECT_EQ(packedRat, packedRat & packedArray);
EXPECT_EQ(packedRat, packedRat & ratArray1);
}
TEST(InterfacesTest, ImuPoseBasicIO)
{
ros::NodeHandle nh;
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input0", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
tf2::Quaternion quat;
quat.setRPY(M_PI/4, -M_PI/4, M_PI/2);
imu.orientation = tf2::toMsg(quat);
for (size_t ind = 0; ind < 9; ind+=4)
{
imu.orientation_covariance[ind] = 1e-6;
}
imu.header.frame_id = "base_link";
// Make sure the pose reset worked. Test will timeout
// if this fails.
ros::Rate loopRate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
ros::spinOnce();
loopRate.sleep();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
tf2::Matrix3x3 mat(quat);
double r, p, y;
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r - M_PI/4), 0.1);
EXPECT_LT(::fabs(p + M_PI/4), 0.1);
EXPECT_LT(::fabs(y - M_PI/2), 0.1);
EXPECT_LT(filtered_.pose.covariance[21], 0.5);
EXPECT_LT(filtered_.pose.covariance[28], 0.25);
EXPECT_LT(filtered_.pose.covariance[35], 0.5);
resetFilter();
// Test to see if the orientation data is ignored when we set the
// first covariance value to -1
sensor_msgs::Imu imuIgnore;
imuIgnore.orientation.x = 0.1;
imuIgnore.orientation.y = 0.2;
imuIgnore.orientation.z = 0.3;
imuIgnore.orientation.w = 0.4;
imuIgnore.orientation_covariance[0] = -1;
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imuIgnore.header.stamp = ros::Time::now();
imuPub.publish(imuIgnore);
loopRate.sleep();
ros::spinOnce();
imuIgnore.header.seq++;
}
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 1e-3);
EXPECT_LT(::fabs(p), 1e-3);
EXPECT_LT(::fabs(y), 1e-3);
resetFilter();
}
// Tests that the Variant::operator< method works for double-float comparison
TEST_F(TestVariant, OperatorLTDoubleFloat) {
Variant v1((double) 124.08);
Variant v2((float) 124.09);
EXPECT_LT(v1, v2);
}
TEST(InterfacesTest, ImuTwistBasicIO)
{
ros::NodeHandle nh;
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input1", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
tf2::Quaternion quat;
imu.angular_velocity.x = (M_PI / 2.0);
for (size_t ind = 0; ind < 9; ind+=4)
{
imu.angular_velocity_covariance[ind] = 1e-6;
}
imu.header.frame_id = "base_link";
ros::Rate loopRate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
tf2::Matrix3x3 mat(quat);
double r, p, y;
mat.getRPY(r, p, y);
// Tolerances may seem loose, but the initial state covariances
// are tiny, so the filter is sluggish to accept velocity data
EXPECT_LT(::fabs(r - M_PI / 2.0), 0.7);
EXPECT_LT(::fabs(p), 0.1);
EXPECT_LT(::fabs(y), 0.1);
EXPECT_LT(filtered_.twist.covariance[21], 1e-3);
EXPECT_LT(filtered_.pose.covariance[21], 0.1);
resetFilter();
imu.angular_velocity.x = 0.0;
imu.angular_velocity.y = -(M_PI / 2.0);
// Make sure the pose reset worked. Test will timeout
// if this fails.
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 0.1);
EXPECT_LT(::fabs(p + M_PI / 2.0), 0.7);
EXPECT_LT(::fabs(y), 0.1);
EXPECT_LT(filtered_.twist.covariance[28], 1e-3);
EXPECT_LT(filtered_.pose.covariance[28], 0.1);
resetFilter();
imu.angular_velocity.y = 0;
imu.angular_velocity.z = M_PI / 4.0;
// Make sure the pose reset worked. Test will timeout
// if this fails.
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
// Now check the values from the callback
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 0.1);
EXPECT_LT(::fabs(p), 0.1);
EXPECT_LT(::fabs(y - M_PI / 4.0), 0.7);
EXPECT_LT(filtered_.twist.covariance[35], 1e-3);
EXPECT_LT(filtered_.pose.covariance[35], 0.1);
resetFilter();
// Test to see if the angular velocity data is ignored when we set the
// first covariance value to -1
sensor_msgs::Imu imuIgnore;
imuIgnore.angular_velocity.x = 100;
imuIgnore.angular_velocity.y = 100;
imuIgnore.angular_velocity.z = 100;
imuIgnore.angular_velocity_covariance[0] = -1;
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imuIgnore.header.stamp = ros::Time::now();
imuPub.publish(imuIgnore);
loopRate.sleep();
ros::spinOnce();
imuIgnore.header.seq++;
}
tf2::fromMsg(filtered_.pose.pose.orientation, quat);
mat.setRotation(quat);
mat.getRPY(r, p, y);
EXPECT_LT(::fabs(r), 1e-3);
EXPECT_LT(::fabs(p), 1e-3);
EXPECT_LT(::fabs(y), 1e-3);
resetFilter();
}
// Tests that the Variant::operator< method works for float-string comparison
TEST_F(TestVariant, OperatorLTFloatString) {
Variant v1((float) 124.08);
Variant v2("124.09");
EXPECT_LT(v1, v2);
}
TEST(InterfacesTest, ImuAccBasicIO)
{
ros::NodeHandle nh;
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("/imu_input2", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
sensor_msgs::Imu imu;
imu.header.frame_id = "base_link";
imu.linear_acceleration_covariance[0] = 1e-6;
imu.linear_acceleration_covariance[4] = 1e-6;
imu.linear_acceleration_covariance[8] = 1e-6;
imu.linear_acceleration.x = 1;
imu.linear_acceleration.y = -1;
imu.linear_acceleration.z = 1;
// Move with constant acceleration for 1 second,
// then check our velocity.
ros::Rate loopRate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - 1.0), 0.4);
EXPECT_LT(::fabs(filtered_.twist.twist.linear.y + 1.0), 0.4);
EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - 1.0), 0.4);
imu.linear_acceleration.x = 0.0;
imu.linear_acceleration.y = 0.0;
imu.linear_acceleration.z = 0.0;
// Now move for another second, and see where we
// end up
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imu.header.stamp = ros::Time::now();
imuPub.publish(imu);
loopRate.sleep();
ros::spinOnce();
imu.header.seq++;
}
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1.2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 1.2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 1.2), 0.4);
resetFilter();
// Test to see if the linear acceleration data is ignored when we set the
// first covariance value to -1
sensor_msgs::Imu imuIgnore;
imuIgnore.linear_acceleration.x = 1000;
imuIgnore.linear_acceleration.y = 1000;
imuIgnore.linear_acceleration.z = 1000;
imuIgnore.linear_acceleration_covariance[0] = -1;
loopRate = ros::Rate(50);
for (size_t i = 0; i < 50; ++i)
{
imuIgnore.header.stamp = ros::Time::now();
imuPub.publish(imuIgnore);
loopRate.sleep();
ros::spinOnce();
imuIgnore.header.seq++;
}
EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 1e-3);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 1e-3);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 1e-3);
resetFilter();
}
TEST(ControlSystem, pitch)
{
double test_pitch = 15;
double overshoot_allowed = 2.5;
double average_threshold = 2.0;
double std_dev_allowed = 1.5;
rs::SubscriberAnalyzer<robosub_msgs::Euler> analyzer;
analyzer.Init("pretty/orientation", &get_pitch_data);
robosub_msgs::control msg;
//keep the sub steady
msg.forward_state = robosub_msgs::control::STATE_ERROR;
msg.forward = 0;
msg.strafe_state = robosub_msgs::control::STATE_ERROR;
msg.strafe_left = 0;
msg.yaw_state = robosub_msgs::control::STATE_RELATIVE;
msg.yaw_left = 0;
msg.dive_state = robosub_msgs::control::STATE_ABSOLUTE;
msg.dive = -1;
msg.roll_state = robosub_msgs::control::STATE_ABSOLUTE;
msg.roll_right = 0;
//just go to pitch goal
msg.pitch_state = robosub_msgs::control::STATE_ABSOLUTE;
msg.pitch_down = test_pitch;
//fill out a control message to pitch
pub.publish(msg);
analyzer.Start();
ROS_INFO("pitching to test angle");
//wait for the sub to reach its pitch
ros::Time exit_time = ros::Time::now() + ros::Duration(10);
while (ros::Time::now() < exit_time)
{
ros::spinOnce();
ros::Duration(0.01).sleep();
}
analyzer.Stop();
//confirm we didn't pitch too far
EXPECT_LT(analyzer.GetMax(), test_pitch + overshoot_allowed);
ROS_INFO("maintaining pitch to check steady-state oscillation...");
analyzer.ClearData();
analyzer.Start();
//wait for 10 seconds to measure wiggle
exit_time = ros::Time::now() + ros::Duration(10);
while (ros::Time::now() < exit_time)
{
ros::spinOnce();
ros::Duration(0.01).sleep();
}
analyzer.Stop();
//confirm pitch is stable
ROS_INFO_STREAM("Pitch Average: " << analyzer.GetAverage());
ROS_INFO_STREAM("Pitch STD Dev: " << analyzer.GetStandardDeviation());
EXPECT_NEAR(test_pitch, analyzer.GetAverage(), average_threshold);
EXPECT_LT(analyzer.GetStandardDeviation(), std_dev_allowed);
}
TEST(InterfacesTest, OdomDifferentialIO)
{
ros::NodeHandle nh;
ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("/odom_input1", 5);
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
nav_msgs::Odometry odom;
odom.pose.pose.position.x = 20.0;
odom.pose.pose.position.y = 10.0;
odom.pose.pose.position.z = -40.0;
odom.pose.pose.orientation.w = 1;
odom.pose.covariance[0] = 2.0;
odom.pose.covariance[7] = 2.0;
odom.pose.covariance[14] = 2.0;
odom.pose.covariance[21] = 0.2;
odom.pose.covariance[28] = 0.2;
odom.pose.covariance[35] = 0.2;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
// No guaranteeing that the zero state
// we're expecting to see here isn't just
// a result of zeroing it out previously,
// so check 10 times in succession.
size_t zeroCount = 0;
while (zeroCount++ < 10)
{
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01);
EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01);
ros::Duration(0.1).sleep();
odom.header.seq++;
}
for (size_t ind = 0; ind < 36; ind+=7)
{
odom.pose.covariance[ind] = 1e-6;
}
// Slowly move the position, and hope that the
// differential position keeps up
ros::Rate loopRate(20);
for (size_t i = 0; i < 100; ++i)
{
odom.pose.pose.position.x += 0.01;
odom.pose.pose.position.y += 0.02;
odom.pose.pose.position.z -= 0.03;
odom.header.stamp = ros::Time::now();
odomPub.publish(odom);
ros::spinOnce();
loopRate.sleep();
odom.header.seq++;
}
ros::spinOnce();
EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2);
EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4);
EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6);
resetFilter();
}
