Path Planning

Overview

Most AirGen maps come with precomputed occupany maps that allow for collision-avoidant path planning. AirGen offers API for computing paths between start and goal points, and the ability to navigate smoothly through a computed safe path. Depending on whether the main robot is aerial or wheeled, AirGen switches between a 3D octree-based occupancy grid, or a 2D navmesh for path planning. Hence, for drones, AirGen computes a 3D path, and for wheeled robots, a path on the ground, such that the path avoids obstacles.

Path Planning between two points: The simPlanPath function computes a collision-free path between two points.
```
client = airgen.MultirotorClient()
start = airgen.Vector3r(0, 0, 5)
goal = airgen.Vector3r(10, 10, 5)
path = simPlanPath(start, goal, smooth_path=True, draw_path=True)
```
Setting smooth_path to True yields a smooth spline trajectory instead of a coarse A*-like path, and draw_path can be used to visualize the trajectory.

If either the start or the goal point lies within an obstacle/occupied area, simPlanPath will fail, returning just the start/goal points back.
Path Planning to a Random Free Point: Another functionality offered by AirGen allows for randomly sampling a free/unoccupied point and planning a path to it, which results in behaviors such as random safe exploration. This can be achieved through the simPlanPathToRandomFreePoint() function, which takes in a search radius as the main argument.
```
client = airgen.MultirotorClient()
search_radius = 50 # in meters
path = simPlanPathToRandomFreePoint(search_radius, smooth_path=True, draw_path=True)
```

Warning

When visualizing a path through draw_path=True, the drawing of the path also makes it appear in the FPV camera images of the robot. If you're capturing onboard camera images from the robot, you might want to set draw_path=False

After a path is computed, the drone can be asked to fly through it using the moveOnPathAsync() function, such as below.

start = airgen.Vector3r(0, 0, 5)
goal = airgen.Vector3r(10, 10, 5)
path = simPlanPath(start, goal, smooth_path=True, draw_path=True)
points = []
for waypoint in trajectory:
    points.append(
        airgen.Vector3r(waypoint["x_val"], waypoint["y_val"], waypoint["z_val"])
    )

# Move the drone along the planned path at a velocity of 5 m/s
velocity = 5.0
self.drone_client.moveOnPathAsync(points, velocity, 120, airgen.DrivetrainType.ForwardOnly, airgen.YawMode(False, 0), -1, 0).join()

Warning

The path tracking/control functionality currently is only available for drones, and is in the works for the wheeled vehicles.

Navigation Extents: AirGen uses a "navigation mesh" to inform which areas of the map are navigable. It is possible to access the extents of this navigation mesh to sample points from, or to understand which parts of the map are accessible through the planning API.
nav_mesh_info = client.getNavMeshInfo() print("Navmesh extents: {}".format(nav_mesh_info))

Tips

Consider visualizing paths before executing.
Remember, smooth paths might deviate slightly compared to direct paths.