# Tangent Planner

A ROS-based autonomous navigation and path planning system with obstacle avoidance capabilities for mobile robots using Time-of-Flight (ToF) sensors.

## Overview

The Tangent Planner implements a tangent bug algorithm for autonomous robot navigation, allowing robots to navigate to goal positions while avoiding obstacles using ToF sensor feedback. The system uses velocity control with intelligent state management to handle different navigation scenarios.

## Features

- **Autonomous Navigation**: Navigate robots from start to goal position automatically
- **Obstacle Avoidance**: Detect and avoid obstacles using ToF sensor data
- **Boundary Following**: Follow obstacle boundaries to circumnavigate obstacles
- **State Machine**: Robust state management for different navigation behaviors
- **ROS Integration**: Full ROS1 integration with standard message types
- **Parameter Configuration**: Flexible controller tuning via configuration files
- **Unit Tests**: Comprehensive Google Test suite for algorithm validation

## Architecture

### Robot States

The robot operates in five distinct states:

1. **StopAtGoal** - Robot has reached the goal position
2. **TurnToGoal** - Robot orients toward the goal before moving (when angular error exceeds threshold)
3. **MotionToGoal** - Robot moves toward the goal with computed velocity
4. **OrientToGoal** - Robot position is correct but orientation needs adjustment
5. **FollowBoundary** - Robot follows obstacle boundary to navigate around obstacles

### Core Workflow

`computeVelocityCommand()` executes four main steps:

1. **Calculate Errors** - Compute x, y, and angular differences between current pose and goal
2. **Update State** - Transition to appropriate state based on errors and obstacles
3. **Update Controller** - Configure controller parameters based on current state
4. **Update Velocity** - Calculate linear and angular velocity using nonlinear control law

### ROS Communication

#### Subscribers

- **Odometry** (`/odom`) - Receives Pose2D messages (x, y, θ) for robot localization
- **ToF Scanner** (`/scan`) - Receives range data (field of view, ranges, min/max distance)

#### Publishers

- **Velocity Command** (`/cmd_vel`) - Publishes twist messages (linear x, linear y, angular z)

### Obstacle Avoidance Algorithm

The system implements a boundary-following algorithm with history-based decision making:

1. Robot begins motion toward goal
2. Upon obstacle detection, switches to FollowBoundary state
3. Saves historical robot and obstacle positions for trajectory planning
4. Decides next movement based on accumulated history
5. Exits FollowBoundary when path to goal is clear
6. Returns to MotionToGoal until reaching destination

### Controllers

**Motion Controller**
- Handles states requiring position changes (x, y movement)
- Tunable parameters: `b`, `c` (gains)
- Computed parameters: `k1`, `k2`, `k3` (derived from gains)

**Turn Controller**
- Handles pure rotation movements
- Tunable parameters: `kp_` (proportional gain), `kp_i_` (integral gain)

## Project Structure

```
.
├── include/
│   ├── tangent_planner.h           # Core algorithm implementation
│   ├── tangent_planner_node.h      # ROS node wrapper
│   └── parameters_helper.h         # Configuration management
├── src/
│   ├── main.cpp                    # Application entry point
│   ├── tangent_planner.cpp         # Algorithm implementation
│   ├── tangent_planner_node.cpp    # ROS node implementation
│   └── gtest.cpp                   # Unit tests
├── launch/                         # ROS launch files
├── param/                          # Configuration parameters
├── srv/                            # ROS service definitions
├── CMakeLists.txt                  # CMake build configuration
├── package.xml                     # ROS package metadata
└── Dockerfile                      # Docker build environment
```

## Dependencies

### ROS Packages

- `roscpp` - C++ ROS client library
- `std_msgs` - Standard ROS message types
- `nav_msgs` - Navigation message types
- `geometry_msgs` - Geometry message types
- `sensor_msgs` - Sensor message types (for scanner data)

### Build Requirements

- CMake 2.8.3+
- GCC/G++ compiler
- catkin build system
- Google Test (for unit tests)

## Building

### From Source with catkin

```bash
# Create and navigate to catkin workspace
mkdir -p catkin_ws/src
cd catkin_ws/src

# Clone repository
git clone <repository-url> tangent_planner
cd ..

# Build
catkin_make
source devel/setup.bash
```

### With Docker

Build a Debian package using Docker:

```bash
sudo ./release.sh
```

Note: Requires Docker to be installed on your system.

## Installation

### From Debian Package

1. Generate package: `sudo ./release.sh`
2. Install package: `dpkg -i tangent_planner_*.deb`
3. Source ROS setup: `source /opt/ros/<distro>/setup.bash`
4. Run the node with your ROS configuration

### Running the Node

```bash
rosrun tangent_planner tangent_planner
```

Or use the provided launch files:

```bash
roslaunch tangent_planner <launch-file-name>.launch
```

## Configuration

### Parameter Files

Configuration parameters are located in the `param/` directory. Adjust controller gains and thresholds:

- Motion controller gains (`b`, `c`, `k1`, `k2`, `k3`)
- Turn controller gains (`kp_`, `kp_i_`)
- State transition thresholds
- Obstacle following parameters

### Services

The package provides several ROS services:

- `nav_path_start` - Start navigation to goal
- `nav_path_stop` - Stop current navigation
- `nav_get_patrol_name` - Get current patrol/goal name
- `nav_get_status` - Get current navigation status
- `nav_low_bat` - Handle low battery conditions

## Testing

Run the unit tests:

```bash
catkin_make run_tests
# or
rosrun tangent_planner tangent_planner_test
```

## License

See LICENSE file for details.

## Maintainer

- **Gfuse** (gfuse@gfuse.it)