Graph traversal algorithm using shapely geometry points

Question

I am trying to create a path planning algorithm for a robotic mower to cut all the grass in a defined area while avoiding obstacles or restricted areas. I am using the Python Shapely library to define the geometry of the area. In the following image, the mowing area is marked blue and the restricted area is marked red.

My current approach is to generate parallel lines at regular intervals between the bounds of the area, then cut them into segments wherever they intersect the obstacle or map boundary. Here is an image to illustrate that.

Once I have my line segments, I want to use turn each point on the map from the area, obstacle, and intersections into nodes on a graph. The graph should faithfully represent each point's adjacency to the others. Afterward, I plan to make a traversal algorithm that visits each node at least once.

I want to know if there is a succinct way to convert geometric features from Shapely into a connected graph for my purposes.

Here is the relevant code for generating the geometry for the mowing area and intersecting lines:

import numpy as np
from shapely.geometry import Point, Polygon, LineString, MultiLineString, GeometryCollection

def generate_intersections(areaCoords, obstacleCoords, spacing = 1.0):
    # Create Shapely polygons
    area = Polygon(areaCoords)
    obstacle = Polygon(obstacleCoords)
    map = area.difference(obstacle)

    # Obtain bounding box
    minX, minY, maxX, maxY = area.bounds
    
    # Generate parallel lines within bounding box
    lines = []
    y = minY
    while y <= maxY:
        lines.append(LineString( [(minX,y),(maxX,y)] ))
        y += spacing
    
    # Cut lines where they intersect with Boundaries/Obstacle,
    intersections = []

    for line in lines:
        parallel = []
        intersection = line.intersection(map)

        if intersection.is_empty:
            continue
        
        # Line intersects with boundaries only
        if isinstance(intersection, LineString):
            coords = list(intersection.xy)
            parallel.append(coords)

        # Line intersects with obstacle and boundaries
        elif isinstance(intersection, (MultiLineString, GeometryCollection)):
            for g in list(intersection.geoms):
                parallel.append(list(g.xy))
        
        intersections.append(parallel)

    return intersections

if __name__ == "__main__":
    # Polygon Coordinates
    areaCoords = [(2,0),(14,0),(14,12),(2,12)]
    obstacleCoords = [(4,5),(9,5),(9,9),(4,9)]

    # Get intersecting points
    intersections = generate_intersections(areaCoords,obstacleCoords,2.0)

Does this point you in a useful direction? networkx.org/documentation/stable/auto_examples/geospatial/… — Vin
– Vin, Commented Feb 6 at 2:04
Are all the obstacles polygons with strictly vertical or horizontal edges? — ravenspoint
– ravenspoint, Commented Feb 6 at 17:04
@ravenspoint At some point, I will need to be able to operate on polygonal areas of any shape. The orange edges will always be horizontal. — Tomasm64
– Tomasm64, Commented Feb 7 at 16:45
Polygons with edges that are not ||el to one or other axis will be harder to deal with. The algorithm I proposed will not change, but checking the 3 tests will become more complex. — ravenspoint
– ravenspoint, Commented Feb 7 at 16:51

ravenspoint · Accepted Answer · 2025-02-09 20:49:13Z

Assuming that the mowing area ( blue lines ) and the obstacles ( red lines ) are polygons with all edges strictly vertical or horizontal, as shown in you example:

There are three ways a pair of points can be connected ( i.e. the graph will have an edge between them ).

Both points are on the edge of the mowing area polygon and have the same y co-ordinate, , and the line segment between them does not intersect an obstacle.
Both points are on the edge of the defined area polygon and have the same x co-ordinate and a y co-ordinates that differ by the spacing between the generated lines ( orange )
One point is on the edge of the mowing area and the other is on the edge of an obstacle, they both have the same y co-ordinate, and the line segment between them does not intersect an obstacle.

So the algorithm looks like this:

 - LOOP P1 over all points
   - LOOP P2 over all points > P1
      - If P1,P2 satisfy any one of the 3 ways described above
            - Add edge between P1 and P2.

That looks fairly simple. However implementing the tests on each pair of points is complex. So, to start things off, I suggest drastically simplifying the testing problems by assuming

There is a single obstacle
Polygons ( the lawn and the obstacle ) are specified by x,y co-ordinates listed in clockwise order starting from the lower left corner, closed ( first and last point is repeated )
Polygons are 4 sided with all edges parallel to one of the axis.

Note that the example in the question meets these assumptions.

I have written C++ code to solve the problem with these assumptions.

Inputting the example in the question gives these lines

And connecting the pairs of points that can be traversed by the mowing machine gives

Now we need to find the zigzag path that follows the graph connections and covers all of the lawn. I wrote this code to do so:

void cLawn::ZigZag()
{
    vPath.clear();
    int vi = 0;
    cxy sxy = locate( 0 );
    double xs = sxy.x;
    double ys = sxy.y;
    vPath.emplace_back(xs, ys);
    bool fdown = true;
    bool zig = true;
    while (true)
    {
        bool foundnext = false;
        for (int adj : gd.g.adjacentOut(vi))
        {
            cxy txy = locate( adj );
            double xt = txy.x;
            double yt = txy.y;
            if (zig)
            {
                if (ys == yt)
                    foundnext = true;
            }
            else
            {
                if (xs == xt)
                {
                    if (fdown)
                    {
                        if (yt > ys)
                            foundnext = true;
                    }
                    else
                    {
                        if (yt < ys)
                            foundnext = true;
                    }
                }
            }

            if (foundnext)
            {
                auto it = std::find( vPath.begin(),vPath.end(),txy);
                if( it != vPath.end()  )
                    return;
                vPath.emplace_back(xt, yt);
                vi = adj;
                xs = xt;
                ys = yt;
                break;
            }
        }
        if (!foundnext)
        {
            if (fdown)
            {
                fdown = false;
                zig = true;
            }
            else
            {
                return;
            }
        }

        zig = !zig;
    }
}

which gives this display, where the graph vertices are numbered in the order they are visited by the mowing machine

The complete code for this is at https://codeberg.org/JamesBremner/mower

Next Steps

You might want to port this code to python. ( I do not recommend doing so, since python is painfully slow for this kind of work )

In any case, the code needs to be further developed so that it can handle more complex problems ( e.g. multiple obstacles, polygons with more sides ) IMHO should be done step by step, first dropping the assumptions that are most unlike the real problems you encounter.

Collectives™ on Stack Overflow

Graph traversal algorithm using shapely geometry points

1 Answer 1

Comments

Your Answer

Hot Network Questions

Collectives™ on Stack Overflow

1 Answer 1

Comments

Your Answer

Sign up or log in

Post as a guest

Related