This is the blog of the P4-RM, where I programmed the behavior of a taxi based on Gradient Path Planning.
Objetive
The objective of this practice was to program the behavior of a taxi using gradient path planning, which generates a cost map and moves following the path of least cost until reaching a target.
Coding
Libraries
- This library provides access to mathematical functions and constants.
import math
- This library provides functions for working in domain of linear algebra, fourier transform, matrices and arrays.
import numpy as np
Implementation
Cost Map
The creation of the cost map consists of three distinct functions: one that creates the simple cost map, another that uses this same cost map to expand the obstacles and penalize the taxi for getting too close to them, and a third function that normalizes the costs.
MAP_URL = '/resources/exercises/global_navigation/images/cityLargenBin.png'
city_map = GUI.getMap(MAP_URL)
def normalize_cost(cost_grid, robot_map, target_map):
...
normalized_grid = np.full_like(cost_grid, np.inf)
normalized_grid[finite_mask] = (cost_grid[finite_mask] / max_cost) * 255
return normalized_grid
def show_cost(target_map, robot_map, city_map):
cost_grid = np.full(city_map.shape, np.inf)
cost_grid[target_map[0], target_map[1]] = 0
queue = [(target_map[0], target_map[1])]
...
# Normalize
normalized_grid = normalize_cost(cost_grid, robot_map, target_map)
return normalized_grid
def expand_obstacle(cost_grid, city_map, cushion_size=3, cushion_cost=255):
...
return expanded_grid
Navigation
The car navigates based on the gradient, it obtains the cost map and analyzes its 8 neighboring cells. These neighbors can range from unreachable (not expanded or obstacles) to cells with varying costs. Among the latter, the taxi will move toward the cell with the lowest cost. This makes the navigation semi-reactive.
def navigate_to_target(cost_grid, robot_map, target_map):
...
while robot_map != target_map:
# Neighbors of the robot
neighbors = [(robot_map[0] - 1, robot_map[1]), (robot_map[0] + 1, robot_map[1]),
(robot_map[0], robot_map[1] - 1), (robot_map[0], robot_map[1] + 1),
(robot_map[0] - 1, robot_map[1] - 1), (robot_map[0] + 1, robot_map[1] + 1),
(robot_map[0] - 1, robot_map[1] + 1), (robot_map[0] + 1, robot_map[1] - 1)]
...
next_step = min(valid_neighbors, key=lambda x: cost_grid[x[0], x[1]])
...
world_point = GUI.gridToWorld((next_step[1], next_step[0]))
while True:
...
target_x, target_y = world_point
direction = np.arctan2(target_y - robot_y, target_x - robot_x)
angle_diff = direction - robot_theta
angle_diff = np.arctan2(np.sin(angle_diff), np.cos(angle_diff))
if abs(angle_diff) > 0.1:
HAL.setW(angle_diff * 2)
HAL.setV(0)
else:
HAL.setW(0)
HAL.setV(5)
...
robot_map = next_step
Difficulties
- One of the main problems I encountered was during navigation. There were times when two adjacent cells had the same cost or were surrounded by higher-cost cells, which caused the robot to get stuck in a constant loop. I solved this by prohibiting the robot from returning to a previous cell.
- Another problem I encountered was that when converting from world to grid, the x and y coordinates were inverted, as x referred to the columns and y to the rows.
Logbook
- Day 1: Test the world and implement conversions from world coordinates to grid.
- Day 2: Create a simple cost map.
- Day 3: Enhance the cost map by expanding obstacles.
- Day 4: Normalize the cost map and finalize its generation when it reaches the taxi.
- Day 5: Implement navigation.
Functionality
Simple test
(In case the video doesn´t play try this link: https://youtu.be/wn9l0Gxg-0M)
Test long ride
I placed the first target in the bottom-left corner and the second one in the top-right corner to observe the taxi's behavior during a very long ride.
(In case the video doesn´t play try this link: https://youtu.be/45cWqGK-cH4)
Changing target in the middle of the ride
The taxi reaches the first target before changing to the next one.
(In case the video doesn´t play try this link: https://youtu.be/hGz6yQENSe0)
Top comments (0)