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