SST Planner Produces Insufficient Sampling in Kinematic Car Problem Despite Valid Path Existence #1237

ob::RealVectorBounds bounds(2);
bounds.setLow(0, 0);
bounds.setLow(1, 0);
bounds.setHigh(0, bmap.getOccupancyGrid()[0].size() * bmap.getResolution());
bounds.setHigh(1, bmap.getOccupancyGrid().size() * bmap.getResolution());
space->setBounds(bounds);

auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
ob::RealVectorBounds cbounds(2);
cbounds.setLow(0, -20.0); 
cbounds.setHigh(0, 20.0); 
cbounds.setLow(1, -20.6); 
cbounds.setHigh(1, 20.6);
cspace->setBounds(cbounds);

auto space_info = std::make_shared<oc::SpaceInformation>(space, cspace);
space_info->setStateValidityChecker([bmap](const ob::State *state){ return isStateValid(state, bmap); });
space_info->getStateSpace()->setLongestValidSegmentFraction((double)1/(double)bmap.getOccupancyGrid().size());
double carLength = 1.0; // Aracın dingil mesafesi
auto propagator = std::make_shared<KinematicCarPropagator>(space_info, carLength);

oc::SimpleSetup ss(space_info); 

auto rrtMBPI = std::make_shared<oc::SST>(ss.getSpaceInformation());
rrtMBPI->setName("MBPI");
auto rrtUniform = std::make_shared<oc::SST>(ss.getSpaceInformation());
rrtUniform->setName("Uniform");

space_info->setStatePropagator(propagator);
space_info->setPropagationStepSize(0.05);
space_info->setMinMaxControlDuration(1, 1);
space_info->setup();

rrtMBPI->setSelectionRadius(0.2);
rrtMBPI->setPruningRadius(0.1); 
rrtUniform->setSelectionRadius(0.002);
rrtUniform->setPruningRadius(0.01);

ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(start_point.first);
start->setY(start_point.second);
start->setYaw(M_PI/2);

ob::ScopedState<ob::SE2StateSpace> goal(space);
goal->setX(goal_point.first);
goal->setY(goal_point.second);
goal->setYaw(M_PI/2);

ss.setStartAndGoalStates(start, goal, 0.1);

// Benchmark setup
ompl::tools::Benchmark benchmark(ss, "NarrowPassage");

std::shared_ptr<ob::MBPIValidStateSampler> mbpiSampler = std::make_shared<ob::MBPIValidStateSampler>(
    ss.getSpaceInformation().get(), 
    bmap.occupancyGrid, 
    bmap.resolution, 
    bmap.origin
);
benchmark.addPlanner(rrtMBPI);
benchmark.addPlanner(rrtUniform);

benchmark.setPlannerSwitchEvent([mbpiSampler, ss](const ob::PlannerPtr &planner) {
    if (planner->getName() == "MBPI") {
        ss.getSpaceInformation()->setValidStateSamplerAllocator([mbpiSampler](const ob::SpaceInformation* si) {
            return mbpiSampler;
        });
        ss.getSpaceInformation()->setup();
    }
});

benchmark.setPostRunEvent([&](const ompl::base::PlannerPtr &planner, ompl::tools::Benchmark::RunProperties &runProperties)
{
    std::string plannerName = planner->getName();
    std::cout << "Processing results for planner: " << plannerName << std::endl;
    
    ob::PlannerData planner_data(planner->getSpaceInformation());
    planner->getPlannerData(planner_data);
  
    if (planner->getProblemDefinition()->hasSolution())
    {
        auto path = planner->getProblemDefinition()->getSolutionPath()->as<oc::PathControl>();
        if (path) {
            std::cout << "Path size: " << path->getStateCount() << std::endl;
            sf::Color pathColor = (plannerName == "MBPI") ? sf::Color::Green : sf::Color::Blue;
            
            for (size_t i = 1; i < path->getStateCount(); ++i)
            {
                auto* state = path->getState(i)->as<ob::SE2StateSpace::StateType>();
                unsigned int mx, my;
                worldToMap(bmap, state->getX(), state->getY(), mx, my);

                auto* state_prev = path->getState(i-1)->as<ob::SE2StateSpace::StateType>();
                unsigned int mx_prev, my_prev;
                worldToMap(bmap, state_prev->getX(), state_prev->getY(), mx_prev, my_prev);
                
                Renderer::instance().drawMatch({mx_prev, my_prev}, {mx, my}, pathColor);
            }
        }
    }

    std::cout << "Planner data size: " << planner_data.numVertices() << std::endl;
    sf::Color sampleColor = (plannerName == "MBPI") ? sf::Color::Yellow : sf::Color::Magenta;
    
    for (unsigned int i = 0; i < planner_data.numVertices(); ++i) {
        const auto* state = planner_data.getVertex(i).getState()->as<ob::SE2StateSpace::StateType>();
        unsigned int mx, my;
        worldToMap(bmap, state->getX(), state->getY(), mx, my);
        Renderer::instance().drawPoints({{(int)mx, (int)my}}, sampleColor);
    }
});

ompl::tools::Benchmark::Request req;
req.saveConsoleOutput = false;
req.simplify = true;
req.maxTime = 5.0; 
req.runCount = 1;
req.displayProgress = true;

benchmark.benchmark(req);
benchmark.saveResultsToFile();

unsigned int mgx, mgy;
worldToMap(bmap, goal_point.first, goal_point.second, mgx, mgy);

unsigned int msx, msy;
worldToMap(bmap, start_point.first, start_point.second, msx, msy);

Renderer::instance().drawPoints({{(int)mgx, (int)mgy}}, sf::Color::Blue);
Renderer::instance().drawPoints({{(int)msx, (int)msy}}, sf::Color::Blue );