Exemplos de WavefrontMapAccessor::getCost em C++ (Cpp)

Exemplo n.º 1

Arquivo: utest.cpp Projeto: rsbGroup1/frobo_rsd

void TrajectoryPlannerTest::footprintObstacles()
{
    //place an obstacle
    map_->operator () (4, 6).target_dist = 1;
    wa->synchronize();
    EXPECT_EQ (wa->getCost (4, 6), costmap_2d::LETHAL_OBSTACLE);
    Trajectory traj (0, 0, 0, 0.1, 30);
    //tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj, 2, 30);
    tc.generateTrajectory (4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj);
    //we expect this path to hit the obstacle
    EXPECT_FLOAT_EQ (traj.cost_, -1.0);

    //place a wall next to the footprint of the robot
    tc.path_map_ (7, 1).target_dist = 1;
    tc.path_map_ (7, 3).target_dist = 1;
    tc.path_map_ (7, 4).target_dist = 1;
    tc.path_map_ (7, 5).target_dist = 1;
    tc.path_map_ (7, 6).target_dist = 1;
    tc.path_map_ (7, 7).target_dist = 1;
    wa->synchronize();

    //try to rotate into it
    //tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj, 2, 30);
    tc.generateTrajectory (4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj);
    //we expect this path to hit the obstacle
    EXPECT_FLOAT_EQ (traj.cost_, -1.0);
}