Is it possibile to generate a path between a startLocation and a goalLocation without generate other nodes?

Hey,

I'm using mobileRobotPRM to find a path between a start and a goal positions. What i'de like to do is to check if the path made by the only start and goal locations is free of obstacles. How can i set the "findpath" function in such a way to do that?

[Remo Pillat]

Hi Simone, just to make sure I understand you correctly, do you just want to verify if the straight line connecting startLocation and goalLocation is obstacle free?

[simone esposito]

Yes, i'd like to check this in a binary occupancy map. Moreover i'd like to generate a path with only those two nodes in such a way to have the variable "path" generate by the mobileRobotPRM empty; anyway the second point is not mandatory.

Answer 1

[Cameron Stabile]

Hi Simone,

I'm not 100% certain this is what you are after, but it sounds like you are trying to perform an efficient pre-check to see whether PRM's full graph-construction and path-planning can be avoided if there exists a direct "line-of-sight" path between start and goal.

Assuming this is correct, and that your robot is either a point-mass or you've already inflated your map, you might be able to leverage the occupancy map's rayIntersection method:

% Load an example map
load exampleMaps

% Construct map
map = occupancyMap(ternaryMap);

% Define robot radius and inflate map
rbtRad = 2;
inflate(map,rbtRad);
show(map);
hold on;

% Define a start/goal pair with free line-of-sight path
start = [100 150];
goal  = [300 400];

% Search for path
path1 = customFindPath(map,start,goal);

% Find path when direct path is occluded
goal  = [350 300];
path2 = customFindPath(map,start,goal);
legend({'LOSFreePath','LOSOccludedPath','PRMPath'});

function path = customFindPath(map,start,goal)
    
    persistent prm
    
    % Check if ray intersects with occupied cell
    delta = goal-start;
    theta = atan2(delta(2),delta(1));
    rayPose = [start theta];
    angle = 0;
    range = norm(start-goal);
    collisionPt = rayIntersection(map,rayPose,angle,range);
    path = [start; goal];
    
    if all(isnan(collisionPt))
        % If no collision point was found, the path between start/goal is free
        disp('Line-of-sight free');
        plot(path(:,1),path(:,2),'g');
    else
        % Otherwise a path must be planned
        disp('Line-of-sight failed, planning path');
        plot(path(:,1),path(:,2),'r');
        if isempty(prm)
            prm = mobileRobotPRM(map);
        end
        path = findpath(prm, start, goal);
        plot(path(:,1),path(:,2),'b');
    end
end

This approach will only get you so far, as it requires certain assumptions on the geometry and motion capabilities of your robot, but hopefully this gets your foot in the door.

If you find that you need more sophisticated planners down the road, you might find plannerRRT or plannerHybridAStar useful as well.

Best,

Cameron