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