# Navigation 2

Nav2 a une grosse documentation alors je vais faire un résumer de son utilisation pour la voiture et donner des conseils pour l'utiliser.

![archi_de_nav2](images/nav2_architecture_diapo.png)

## TF 

```
map
 │
 │  publié par AMCL
 │  TF : map -> odom
 ▼
odom
 │
 │  publié par odom_node
 │  TF : odom -> base_footprint
 ▼
base_footprint
 │
 │  TF fixe
 ▼
base_link
 ├── laser_frame
 │     LiDAR
 │
 ├── sonar_frame
 │     Ultrason
 │
 ├── ir_left_frame
 │
 └── ir_right_frame

```

## Behavior Tree

Les Behavior Tree sont une autre façon de faire des machines à état.

[Tuto pour visualiser les BT](https://docs.nav2.org/tutorials/docs/groot2.html)
(Le BT `navigate_to_pose_ackermann.xml` est utiliser dans race_node mais pas dans race_node_sliding_path)


Liste des packages de nav2 utiliser : 

Localisation : 
'map_server', 'amcl'

Navigation : 
'controller_server',
'planner_server',
'behavior_server',
'velocity_smoother',
'collision_monitor',
'bt_navigator',


'smoother_server' : Pas utiliser mais peut etre utile dans le future si on utilise encore Hybride A* et qu'on veut des trajectoires plus arrondie.

## Descriptions des packages

### map_server

Affiche et publie la map (occupancy grid) de la zone.

### amcl
Localisation probabiliste de la voiture sur la map à partir des données du LiDAR.

### controller_server
Permet de suivre une trajectoire en utilisant un controller (Regulated Pure Pursuit ou VectorPursuit).  
Exemple de Regulated Pure Pursuit :  
![RPP](images/RPP.gif)


### planner_server
Permet de planifier une trajectoire entre un point de départ et un point d'arrivé en utilisant Hybride A*.

### behavior_server
Permet de faire des actions simple comme backup (reculer), drive_on_heading (avancer) et wait.

### velocity_smoother
Permet de limiter l'accélération de la voiture pour éviter d'abimer les engrenages en plastique et d'avoir une vitesse plus stable.

### Collision_monitor

Ralenti de manière progressivie (généralement pas assez) quand il predit un collision mais a le défaut de faire arrete la voiture si il y a une mauvaise prediction (objet esquiver de justesse).


### bt_navigator

Permet d'utiliser des Behavior Tree.

## Workflow

Pour changer les params dans nav2_params il est recommandé de regarder la documentation pour voir le params par defaut et leur signification.  
La voiture est sous Jazzy, donc il faut faire attention au nom des params utiliser par rapport a la doc. On peut les verifier dans le code dans le github de nav2 sur la branche jazzy [https://github.com/ros-navigation/navigation2/tree/jazzy](https://github.com/ros-navigation/navigation2/tree/jazzy).

Les paramètres important sont : 

```yaml
amcl:
  ros__parameters:
    # rafraichissement de AMCL
    update_min_d: 0.15
    update_min_a: 0.05 

    # param qui ont un impact sur le temps de calcul d'AMCL
    max_particles: 3000
    max_beams: 120

controller_server:
  ros__parameters:
    general_goal_checker: # pour race_node uniquement
      plugin: "nav2_controller::PositionGoalChecker"
      xy_goal_tolerance: 1.0 #m
      path_length_tolerance: 1.0 #m

    # --- Regulated Pure Pursuit Controller ---
    FollowPath:
      desired_linear_vel: 1.2          # vitesse de croisière (m/s)
      max_lookahead_dist: 0.9
      use_regulated_linear_velocity_scaling: false
      use_collision_detection: false


local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 12.0 # frequence du lidar
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0
        inflation_radius: 0.25
      range_layer:
        plugin: "nav2_costmap_2d::RangeSensorLayer"
        enabled: false   # infrarouge désactiver car trop bruité

global_costmap:
  global_costmap:
    ros__parameters:
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.30


velocity_smoother:
  ros__parameters:
    max_velocity: [3.0, 0.0, 99.0]
    min_velocity: [-1.5, 0.0, -99.0]
    max_accel: [1.0, 0.0, 99.0]
    max_decel: [-3.0, 0.0, -99.0]

collision_monitor:
  ros__parameters:
    cmd_vel_out_topic: "cmd_twist" #nom du topic en sortie de nav2
```