Skip to content

YehiaElkh/turtle_navigation

BranchesTags

Repository files navigation

🐢 TurtleSim Navigation - ROS 2 Navigation Algorithms

ROS 2 Python License Status

Implementation of fundamental robot navigation algorithms using ROS 2 and TurtleSim

Features • Installation • Usage • Algorithms • Demo

📖 Table of Contents

🎯 Overview

This project implements three fundamental navigation algorithms used in autonomous robotics, developed as part of my robotics engineering curriculum. The implementation uses ROS 2 (Robot Operating System 2) with the TurtleSim simulator to demonstrate core concepts in mobile robot navigation.

🎓 Academic Context

  • Purpose: Educational project on autonomous navigation
  • Framework: ROS 2 Jazzy
  • Language: Python 3.8+

🔑 Key Concepts Implemented

  • PID Control for goal-seeking behavior
  • Finite State Machines for wall following
  • Potential Fields for obstacle avoidance
  • ROS 2 Architecture (nodes, topics, publishers/subscribers)

✨ Features

Core Functionality

  • ✅ Go-to-Goal Navigation - PID-based controller for precise target reaching
  • ✅ Wall Following - State machine logic for wall tracking
  • ✅ Obstacle Avoidance - Potential field method for dynamic obstacle handling
  • ✅ Interactive Launcher - User-friendly menu for algorithm selection
  • ✅ Automated Testing - Comprehensive test suite for validation

Technical Highlights

  • 🔄 Real-time velocity control with safety limits
  • 📊 Detailed logging with position, distance, and velocity tracking
  • 🛡️ Robust error handling and graceful shutdown
  • 🎯 Configurable parameters for experimentation
  • 🧪 Unit and integration tests included

📁 Project Structure

turtle_navigation_ws/
├── src/
│   └── turtle_navigation/
│       ├── package.xml              # ROS 2 package manifest
│       ├── setup.py                 # Python package setup with entry_points
│       ├── setup.cfg                # Installation configuration
│       ├── resource/                # Package resources
│       │   └── turtle_navigation
│       └── turtle_navigation/       # Source code
│           ├── __init__.py          # Python package initialization
│           ├── go_to_goal.py        # PID-based goal navigation
│           ├── wall_follower.py     # Wall following algorithm
│           ├── obstacle_avoidance.py # Potential field obstacle avoidance
│           └── launcher.py          # Interactive menu launcher
├── build/                           # Build artifacts (generated)
├── install/                         # Installation files (generated)
├── log/                             # Build and runtime logs (generated)
├── test_all_modules.sh              # Automated test script
├── README.md                        # Documentation principale ultra-complète...
└── test.webm                        # Sortie

🛠️ Installation

Prerequisites

Ensure you have the following installed:

  • Ubuntu 22.04 (or 20.04)
  • ROS 2 Jazzy (or Hunble)
  • Python 3.8+
  • TurtleSim package

Step 1: Install ROS 2 (if not already installed)

# Add ROS 2 repository
sudo apt update && sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt update && sudo apt install curl -y

# Install ROS 2 Humble
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

sudo apt update
sudo apt install ros-jazzy-desktop

Step 2: Install TurtleSim

sudo apt install ros-jazzy-turtlesim

Step 3: Clone and Build the Project

# Create workspace
mkdir -p ~/turtle_navigation_ws/src
cd ~/turtle_navigation_ws/src

# Clone the repository
git clone https://github.com/YehiaElkh/turtle_navigation.git
cd ..

# Source ROS 2
source /opt/ros/jazzy/setup.bash

# Build the package
colcon build --packages-select turtle_navigation

# Source the workspace
source install/setup.bash

🚀 Usage

🎬 Demonstration

Navigation Demo

Method 1: Interactive Launcher (Recommended)

# Source the workspace
source ~/turtle_navigation_ws/install/setup.bash

# Launch interactive menu
ros2 run turtle_navigation launcher

Interactive Menu:

🐢 === TurtleSim Navigation Launcher === 🐢

Sélectionnez un algorithme:
1. Go to Goal (Navigation vers un point)
2. Wall Follower (Suivi de mur)
3. Obstacle Avoidance (Évitement d'obstacles)
4. Quitter

Votre choix (1-4): _

Method 2: Direct Node Execution

Go-to-Goal Navigation

# Terminal 1: Launch TurtleSim
ros2 run turtlesim turtlesim_node

# Terminal 2: Run go-to-goal
ros2 run turtle_navigation go_to_goal --ros-args -p goal_x:=8.0 -p goal_y:=8.0

Parameters:

  • goal_x: Target X coordinate (default: 8.0)
  • goal_y: Target Y coordinate (default: 8.0)
  • distance_tolerance: Arrival threshold (default: 0.1)

Wall Follower

# Terminal 1: Launch TurtleSim
ros2 run turtlesim turtlesim_node

# Terminal 2: Run wall follower
ros2 run turtle_navigation wall_follower

Obstacle Avoidance

# Terminal 1: Launch TurtleSim
ros2 run turtlesim turtlesim_node

# Terminal 2: Run obstacle avoidance
ros2 run turtle_navigation obstacle_avoidance --ros-args -p goal_x:=9.0 -p goal_y:=9.0

Parameters:

  • goal_x: Target X coordinate (default: 9.0)
  • goal_y: Target Y coordinate (default: 9.0)

Method 3: Automated Testing

cd ~/turtle_navigation_ws
./test_all_modules.sh

This script automatically tests all three navigation algorithms with predefined scenarios.

🧠 Algorithms

1. 🎯 Go-to-Goal (PID Control)

File: go_to_goal.py

Algorithm Overview

Uses a PID (Proportional-Integral-Derivative) controller to navigate the turtle to a target position.

Key Features

  • ✅ Proportional control for both linear and angular velocity
  • ✅ Angle normalization to handle [-π, π] wrapping
  • ✅ Velocity saturation for safety
  • ✅ Distance-based threshold for goal detection

Control Law

Linear Velocity:

v_linear = K_linear × distance_to_goal

Angular Velocity:

θ_error = atan2(Δy, Δx) - current_θ
v_angular = K_angular × normalize_angle(θ_error)

Parameters

Parameter Value Description
K_linear 1.5 Linear velocity gain
K_angular 6.0 Angular velocity gain
max_linear_speed 1.5 Maximum linear velocity (m/s)
max_angular_speed 2.0 Maximum angular velocity (rad/s)
distance_tolerance 0.1 Goal reached threshold (m)

Performance

  • Convergence Time: ~7 seconds to target (8,8)
  • Precision: ±0.1m from goal
  • Trajectory: Smooth curved path with velocity decay near target

2. 🧱 Wall Follower (Finite State Machine)

File: wall_follower.py

Algorithm Overview

Implements a two-state Finite State Machine (FSM) to follow walls in the environment.

State Machine

┌─────────────┐    Wall Detected    ┌──────────────┐
│ FIND_WALL   │ ──────────────────> │ FOLLOW_WALL  │
└─────────────┘                     └──────────────┘
      ↑                                     │
      └─────────────────────────────────────┘
           Wall Lost or Obstacle Ahead

States

State 1: FIND_WALL

  • Turn right until a wall is detected on the right side
  • Activation: Initial state or when wall is lost

State 2: FOLLOW_WALL

  • Move forward while maintaining constant distance from wall
  • Use right-side distance feedback for correction
  • Obstacle detection: Switch to find_wall if front blocked

Control Logic

if state == "find_wall":
    if right_distance < wall_detect_threshold:
        state = "follow_wall"
    else:
        turn_right()

elif state == "follow_wall":
    if front_distance < obstacle_threshold:
        state = "find_wall"
    else:
        maintain_wall_distance(desired_distance)

Parameters

Parameter Value Description
desired_wall_distance 1.0 Target distance from wall (m)
wall_detect_threshold 1.5 Wall detection range (m)
obstacle_threshold 0.8 Front obstacle avoidance (m)

3. 🚧 Obstacle Avoidance (Potential Fields)

File: obstacle_avoidance.py

Algorithm Overview

Uses Artificial Potential Fields method combining attractive forces (goal) and repulsive forces (obstacles).

Potential Field Theory

Total Force:

F_total = F_attractive + Σ F_repulsive

Attractive Force (Goal):

F_att = K_att × (goal - position)

Repulsive Force (Obstacle):

F_rep = K_rep × (1/d - 1/d₀) × (1/d²) × (position - obstacle)
where d = distance to obstacle, d₀ = influence radius

Configuration

Predefined Obstacles:

  • Obstacle 1: (3.0, 3.0)
  • Obstacle 2: (6.0, 3.0)
  • Obstacle 3: (5.0, 6.0)

Parameters

Parameter Value Description
K_attractive 1.0 Attractive force gain
K_repulsive 2.0 Repulsive force gain
obstacle_influence 1.5 Obstacle influence radius (m)
max_linear_speed 1.5 Maximum linear velocity (m/s)
max_angular_speed 3.0 Maximum angular velocity (rad/s)

Advantages

  • ✅ Smooth trajectories around obstacles
  • ✅ Real-time adaptation to environment
  • ✅ No need for path pre-planning

Limitations

  • ⚠️ Local minima possible in complex environments
  • ⚠️ Requires tuning of force gains

🔧 Technical Details

ROS 2 Architecture

Node Communication

┌─────────────────┐         /cmd_vel         ┌──────────────┐
│  Navigation     │ ─────────────────────────>│  TurtleSim   │
│  Algorithm Node │                           │  Simulator   │
└─────────────────┘ <─────────────────────────└──────────────┘
                         /turtle1/pose

Topics Used

Topic Type Direction Description
/cmd_vel geometry_msgs/Twist Publish Velocity commands
/turtle1/pose turtlesim/Pose Subscribe Current turtle position

Message Types

Twist Message (Velocity Command):

linear:
  x: float  # Forward velocity (m/s)
  y: 0.0    # Not used in 2D
  z: 0.0    # Not used in 2D
angular:
  x: 0.0    # Not used in 2D
  y: 0.0    # Not used in 2D
  z: float  # Rotational velocity (rad/s)

Pose Message (Position Feedback):

x: float           # X position (0-11)
y: float           # Y position (0-11)
theta: float       # Orientation (-π to π)
linear_velocity: float
angular_velocity: float

Code Quality

Software Engineering Practices

  • ✅ Modular Design: Each algorithm in separate file
  • ✅ Error Handling: Try-catch blocks and cleanup
  • ✅ Logging: Detailed INFO level logs with emojis
  • ✅ Configuration: Parameterized for flexibility
  • ✅ Documentation: Inline comments and docstrings

Entry Points Configuration

# setup.py
entry_points={
    'console_scripts': [
        'go_to_goal = turtle_navigation.go_to_goal:main',
        'wall_follower = turtle_navigation.wall_follower:main',
        'obstacle_avoidance = turtle_navigation.obstacle_avoidance:main',
        'launcher = turtle_navigation.launcher:main',
    ],
}

📊 Test Results

Automated Test Suite

The project includes a comprehensive test script (test_all_modules.sh) that validates all algorithms.

Test 1: Go-to-Goal Navigation

Scenario: Navigate from (5.54, 5.54) → (8.0, 8.0)

Results:

  • ✅ Success: Target reached in ~7 seconds
  • ✅ Final Distance: 0.10m (within tolerance)
  • ✅ Trajectory: Smooth curved path
  • ✅ Velocity Profile: Progressive deceleration near target

Sample Output:

[INFO] GoToGoal démarré - Cible: (8.0, 8.0)
[INFO] 📍 Position: (5.54, 5.54) | 📏 Distance: 3.47 | ⚡ Vitesse: lin=1.50, ang=2.00
[INFO] 📍 Position: (7.93, 7.92) | 📏 Distance: 0.10 | ⚡ Vitesse: lin=0.05, ang=-0.00
[INFO] 🎯 Cible atteinte!

Test 2: Wall Follower

Scenario: Follow wall perimeter for 15 seconds

Results:

  • ✅ State Transitions: Smooth switching between states
  • ✅ Wall Distance: Maintained at ~1.0m
  • ✅ Collision Avoidance: No boundary violations

Test 3: Obstacle Avoidance

Scenario: Navigate to (9.0, 9.0) avoiding 3 obstacles

Results:

  • ✅ Obstacle Detection: All obstacles successfully avoided
  • ✅ Path Planning: Dynamic trajectory adjustment
  • ✅ Goal Reaching: Successfully reached target

Build Verification

Starting >>> turtle_navigation
Finished <<< turtle_navigation [1.31s]

Summary: 1 package finished [1.43s]
✅ Tests terminés avec succès !

🎓 Learning Outcomes

ROS 2 Competencies

  • ✅ Creating and configuring ROS 2 packages (ros2 pkg create)
  • ✅ Understanding ROS 2 architecture (nodes, topics, QoS)
  • ✅ Publishers and Subscribers implementation
  • ✅ Parameter handling and runtime configuration
  • ✅ Colcon build system and workspace management

Robotics Algorithms

  • ✅ PID Control: Proportional feedback for trajectory tracking
  • ✅ State Machines: Behavior-based robotics programming
  • ✅ Potential Fields: Reactive navigation and obstacle avoidance
  • ✅ Velocity Control: Safe motion command generation

Software Engineering

  • ✅ Python package structure and setup.py configuration
  • ✅ Error handling and graceful shutdown (SIGINT)
  • ✅ Logging and debugging techniques
  • ✅ Automated testing and continuous validation
  • ✅ Code modularity and reusability

🔮 Future Enhancements

Short Term

  • Launch Files: Create .launch.py files for easier deployment
  • Parameter Files: YAML configuration files for parameters
  • Visualization: RViz integration for trajectory visualization
  • Dynamic Reconfigure: Runtime parameter adjustment

Medium Term

  • Sensor Integration: Lidar-based obstacle detection
  • Path Planning: A* or RRT algorithms for global planning
  • Localization: Kalman filter for pose estimation
  • Multi-Robot: Coordination between multiple turtles

Long Term

  • Real Robot: Port to TurtleBot3 or similar platform
  • SLAM: Simultaneous Localization and Mapping
  • Machine Learning: RL-based navigation
  • 3D Navigation: Extend to 3D environments with drones

👨‍💻 Author

Yahya Elkhoulati

🤝 Contributing

Contributions are welcome! Here's how you can help:

  1. Fork the repository
  2. Create a feature branch (git checkout -b feature/AmazingFeature)
  3. Commit your changes (git commit -m 'Add some AmazingFeature')
  4. Push to the branch (git push origin feature/AmazingFeature)
  5. Open a Pull Request

Contribution Guidelines

  • Follow PEP 8 style guide for Python code
  • Add tests for new features
  • Update documentation as needed
  • Use meaningful commit messages

📄 License

This project is licensed under the MIT License.

MIT License

Copyright (c) 2025 Yehia Elkh

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

🙏 Acknowledgments

  • ROS 2 Community for excellent documentation and support
  • TurtleSim developers for the simulation environment
  • Open Robotics for maintaining the ROS ecosystem
  • Robotics Engineering Program for the learning opportunity

📚 References

Documentation

Academic Papers

  • Khatib, O. (1986). "Real-time obstacle avoidance for manipulators and mobile robots"
  • Siegwart, R., & Nourbakhsh, I. R. (2004). "Introduction to Autonomous Mobile Robots"

Related Projects

⭐ Star this repository if you find it helpful!

Made with ❤️ by Yehia Elkh

About

ROS 2 navigation algorithms (PID, FSM, Potential Fields) for autonomous mobile robots | TurtleSim implementation...

Topics

python robotics path-planning ros finite-state-machine potential-fields control-systems mobile-robots obstacle-avoidance ros2 pid-controller turtlesim autonomous-navigation

Resources

Readme
Activity

Stars

4 stars

Watchers

0 watching

Forks

0 forks
Report repository

Releases 1

v1.0.0 - Initial Release 🚀 Latest
Feb 11, 2026

Packages

 
 
 

Contributors

Languages