示例#1
0
TEST_F(TrajectoryLibraryTest, RemainderTrajectory) {
    StereoOctomap octomap(bot_frames_);

    // Load a complicated trajectory
    TrajectoryLibrary lib(0);
    lib.LoadLibrary("trajtest/many", true);

    double altitude = 30;
    BotTrans trans;
    bot_trans_set_identity(&trans);
    trans.trans_vec[2] = altitude;

    const Trajectory *traj = lib.GetTrajectoryByNumber(1);

    double dist;

    double point[3] = { 6.65, -7.23, 9.10 };
    AddPointToOctree(&octomap, point, altitude);
    dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0, 0);
    EXPECT_NEAR(dist, 11.748141101535500, TOLERANCE2); // from matlab

    dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 0.95, 0);
    EXPECT_NEAR(dist, 11.8831, TOLERANCE2);

    dist = traj->ClosestObstacleInRemainderOfTrajectory(octomap, trans, 1.5, 0); // t after trajectory
    EXPECT_NEAR(dist, 12.3832, TOLERANCE2); // from matlab
}
示例#2
0
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);


}
示例#3
0
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);
}
示例#4
0
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);
}
示例#5
0
文件: trans.c 项目: PennPanda/libbot2
void
bot_trans_set_from_velocities(BotTrans *dest, const double angular_rate[3],
    const double velocity[3], double dt)
{
  //see Frazolli notes, Aircraft Stability and Control, lecture 2, page 15
  if (dt == 0) {
    bot_trans_set_identity(dest);
    return;
  }

  double identity_quat[4] = { 1, 0, 0, 0 };
  double norm_angular_rate = bot_vector_magnitude_3d(angular_rate);

  if (norm_angular_rate == 0) {
    double delta[3];
    memcpy(delta, velocity, 3 * sizeof(double));
    bot_vector_scale_3d(delta, dt);
    bot_trans_set_from_quat_trans(dest, identity_quat, delta);
    return;
  }

  //"exponential of a twist: R=exp(skew(omega*t));
  //rescale vel, omega, t so ||omega||=1
  //trans =  (I-R)*(omega \cross v) + (omega \dot v) *omega* t
  //                term2                       term3
  // R*(omega \cross v)
  //     term1
  //rescale
  double t = dt * norm_angular_rate;
  double omega[3], vel[3];
  memcpy(omega, angular_rate, 3 * sizeof(double));
  memcpy(vel, velocity, 3 * sizeof(double));
  bot_vector_scale_3d(omega, 1.0/norm_angular_rate);
  bot_vector_scale_3d(vel, 1.0/norm_angular_rate);

  //compute R (quat in our case)
  bot_angle_axis_to_quat(t, omega, dest->rot_quat);

  //cross and dot products
  double omega_cross_vel[3];
  bot_vector_cross_3d(omega, vel, omega_cross_vel);
  double omega_dot_vel = bot_vector_dot_3d(omega, vel);


  double term1[3];
  double term2[3];
  double term3[3];

  //(I-R)*(omega \cross v) = term2
  memcpy(term1, omega_cross_vel, 3 * sizeof(double));
  bot_quat_rotate(dest->rot_quat, term1);
  bot_vector_subtract_3d(omega_cross_vel, term1, term2);

  //(omega \dot v) *omega* t
  memcpy(term3, omega, 3 * sizeof(double));
  bot_vector_scale_3d(term3, omega_dot_vel * t);

  bot_vector_add_3d(term2, term3, dest->trans_vec);
}
示例#6
0
TEST_F(TrajectoryLibraryTest, TimingTest) {
    StereoOctomap octomap(bot_frames_);

    double altitude = 30;

    // create a random point cloud

    int num_points = 10000;
    int num_lookups = 1000;

    std::uniform_real_distribution<double> uniform_dist(-1000, 1000);
    std::random_device rd;
    std::default_random_engine rand_engine(rd());

    float x[num_points], y[num_points], z[num_points];

    for (int i = 0; i < num_points; i++) {


        // generate a random point
        x[i] = uniform_dist(rand_engine);
        y[i] = uniform_dist(rand_engine);
        z[i] = uniform_dist(rand_engine);

        //std::cout << "Point: (" << this_point[0] << ", " << this_point[1] << ", " << this_point[2] << ")" << std::endl;

    }

    AddManyPointsToOctree(&octomap, x, y, z, num_points, altitude);

    TrajectoryLibrary lib(0);

    lib.LoadLibrary("trajtest/many", true);

    BotTrans trans;
    bot_trans_set_identity(&trans);
    trans.trans_vec[2] = altitude;

    tic();

    for (int i = 0; i < num_lookups; i++) {
        double dist;
        const Trajectory *best_traj;
        std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 50.0);
    }

    double num_sec = toc();

    std::cout << num_lookups <<  " lookups with " << lib.GetNumberTrajectories() << " trajectories on a cloud (" << num_points << ") took: " << num_sec << " sec (" << num_sec / (double)num_lookups*1000.0 << " ms / lookup)" << std::endl;
}
示例#7
0
/**
 * Tests point transformation with a simple two-point trajectory
 */
TEST_F(TrajectoryLibraryTest, GetTransformedPoint) {
    Trajectory traj("trajtest/simple/two-point-00000", true);

    BotTrans trans;
    bot_trans_set_identity(&trans);

    trans.trans_vec[0] = 1;

    double point[3] = {1, 0, 0};

    double output[3];

    traj.GetXyzYawTransformedPoint(0, trans, output);

    //std::cout << "point = (" << point[0] << ", " << point[1] << ", " << point[2] << ")" << std::endl;
    //std::cout << "output = (" << output[0] << ", " << output[1] << ", " << output[2] << ")" << std::endl;

    for (int i = 0; i < 3; i++) {
        EXPECT_NEAR(point[i], output[i], TOLERANCE);
    }

    trans.trans_vec[0] = 0;
    traj.GetXyzYawTransformedPoint(1, trans, output);

    for (int i = 0; i < 3; i++) {
        EXPECT_NEAR(point[i], output[i], TOLERANCE);
    }

    // transform with rotation
    trans.rot_quat[0] = 0.707106781186547;
    trans.rot_quat[1] = 0;
    trans.rot_quat[2] = 0;
    trans.rot_quat[3] = 0.707106781186547;

    traj.GetXyzYawTransformedPoint(1, trans, output);

    point[0] = 0;
    point[1] = 1;
    point[2] = 0;

    //std::cout << "point = (" << point[0] << ", " << point[1] << ", " << point[2] << ")" << std::endl;
    //std::cout << "output = (" << output[0] << ", " << output[1] << ", " << output[2] << ")" << std::endl;

    for (int i = 0; i < 3; i++) {
        EXPECT_NEAR(point[i], output[i], TOLERANCE);
    }

}
示例#8
0
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);

}
示例#9
0
int
bot_ctrans_path_to_trans_latest(const BotCTransPath * path, BotTrans *result)
{
    bot_trans_set_identity(result);
    BotTrans temp_trans;
    for(int lind=0; lind<path->nlinks; lind++) {
        BotCTransLink *link = path->links[lind];
        int have_trans = _link_get_trans_latest(link, &temp_trans);
        if(!have_trans) {
            return 0;
        }
        if(path->invert[lind]) {
            bot_trans_invert(&temp_trans);
        }
        bot_trans_apply_trans(result, &temp_trans);
    }
    return 1;
}
示例#10
0
TEST_F(TrajectoryLibraryTest, Altitude) {
    StereoOctomap octomap(bot_frames_);
    TrajectoryLibrary lib(0);

    lib.LoadLibrary("trajtest/simple", true);

    double altitude = 0.5;
    double point[3] = { 1.05, 0, 0 };
    AddPointToOctree(&octomap, point, altitude);

    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, 2.0);

    EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1);
}
示例#11
0
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);


}
示例#12
0
TEST_F(TrajectoryLibraryTest, TwoTrajectoriesOnePointWithTransform) {

    StereoOctomap octomap(bot_frames_);

    TrajectoryLibrary lib(0);

    lib.LoadLibrary("trajtest/simple", true);
    double altitude = 30;

    double point[3] = { 1.05, 0, 0 };
    AddPointToOctree(&octomap, point, altitude);

    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, 2.0);

    EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1);

    trans.rot_quat[0] = 0.8349;
    trans.rot_quat[1] = 0.3300;
    trans.rot_quat[2] = 0.4236;
    trans.rot_quat[3] = -0.1206;

    // rotation shouldn't matter
    // TODO: this will fail when I introduce aircraft rotation into the check

    std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0);
    EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 1);

    // with a translation, we expect a different result

    trans.trans_vec[0] = 1;

    std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 2.0);
    EXPECT_TRUE(best_traj->GetTrajectoryNumber() == 0);
}
示例#13
0
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);

}
示例#14
0
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);
}
示例#15
0
/**
 * 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);

}