The main task is to implement a simple grid-based planning approach. You will learn how to plan on the grid such that the robot embodiment is considered.
Due date | October 20, 2024, 23:59 PST |
Deadline | January 11, 2025, 23:59 PST |
Points | 3 |
Label in BRUTE | t1c-plan |
Files to submit | archive with HexapodExplorer.py file |
Resources | B4M36UIR_t1_resource_pack |
In class HexapodExplorer.py
implement the following basic steps of the grid-based navigation pipeline:
grow_obstacles
) to take into account the robot embodiment
plan_path
) to find a path between the start and goal position (considering planning with 8-neighborhood)
simplify_path
) which purpose is to provide a smoother path for the robot
The detailed description and guidelines on individual steps follow.
The purpose of the grow_obstacles
function is to pre-process the grid map into a binary map by rejecting obstacles, unknown and inaccessible areas for the robot given its embodiment. The robot is considered to be a circle with $r_\mathrm{robot}$ radius robot_size
. The result of the obstacle growing is a binary map with the used binary values 1 (for occupied, inaccessible and unknown cells) and 0 (for traversable cells).
For each grid_map
cell $m_i$ the new binary value $b_i$ of the grid_map_grow
is based on its Bayesian occupancy probability $P(m_i = occupied \vert z) = 0.5$ and distance to the nearby obstacles according to the following rules:
The input parameters of the function are:
grid_map
- OccupancyGrid message - grid map representationrobot_size
- the robot radius in metresThe function returns:
The grow_obstacles
function in the HexapodExplorer.py
class has a following prescription:
def grow_obstacles(self, grid_map, robot_size): """ Method to grow the obstacles to take into account the robot embodiment Args: grid_map: OccupancyGrid - gridmap for obstacle growing robot_size: float - size of the robot Returns: grid_map_grow: OccupancyGrid - gridmap with considered robot body embodiment """Example obstacle growing for first evaluation scenario is depicted in following figure. From left to right:
Suggested approach (click to view)
The purpose of the plan_path
function is to plan the path between the start and goal poses on the pre-processed grid map considering planning with 8-neighborhood. The function returns a path on the output. If the path is not found, the planner returns None
.
The input parameters of the function are:
grid_map
- OccupancyGrid message - pre-processed grid map representation (contains 1 and 0 only)start
- Pose message - start robot posegoal
- Pose message - goal robot poseThe function returns:
None
when the path cannot be found. It is supposed that the path contains only 2D coordinates and not the heading, i.e., only position.x
and position.y
entries of individual poses in the path are filled-in. The path is returned in the world coordinates!
The plan_path
function in the HexapodExplorer.py
class has a following prescription:
def plan_path(self, grid_map, start, goal): """ Method to plan the path from start to the goal pose on the grid Args: grid_map: OccupancyGrid - gridmap for obstacle growing start: Pose - robot start pose goal: Pose - robot goal pose Returns: path: Path - path between the start and goal Pose on the map """An example of correctly planned path with A* algorithm for different sizes of of robot is visualized in following figure. From left to right, the planning for:
Suggested approach (click to view)
Path simplification is usually done by excluding navigation points from the path, that are not necessary in the sense that the robot does not have to visit them precisely. A typical approach to trajectory smoothing is to connect the neighboring segments one by one using straight-line segments (using Bresenham line algorithm) up to the point where the straight-line segment collide with an obstacle (grown obstacle) and then follow with another straight-line segment.
The input parameters of the function are:
grid_map
- OccupancyGrid message - pre-processed grid map representation (contains 1 and 0 only)path
- Path message - path to be simplifiedThe function returns:
The simplify_path
function in the HexapodExplorer.py
class has a following prescription:
def simplify_path(self, grid_map, path): """ Method to simplify the found path on the grid Args: grid_map: OccupancyGrid - gridmap for obstacle growing path: Path - path to be simplified Returns: path_simple: Path - simplified path """The result of path simplification may vary, but an exmple of correctly planned and simplified path using the suggested approach is visualized in following figure. From left to right, the planning and simplification for:
Suggested approach (click to view)
The evaluation focus on correct behavior of the above described robot navigation pipeline.
#!/usr/bin/env python3 # -*- coding: utf-8 -*- import matplotlib.pyplot as plt import sys import os import math import numpy as np sys.path.append('hexapod_robot') sys.path.append('hexapod_explorer') #import hexapod robot and explorer import HexapodRobot as hexapod import HexapodExplorer as explorer #import communication messages from messages import * #pickle import pickle ROBOT_SIZE = 0.3 if __name__=="__main__": #instantiate the explorer robot explor = explorer.HexapodExplorer() #load scenarios scenarios = pickle.load( open( "scenarios.bag", "rb" ) ) #evaluate individual scenarios for gridmap, start, goal in scenarios: #obstacle growing gridmap_processed = explor.grow_obstacles(gridmap, ROBOT_SIZE) #path planning path = explor.plan_path(gridmap_processed, start, goal) #path simplification path_simple = explor.simplify_path(gridmap_processed, path) #plot the result def plot_path(path, ax, clr): """ simple method to draw the path """ if path is not None: poses = np.asarray([(pose.position.x,pose.position.y) for pose in path.poses]) ax.plot(poses[:,0], poses[:,1], '.',color=clr) ax.plot(poses[:,0], poses[:,1], '-',color=clr) fig, ax = plt.subplots() gridmap.plot(ax) plot_path(path, ax, 'r') plot_path(path_simple, ax, 'b') plt.xlabel('x[m]') plt.ylabel('y[m]') plt.axis('square') plt.show()
int()
conversion is used to transform to the grid coordinates. Since int()
behaves similarly to floor()
, in the inverse transform to the world coordinates you need to offset the map coordinates by half a cell before multiplying by the grid resolution.