Algorithms and implementations for autonomous robot navigation in e-Yantra projects.

INFO

This article assumes ROS 2 Humble is installed. See ROS 2 Basics first.

---

Global vs Local Planners

Type	Purpose	Common Algorithms
Global Planner	Plan full path on a static map	A*, Dijkstra, NavFn
Local Planner	React to dynamic obstacles	DWB, TEB, MPPI

---

Nav2 Setup

# Install Nav2
sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup

# Launch with your robot
ros2 launch nav2_bringup navigation_launch.py \
  use_sim_time:=True \
  map:=/path/to/map.yaml

---

A* Implementation

import heapq

def astar(grid, start, goal):
    open_set = []
    heapq.heappush(open_set, (0, start))
    came_from = {}
    g_score = {start: 0}

    while open_set:
        _, current = heapq.heappop(open_set)
        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            return path[::-1]

        for neighbor in get_neighbors(grid, current):
            tentative_g = g_score[current] + 1
            if tentative_g < g_score.get(neighbor, float('inf')):
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f = tentative_g + heuristic(neighbor, goal)
                heapq.heappush(open_set, (f, neighbor))
    return []

def heuristic(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

---

Sending a Navigation Goal

from nav2_simple_commander.robot_navigator import BasicNavigator
from geometry_msgs.msg import PoseStamped

navigator = BasicNavigator()

goal = PoseStamped()
goal.header.frame_id = 'map'
goal.pose.position.x = 2.0
goal.pose.position.y = 1.5
goal.pose.orientation.w = 1.0

navigator.goToPose(goal)

while not navigator.isTaskComplete():
    feedback = navigator.getFeedback()
    print(f'Distance remaining: {feedback.distance_remaining:.2f}m')

---

References

• Nav2 Documentation
• Python Robotics — AtsushiSakai