TEST_F(TrajectoryLibraryTest, RemainderTrajectorySimpleAltitude) { StereoOctomap octomap(bot_frames_); // get a trajectory TrajectoryLibrary lib(0); lib.LoadLibrary("trajtest/simple", true); double altitude = 5; BotTrans trans; bot_trans_set_identity(&trans); trans.trans_vec[2] = altitude; const Trajectory *traj = lib.GetTrajectoryByNumber(0); double dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0); EXPECT_EQ_ARM(dist, -1); // add an obstacle double point[3] = { 0, 0, 6}; AddPointToOctree(&octomap, point, 0); dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0); EXPECT_EQ_ARM(dist, 6 - altitude); }
TEST_F(TrajectoryLibraryTest, ManyTrajectoriesWithTransform) { StereoOctomap octomap(bot_frames_); TrajectoryLibrary lib(0); lib.LoadLibrary("trajtest/full", true); double altitude = 30; double point[3] = {18, 12, 0}; AddPointToOctree(&octomap, point, altitude); BotTrans trans; bot_trans_set_identity(&trans); trans.trans_vec[0] = 17; trans.trans_vec[1] = 11; trans.trans_vec[2] = altitude; // send th double dist; const Trajectory *best_traj; std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0); DrawOriginLcmGl(lcm_); bot_lcmgl_color3f(lcmgl_, 0, 0, 1); best_traj->Draw(lcmgl_, &trans); EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 3); EXPECT_NEAR(dist, 1.025243, TOLERANCE); // now add a yaw trans.rot_quat[0] = 0.642787609686539; trans.rot_quat[1] = 0; trans.rot_quat[2] = 0; trans.rot_quat[3] = 0.766044443118978; bot_lcmgl_color3f(lcmgl_, 1, 0, 0); best_traj->Draw(lcmgl_, &trans); bot_lcmgl_switch_buffer(lcmgl_); std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0); //lib.Draw(lcm_, &trans); EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 2); EXPECT_NEAR(dist, 1.174604, TOLERANCE); // now have a transform with roll, pitch, and yaw trans.rot_quat[0] = 0.863589399067779; trans.rot_quat[1] = -0.004581450790098; trans.rot_quat[2] = 0.298930259006064; trans.rot_quat[3] = 0.405996379758463; std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0); EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 4); EXPECT_NEAR(dist, 0.327772, TOLERANCE); }
TEST(StereoFilterTest, OnePoint) { StereoFilter filter(0.01); lcmt::stereo msg; msg.timestamp = GetTimestampNow(); msg.video_number = 3; msg.frame_number = 452; msg.x.push_back(0); msg.y.push_back(0); msg.z.push_back(0); msg.number_of_points = 1; const lcmt::stereo *msg2; msg2 = filter.ProcessMessage(msg); EXPECT_EQ_ARM(msg2->number_of_points, 0); delete msg2; msg.frame_number = 453; msg2 = filter.ProcessMessage(msg); ASSERT_TRUE(msg2->number_of_points == 1); EXPECT_EQ_ARM(msg2->x[0], 0); EXPECT_EQ_ARM(msg2->y[0], 0); EXPECT_EQ_ARM(msg2->z[0], 0); }
TEST_F(TrajectoryLibraryTest, LoadLibrary) { TrajectoryLibrary lib(0); ASSERT_TRUE(lib.LoadLibrary("trajtest/simple", true)); EXPECT_EQ_ARM(lib.GetNumberTrajectories(), 2); EXPECT_EQ_ARM(lib.GetTrajectoryByNumber(0)->GetTrajectoryNumber(), 0); }
TEST_F(TrajectoryLibraryTest, MinimumAltitude) { Trajectory traj("trajtest/simple/two-point-00000", true); EXPECT_EQ_ARM(traj.GetMinimumAltitude(), 0); Trajectory traj2("trajtest/full/unit-testing-super-aggressive-dive-open-loop-00009", true); EXPECT_EQ_ARM(traj2.GetMinimumAltitude(), -7.9945); Trajectory traj3("trajtest/full/unit-testing-knife-edge-open-loop-00010", true); EXPECT_EQ_ARM(traj3.GetMinimumAltitude(), -1.2897); }
TEST_F(StateMachineControlTest, BasicStateMachineTest) { StateMachineControl *fsm_control = new StateMachineControl(lcm_, "../TrajectoryLibrary/trajtest/full", "tvlqr-action-out", "state-machine-state", "altitude-reset", false); int stable_traj_num = 0; int takeoff_traj_num = 2; EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), takeoff_traj_num); EXPECT_EQ_ARM(fsm_control->GetStableTrajectory().GetTrajectoryNumber(), stable_traj_num); delete fsm_control; }
/** * Tests that the state machine requests the cruise trajectory after a timeout */ TEST_F(StateMachineControlTest, TrajectoryTimeout) { StateMachineControl *fsm_control = new StateMachineControl(lcm_, "../TrajectoryLibrary/trajtest/full", "tvlqr-action-out", "state-machine-state", "altitude-reset", false); //fsm_control->GetFsmContext()->setDebugFlag(true); float altitude = 100; SubscribeLcmChannels(fsm_control); // force the state machine into autonmous mode ForceAutonomousMode(); // send an obstacle to get it to transition to a new time float point[3] = { 17, 11, 3+altitude }; SendStereoPointTriple(point); float point2[3] = { 24, 0, 0+altitude }; SendStereoPointTriple(point2); mav::pose_t msg = GetDefaultPoseMsg(); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); // ensure that we have changed trajectories EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 3); // from matlab // wait for that trajectory to time out int64_t t_start = GetTimestampNow(); double t = 0; while (t < 4.1) { usleep(7142); // 1/140 of a second msg.utime = GetTimestampNow(); t = (msg.utime - t_start) / 1000000.0; msg.pos[0] = t * 10; lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); } EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 0); delete fsm_control; UnsubscribeLcmChannels(); }
TEST_F(TrajectoryLibraryTest, ManyPointsAgainstMatlab) { StereoOctomap octomap(bot_frames_); double altitude = 30; // load points AddManyPointsToOctree(&octomap, x_points_, y_points_, z_points_, number_of_reference_points_, altitude); TrajectoryLibrary lib(0); lib.LoadLibrary("trajtest/many", true); BotTrans trans; bot_trans_set_identity(&trans); trans.trans_vec[2] = altitude; double dist; const Trajectory *best_traj; std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 50.0); ASSERT_TRUE(best_traj != nullptr); EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 1); EXPECT_NEAR(dist, 4.2911, 0.001); }
TEST_F(TrajectoryLibraryTest, CheckBounds) { Trajectory traj("trajtest/simple/two-point-00000", true); BotTrans trans; bot_trans_set_identity(&trans); double output[3]; traj.GetXyzYawTransformedPoint(0, trans, output); EXPECT_EQ_ARM(output[0], 0); traj.GetXyzYawTransformedPoint(0.01, trans, output); EXPECT_EQ_ARM(output[0], 1); traj.GetXyzYawTransformedPoint(10, trans, output); EXPECT_EQ_ARM(output[0], 1); }
/** * Loads a trajectory with two points and does basic manipulation of it. */ TEST_F(TrajectoryLibraryTest, LoadTrajectory) { // load a test trajectory Trajectory traj("trajtest/simple/two-point-00000", true); // ensure that we can access the right bits EXPECT_EQ_ARM(traj.GetDimension(), 12); EXPECT_EQ_ARM(traj.GetUDimension(), 3); Eigen::VectorXd output = traj.GetState(0); Eigen::VectorXd origin(12); origin << 0, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0; EXPECT_APPROX_MAT( origin, output, TOLERANCE); Eigen::VectorXd point(12); point << 1, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0; output = traj.GetState(0.01); EXPECT_APPROX_MAT(output, point, TOLERANCE); Eigen::VectorXd upoint(3); upoint << 0, 0, 0; EXPECT_APPROX_MAT(upoint, traj.GetUCommand(0), TOLERANCE); Eigen::VectorXd upoint2(3); upoint2 << 1, 2, 3; EXPECT_APPROX_MAT(upoint2, traj.GetUCommand(0.01), TOLERANCE); EXPECT_EQ_ARM(traj.GetTimeAtIndex(0), 0); EXPECT_EQ_ARM(traj.GetTimeAtIndex(1), 0.01); EXPECT_EQ_ARM(traj.GetNumberOfPoints(), 2); }
TEST_F(TrajectoryLibraryTest, FindFarthestWithTI) { StereoOctomap octomap(bot_frames_); TrajectoryLibrary lib(0); lib.LoadLibrary("trajtest/full", true); double altitude = 30; BotTrans trans; bot_trans_set_identity(&trans); trans.trans_vec[2] = altitude; double dist; const Trajectory *best_traj; std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 50.0); ASSERT_TRUE(best_traj != nullptr); // with no obstacles, we should always get the first trajectory EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 0); // put an obstacle along the path of the TI trajectory double point[3] = { 1.10, -0.1, 0 }; AddPointToOctree(&octomap, point, altitude); std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 50.0); EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 4); // from matlab EXPECT_NEAR(dist, 0.2032, TOLERANCE2); // from matlab // create a new octre StereoOctomap octomap2(bot_frames_); AddManyPointsToOctree(&octomap2, x_points_, y_points_, z_points_, number_of_reference_points_, altitude); std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap2, trans, 50.0); EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 3); EXPECT_NEAR(dist, 5.0060, TOLERANCE); }
TEST_F(TrajectoryLibraryTest, RemainderTrajectorySimple) { StereoOctomap octomap(bot_frames_); // get a trajectory TrajectoryLibrary lib(0); lib.LoadLibrary("trajtest/simple", true); double altitude = 100; BotTrans trans; bot_trans_set_identity(&trans); trans.trans_vec[2] = altitude; const Trajectory *traj = lib.GetTrajectoryByNumber(0); double dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0); // add an obstacle double point[3] = { 0, 0, 0}; AddPointToOctree(&octomap, point, altitude); // now we expect zero distance since the obstacle and the trajectory start at the origin dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0); EXPECT_NEAR(dist, 0, TOLERANCE); // at t = 0.01, we expect a distance of 1 since the point is already behind us dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0.01, 0); EXPECT_EQ_ARM(dist, 1); // add a transform trans.trans_vec[2] += 42; dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0); EXPECT_EQ_ARM(dist, 42); dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0.01, 0); EXPECT_NEAR(dist, sqrt( 42*42 + 1*1 ), TOLERANCE); }
TEST_F(TrajectoryLibraryTest, RemainderTrajectoryTi) { StereoOctomap octomap(bot_frames_); double altitude = 30; BotTrans trans; bot_trans_set_identity(&trans); trans.trans_vec[2] = altitude; Trajectory traj("trajtest/ti/TI-test-TI-straight-pd-no-yaw-00000", true); double dist = traj.ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0); EXPECT_EQ_ARM(dist, -1); double point[3] = { 1.5, -0.5, 3 }; AddPointToOctree(&octomap, point, altitude); dist = traj.ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0); EXPECT_NEAR(dist, 3.041506, TOLERANCE2); // from matlab dist = traj.ClosestObstacleInRemainderOfTrajectory(octomap, trans, 1.21, 0); EXPECT_NEAR(dist, 13.6886, TOLERANCE2); }
TEST(StereoFilterTest, NoHitPoints) { StereoFilter filter(0.01); lcmt::stereo msg; msg.timestamp = GetTimestampNow(); msg.video_number = 1; msg.frame_number = 0; msg.x.push_back(3.232); msg.y.push_back(10); msg.z.push_back(12.13); msg.number_of_points = 1; const lcmt::stereo *msg2; msg2 = filter.ProcessMessage(msg); delete msg2; msg2 = filter.ProcessMessage(msg); ASSERT_TRUE(msg2->number_of_points == 1); delete msg2; msg.x[0] = 1; msg.x.push_back(3.232); msg.y.push_back(10); msg.z.push_back(12.13); msg.number_of_points = 2; msg2 = filter.ProcessMessage(msg); EXPECT_EQ_ARM(msg2->number_of_points, 1); delete msg2; }
TEST_F(StateMachineControlTest, BearingController) { StateMachineControl *fsm_control = new StateMachineControl(lcm_, "../TrajectoryLibrary/trajtest/full", "tvlqr-action-out", "state-machine-state", "altitude-reset", false); //fsm_control->GetFsmContext()->setDebugFlag(true); SubscribeLcmChannels(fsm_control); // ensure that we are in the start state EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::WaitForTakeoff") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); // send some pose messages mav::pose_t msg = GetDefaultPoseMsg(); msg.pos[2] = 0; // check for unexpected transitions out of wait state lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::WaitForTakeoff") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); // cause a takeoff transistion msg.accel[0] = 100; lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::TakeoffNoThrottle") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); // wait for takeoff to finish for (int i = 0; i < 12; i++) { msg = GetDefaultPoseMsg(); msg.pos[2] = 0; msg.vel[0] = 20; lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); usleep(100000); } EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::Climb") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); // reach altitude msg = GetDefaultPoseMsg(); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::ExecuteTrajectory") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 0); // now add an obstacle in front of it and make sure it transitions! ProcessAllLcmMessages(fsm_control); ProcessAllLcmMessages(fsm_control); // send message indicating that we are turned to the right msg = GetDefaultPoseMsg(); double rpy[3] = { 0, 0, -0.5 }; double quat[4]; bot_roll_pitch_yaw_to_quat(rpy, quat); msg.orientation[0] = quat[0]; msg.orientation[1] = quat[1]; msg.orientation[2] = quat[2]; msg.orientation[3] = quat[3]; lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 3); // 3 = left turn controller // check for stop msg = GetDefaultPoseMsg(); lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 0); // check for right turn msg = GetDefaultPoseMsg(); double rpy2[3] = { 0, 0, 0.5 }; double quat2[4]; bot_roll_pitch_yaw_to_quat(rpy2, quat2); msg.orientation[0] = quat2[0]; msg.orientation[1] = quat2[1]; msg.orientation[2] = quat2[2]; msg.orientation[3] = quat2[3]; lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 4); // 4 = right turn controller // check for stop msg = GetDefaultPoseMsg(); lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 0); delete fsm_control; UnsubscribeLcmChannels(); }
/** * Tests an obstacle appearing during a trajectory execution */ TEST_F(StateMachineControlTest, TrajectoryInterrupt) { StateMachineControl *fsm_control = new StateMachineControl(lcm_, "../TrajectoryLibrary/trajtest/full", "tvlqr-action-out", "state-machine-state", "altitude-reset", false); //fsm_control->GetFsmContext()->setDebugFlag(true); SubscribeLcmChannels(fsm_control); ForceAutonomousMode(); float altitude = 100; // send an obstacle to get it to transition to a new time mav::pose_t msg = GetDefaultPoseMsg(); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); float point[3] = { 24, 0, 0+altitude }; SendStereoPointTriple(point); ProcessAllLcmMessages(fsm_control); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); // ensure that we have changed trajectories EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 2); // from matlab Trajectory running_traj = fsm_control->GetCurrentTrajectory(); // wait for that trajectory to time out int64_t t_start = GetTimestampNow(); double t = 0; while (t < 1.0) { usleep(7142); // 1/140 of a second msg.utime = GetTimestampNow(); t = (msg.utime - t_start) / 1000000.0; Eigen::VectorXd state_t = running_traj.GetState(t); msg.pos[0] = state_t(0); msg.pos[1] = state_t(1); msg.pos[2] = state_t(2) + altitude; msg.vel[0] = state_t(6); msg.vel[1] = state_t(7); msg.vel[2] = state_t(8); double rpy[3]; rpy[0] = state_t(3); rpy[1] = state_t(4); rpy[2] = state_t(5); double quat[4]; bot_roll_pitch_yaw_to_quat(rpy, quat); msg.orientation[0] = quat[0]; msg.orientation[1] = quat[1]; msg.orientation[2] = quat[2]; msg.orientation[3] = quat[3]; msg.rotation_rate[0] = state_t(9); msg.rotation_rate[1] = state_t(10); msg.rotation_rate[2] = state_t(11); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); } // now add a new obstacle right in front! std::cout << "NEW POINT" << std::endl; lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); float point2[3] = { 18, 12, 0+altitude }; SendStereoPointTriple(point2); ProcessAllLcmMessages(fsm_control); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); fsm_control->GetOctomap()->Draw(lcm_->getUnderlyingLCM()); BotTrans body_to_local; bot_frames_get_trans(bot_frames_, "body", "local", &body_to_local); fsm_control->GetTrajectoryLibrary()->Draw(lcm_->getUnderlyingLCM(), &body_to_local); fsm_control->GetTrajectoryLibrary()->Draw(lcm_->getUnderlyingLCM(), &body_to_local); bot_lcmgl_t *lcmgl = bot_lcmgl_init(lcm_->getUnderlyingLCM(), "Trjaectory"); bot_lcmgl_color3f(lcmgl, 0, 1, 0); fsm_control->GetCurrentTrajectory().Draw(lcmgl ,&body_to_local); bot_lcmgl_switch_buffer(lcmgl); bot_lcmgl_destroy(lcmgl); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 3); // from matlab delete fsm_control; UnsubscribeLcmChannels(); }
TEST_F(StereoOctomapTest, SimpleNearestNeighbor) { StereoOctomap *stereo_octomap = new StereoOctomap(bot_frames_); // first test when no points are there double origin[3]; origin[0] = 0; origin[1] = 0; origin[2] = 0; EXPECT_TRUE(stereo_octomap->NearestNeighbor(origin) == -1) << "No points in octomap failed." << std::endl; // add a point double point[3], trans_point[3]; point[0] = 1; point[1] = 0; point[2] = 0; GlobalToCameraFrame(point, trans_point); lcmt::stereo msg; msg.timestamp = GetTimestampNow(); vector<float> x, y, z; x.push_back(trans_point[0]); y.push_back(trans_point[1]); z.push_back(trans_point[2]); msg.x = x; msg.y = y; msg.z = z; msg.number_of_points = 1; msg.frame_number = 0; msg.video_number = 0; stereo_octomap->ProcessStereoMessage(&msg); EXPECT_EQ_ARM(stereo_octomap->NearestNeighbor(origin), 1); EXPECT_EQ_ARM(stereo_octomap->NearestNeighbor(point), 0); double point2[3] = {0, 0, 1}; EXPECT_NEAR(stereo_octomap->NearestNeighbor(point2), sqrt(2), TOLERANCE); lcmt::stereo msg2; msg2.timestamp = GetTimestampNow(); vector<float> x_2, y_2, z_2; double trans_point2[3]; point[0] = 0; point[1] = 2; point[2] = 0; GlobalToCameraFrame(point, trans_point2); x_2.push_back(trans_point2[0]); y_2.push_back(trans_point2[1]); z_2.push_back(trans_point2[2]); msg2.x = x_2; msg2.y = y_2; msg2.z = z_2; msg2.number_of_points = 1; msg2.frame_number = 0; msg2.video_number = 0; // add the points to the octomap stereo_octomap->ProcessStereoMessage(&msg2); // check EXPECT_NEAR(stereo_octomap->NearestNeighbor(origin), 1, TOLERANCE); double query[3] = { 1.5, 0, 0 }; EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), 0.5, TOLERANCE); query[0] = 1; query[1] = 0; query[2] = 1; // check z EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), 1, TOLERANCE); query[0] = 0; query[1] = 2.01; query[2] = 0; EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), 0.01, TOLERANCE); query[0] = 0; query[1] = 2.1; query[2] = 0.5; EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), sqrt(0.1*0.1 + 0.5*0.5), TOLERANCE); query[0] = -1; query[1] = 2.5; query[2] = 1.5; EXPECT_NEAR(stereo_octomap->NearestNeighbor(query), sqrt(1*1 + .5*.5 + 1.5*1.5), TOLERANCE); delete stereo_octomap; }
TEST_F(StateMachineControlTest, OneObstacleLowAltitude) { StateMachineControl *fsm_control = new StateMachineControl(lcm_, "../TrajectoryLibrary/trajtest/full", "tvlqr-action-out", "state-machine-state", "altitude-reset", false); //fsm_control->GetFsmContext()->setDebugFlag(true); float altitude = 5; SubscribeLcmChannels(fsm_control); // ensure that we are in the start state EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::WaitForTakeoff") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); // send some pose messages mav::pose_t msg = GetDefaultPoseMsg(); msg.pos[2] = 0; // check for unexpected transitions out of wait state lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::WaitForTakeoff") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); // cause a takeoff transistion msg.accel[0] = 100; lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::TakeoffNoThrottle") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); // wait for takeoff to finish for (int i = 0; i < 12; i++) { msg = GetDefaultPoseMsg(); msg.pos[2] = 0; msg.vel[0] = 20; lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); usleep(100000); } EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::Climb") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); // reach large altitude msg = GetDefaultPoseMsg(); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::ExecuteTrajectory") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName(); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 0); // go to altitude msg = GetDefaultPoseMsg(); msg.pos[2] = altitude; lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); // now add an obstacle in front of it and make sure it transitions! ProcessAllLcmMessages(fsm_control); ProcessAllLcmMessages(fsm_control); float point[3] = { 24, 0, altitude }; SendStereoPointTriple(point); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 2); // from matlab, with 5.0 = safe // NOTE: modification of x_points_ if you're checking in MATLAB (!) vector<float> xpts = x_points_; vector<float> zpts = z_points_; for (int i = 0; i < int(x_points_.size()); i++) { xpts[i] += 5; zpts[i] += altitude; } SendStereoManyPointsTriple(xpts, y_points_, zpts); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 3); // from matlab delete fsm_control; UnsubscribeLcmChannels(); }
/** * Test FindFarthestTrajectory on: * - no obstacles * - one obstacle * - two obstacles */ TEST_F(TrajectoryLibraryTest, FindFarthestTrajectory) { StereoOctomap octomap(bot_frames_); TrajectoryLibrary lib(0); lib.LoadLibrary("trajtest/simple", true); EXPECT_EQ_ARM(lib.GetNumberTrajectories(), 2); BotTrans trans; bot_trans_set_identity(&trans); double altitude = 30; trans.trans_vec[2] = altitude; // flying high up // check that things work with no obstacles (should return first trajectory) double dist; const Trajectory *best_traj; std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0); ASSERT_TRUE(best_traj != nullptr); EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 0); // check for preferred trajectory std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0, nullptr, 1); ASSERT_TRUE(best_traj != nullptr); EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1); // preferred trajectory error std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0, nullptr, 2); ASSERT_TRUE(best_traj != nullptr); EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 0); // add an obstacle close to the first trajectory double point[3] = { 1.05, 0, 0 }; AddPointToOctree(&octomap, point, altitude); // now we expect to get the second trajectory std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0); ASSERT_TRUE(best_traj != nullptr); EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1); EXPECT_NEAR(dist, 2.0 - 1.05, TOLERANCE); // add another point close to the second trajectory point[0] = 2.01; point[1] = 0.01; point[2] = -0.015; AddPointToOctree(&octomap, point, altitude); // now we expect to get the first trajectory std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0); ASSERT_TRUE(best_traj != nullptr); EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 0); EXPECT_NEAR(dist, 0.05, TOLERANCE); }