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);
}
//make sure that trajectories that intersect obstacles are invalidated
TEST(TrajectoryController, footprintObstacles){
//place an obstacle
tc->map_(4, 6).occ_state = 1;
wa.synchronize();
Trajectory traj(0, 0, 0, 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->map_(7, 1).occ_state = 1;
tc->map_(7, 3).occ_state = 1;
tc->map_(7, 4).occ_state = 1;
tc->map_(7, 5).occ_state = 1;
tc->map_(7, 6).occ_state = 1;
tc->map_(7, 7).occ_state = 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);
}
void TrajectoryPlannerTest::checkGoalDistance()
{
//let's box a cell in and make sure that its distance gets set to max
map_->operator () (1, 2).target_dist = 1;
map_->operator () (1, 1).target_dist = 1;
map_->operator () (1, 0).target_dist = 1;
map_->operator () (2, 0).target_dist = 1;
map_->operator () (3, 0).target_dist = 1;
map_->operator () (3, 1).target_dist = 1;
map_->operator () (3, 2).target_dist = 1;
map_->operator () (2, 2).target_dist = 1;
wa->synchronize();
//set a goal
tc.path_map_.resetPathDist();
queue<MapCell*> target_dist_queue;
MapCell& current = tc.path_map_ (4, 9);
current.target_dist = 0.0;
current.target_mark = true;
target_dist_queue.push (¤t);
tc.path_map_.computeTargetDistance (target_dist_queue, tc.costmap_);
EXPECT_FLOAT_EQ (tc.path_map_ (4, 8).target_dist, 1.0);
EXPECT_FLOAT_EQ (tc.path_map_ (4, 7).target_dist, 2.0);
EXPECT_FLOAT_EQ (tc.path_map_ (4, 6).target_dist, 100.0); //there's an obstacle here placed above
EXPECT_FLOAT_EQ (tc.path_map_ (4, 5).target_dist, 6.0);
EXPECT_FLOAT_EQ (tc.path_map_ (4, 4).target_dist, 7.0);
EXPECT_FLOAT_EQ (tc.path_map_ (4, 3).target_dist, 8.0);
EXPECT_FLOAT_EQ (tc.path_map_ (4, 2).target_dist, 9.0);
EXPECT_FLOAT_EQ (tc.path_map_ (4, 1).target_dist, 10.0);
EXPECT_FLOAT_EQ (tc.path_map_ (4, 0).target_dist, 11.0);
EXPECT_FLOAT_EQ (tc.path_map_ (5, 8).target_dist, 2.0);
EXPECT_FLOAT_EQ (tc.path_map_ (9, 4).target_dist, 10.0);
//check the boxed in cell
EXPECT_FLOAT_EQ (100.0, tc.path_map_ (2, 2).target_dist);
}
//make sure that goal distance is being computed as expected
TEST(TrajectoryController, checkGoalDistance){
//let's box a cell in and make sure that its distance gets set to max
tc->map_(1, 2).occ_state = 1;
tc->map_(1, 1).occ_state = 1;
tc->map_(1, 0).occ_state = 1;
tc->map_(2, 0).occ_state = 1;
tc->map_(3, 0).occ_state = 1;
tc->map_(3, 1).occ_state = 1;
tc->map_(3, 2).occ_state = 1;
tc->map_(2, 2).occ_state = 1;
wa.synchronize();
//set a goal
tc->map_.resetPathDist();
queue<MapCell*> goal_dist_queue;
MapCell& current = tc->map_(4, 9);
current.goal_dist = 0.0;
current.goal_mark = true;
goal_dist_queue.push(¤t);
tc->computeGoalDistance(goal_dist_queue);
EXPECT_FLOAT_EQ(tc->map_(4, 8).goal_dist, 1.0);
EXPECT_FLOAT_EQ(tc->map_(4, 7).goal_dist, 2.0);
EXPECT_FLOAT_EQ(tc->map_(4, 6).goal_dist, 100.0); //there's an obstacle here placed above
EXPECT_FLOAT_EQ(tc->map_(4, 5).goal_dist, 6.0);
EXPECT_FLOAT_EQ(tc->map_(4, 4).goal_dist, 7.0);
EXPECT_FLOAT_EQ(tc->map_(4, 3).goal_dist, 8.0);
EXPECT_FLOAT_EQ(tc->map_(4, 2).goal_dist, 9.0);
EXPECT_FLOAT_EQ(tc->map_(4, 1).goal_dist, 10.0);
EXPECT_FLOAT_EQ(tc->map_(4, 0).goal_dist, 11.0);
EXPECT_FLOAT_EQ(tc->map_(5, 8).goal_dist, 2.0);
EXPECT_FLOAT_EQ(tc->map_(9, 4).goal_dist, 10.0);
//check the boxed in cell
EXPECT_FLOAT_EQ(tc->map_(2, 2).goal_dist, 100.0);
}
