[ros-users] SBPL plan and navigate segmentation faults

[Nitin Dhiman]

Greetings,
I am trying to use Lattice generation code of SBPL for D* Lite
implementation. First iteration of algorithm do output result, but as some
change in costmap is observed, incorporation of it in environment lattice
results in segmentation fault. This also happen with test case provided with
sbpl package. Using following statements failure case can be regenerated.

Following is the console statement:
n at tangri:/opt/ros/cturtle/stacks/motion_planners/sbpl$ rosrun sbpl test_sbpl
/opt/ros/cturtle/stacks/motion_planners/sbpl/env_examples/nav3d/env2.cfg
/opt/ros/cturtle/stacks/motion_planners/sbpl/matlab/mprim/pr2.mprim
/opt/ros/cturtle/ros/bin/rosrun: line 35:  3472 Segmentation fault
 $exepath "$@"

I am using pr2.mprim motion primitive file, available in matlab/mprim
directory in sbpl package. The environment configuration file is env2.cfg
 with following modification on cellsize, start and goal pose:

cellsize(meters): 0.025
nominalvel(mpersecs): 1.0
timetoturn45degsinplace(secs): 1.0
start(meters,rads): 0.25 0.25 14
end(meters,rads): 1 0.25 14
This is done to make environment consistent with motion primitive file.

Following are the change to main.cpp  : I have modified size of robot

line number 893:  double halfwidth = 0.025; //0.3;
line number 894: double halflength = 0.025; //0.45;

I am using ADPlanner to test because of similarity with D* lite.
line number 987: ADPlanner planner(&environment_navxythetalat,
bforwardsearch);

int main(int argc, char *argv[])
{
#ifdef ROS
  ros::init(argc, argv, "sbpl_test");
#endif

if(argc < 2)
{
PrintUsage(argv);
 throw new SBPL_Exception();
}


    //xytheta planning
 //usage: see the comments for planxythetalat() above
    planandnavigatexythetalat(argc, argv);
return 0;
}

One source of error I found out is
in EnvironmentNAVXYTHETALAT::GetPredsofChangedEdges() function. It try to
get an hash entry for translated affected cell (1,-1,2), which IMHO shall
never happen as translated coordinates shall be always positive.
Following are the iteration details: i=0, cell  = {6,6}, sind=102
EnvironmentNAVXYTHETALAT::GetPredsofChangedEdges()  call
EnvironmentNAVXYTHETALAT::GetHashEntry_lookup(1, -1, 2).
GetHashEntry_lookup() computes macro expansion  XYTHETA2INDEX which
obviously results in negetive index.
                                index = 2 + 1*16 + (-1)*100*16
Next execution of statement Coord2StateIDHashTable_lookup[index]  results in
segmentation fault.

One simple solution will be to put a conditional check for translated
affectedCell so that none of coordinates (including theta) are negetive. But
is it intended?

I have seen running example of this library in sbpl_lattice_planner.  May be
I am missing something! Do anyone has faced this problem ?

regards
Nitin
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.osuosl.org/pipermail/ros-users/attachments/20110728/d3d14ed8/attachment-0001.html>