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