Segmentation Fault on trying to access the vertex state of SPARS

[JACOBIN-SCTCS]

My system configuration is Ubuntu 20.04. I am attaching the program 'state_sampling.cpp' and the CMakeLists.txt file. I am getting a segmentation fault error while trying to access the vertex state of the roadmap constructed by SPARS, while the same code works without errors on using Probabilistic RoadMap Planners (PRM)

state_sampling.cpp

#include <ompl-1.6/ompl/base/spaces/SE3StateSpace.h>
#include <ompl-1.6/ompl/base/samplers/ObstacleBasedValidStateSampler.h>
#include <ompl-1.6/ompl/base/samplers/GaussianValidStateSampler.h>
#include <ompl-1.6/ompl/base/samplers/MaximizeClearanceValidStateSampler.h>
#include <ompl-1.6/ompl/base/terminationconditions/IterationTerminationCondition.h>


#include <ompl-1.6/ompl/geometric/planners/prm/PRM.h>
#include <ompl-1.6/ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl-1.6/ompl/geometric/planners/prm/PRMstar.h>
#include <ompl-1.6/ompl/geometric/SimpleSetup.h>
#include <ompl-1.6/ompl/base/spaces/DiscreteStateSpace.h>
#include <ompl-1.6/ompl/geometric/PathGeometric.h>
#include <ompl-1.6/ompl/base/PlannerTerminationCondition.h>
#include <boost/graph/adjacency_list.hpp>
#include <ompl-1.6/ompl/geometric/planners/prm/SPARS.h>
#include <ompl-1.6/ompl/geometric/planners/prm/SPARStwo.h>

#include <fstream>
#include <ompl-1.6/ompl/config.h>
#include <iostream>
#include <thread>

namespace ob = ompl::base;
namespace og = ompl::geometric;

std::vector<std::vector<int>> grid;

class myStateValidityCheckerClass : public ob::StateValidityChecker
{
public:
     myStateValidityCheckerClass(const ob::SpaceInformationPtr &si) :
       ob::StateValidityChecker(si)
       {

       }
 
     virtual bool isValid(const ob::State *state) const
     {
          int x  = state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(0)->value;
          int y = state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(1)->value;

          if(grid[x][y] == 1)
            return false;
          
          return true;
     }
};


void plan()
{
    grid.resize(50,std::vector<int>(50,0));
    for(int i=20;i<30;++i)
    {
      for(int j=20;j<30;++j)
      {
        grid[i][j]=1;
      }
    }

    ompl::base::StateSpacePtr dim1(new ompl::base::DiscreteStateSpace(0,49));
    ompl::base::StateSpacePtr dim2(new ompl::base::DiscreteStateSpace(0,49));
    ompl::base::StateSpacePtr space = dim1 + dim2;
    
    auto si(std::make_shared<ob::SpaceInformation>(space));
    si->setStateValidityChecker(std::make_shared<myStateValidityCheckerClass>(si));
   
    ob::ScopedState<ompl::base::CompoundStateSpace> start(space);
    start->as<ompl::base::DiscreteStateSpace::StateType>(0)->value=5;
    start->as<ompl::base::DiscreteStateSpace::StateType>(1)->value=5;


    ob::ScopedState<ompl::base::CompoundStateSpace> goal(space);
    goal->as<ompl::base::DiscreteStateSpace::StateType>(0)->value=45;
    goal->as<ompl::base::DiscreteStateSpace::StateType>(1)->value=49;

    auto pdef(std::make_shared<ob::ProblemDefinition>(si));
    pdef->setStartAndGoalStates(start,goal);

    auto planner(std::make_shared<og::SPARS>(si));

    planner->setProblemDefinition(pdef);
    planner->setup();

    planner->constructRoadmap(ompl::base::IterationTerminationCondition(3500));
    planner->constructRoadmap(ompl::base::IterationTerminationCondition(3500));

    // planner->setConnectionFilter(customConnectionFilter);
    // planner->setConnectionFilter(customConnectionFilter);    

    auto graph = planner->getRoadmap();
    auto num_vertices = boost::num_vertices(graph);
    std::cout <<"Number of vertices = " << num_vertices;
    std::cout << "g = " << graph.m_vertices.size();

    auto state_map = boost::get(og::SPARS::vertex_state_t(),graph);

    std::cout<< "(" << state_map[0]->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(0)->value
      << "," << state_map[0]->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(1)->value <<")"
      ;
      
    std::pair<og::PRM::Graph::edge_iterator, og::PRM::Graph::edge_iterator> edges=  boost::edges(graph);
      
    for(auto s = edges.first ; s!= edges.second;++s)
    {
        std::cout<<"("<<s->m_source<<"," << s->m_target <<")"<< std::endl;
    }
}

int main(int /*argc*/, char ** /*argv*/)
{
    plan();
    return 0;
}

CMakeLists.txt

project(state_sampler)

set(CMAKE_PREFIX_PATH "/home/depressedcoder/eigen-3.4.0")

find_package(Eigen3 REQUIRED NO_MODULE)

include_directories(/usr/local/include/ompl-1.6)
link_directories(/usr/local/lib)


add_executable(state_sampler state_sampling.cpp)
target_link_libraries(state_sampler ompl)
target_link_libraries(state_sampler Eigen3::Eigen)
target_compile_features(state_sampler PUBLIC c_std_99 cxx_std_17)  

Is this an issue due to the library or am I accessing the vertex states in an incorrect fashion.

[zkingston]

Seems like this is an issue with SPARS, where the vertex at index 0 in the sparse roadmap is null. If you check the edges that exist in the roadmap, you'll see that index 0 is never used. I would just throw a check if state_map[index] is null before accessing state data, e.g.,

    auto state_map = boost::get(og::SPARS::vertex_state_t(), graph);

    auto vertices = boost::vertices(graph);
    for (auto s = vertices.first; s != vertices.second; ++s)
    {
        if (not state_map[*s])
            continue;

        std::cout << *s << "=(" << state_map[*s]->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(0)->value
                  << "," << state_map[*s]->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(1)->value
                  << ")" << std::endl;
    }

[JACOBIN-SCTCS]

@zkingston you are right. Thank you. Closing the issue