# Practicum 2: A navigation example

<div align="center">

**Practicum date: {{prac2_date}}**  

</div>

**Course:** RO47014 Knowledge Representation and Symbolic Reasoning, TU Delft, CoR  
**Instructors:** Carlos Hernandez Corbato, email: c.h.corbato@tudelft.nl


In this practicum you will work with a ROS 2 Python node that sends navigation goals to Nav2 using the `navigate_to_pose` action.

You will:
1. Configure custom waypoints for the robot using a configuration file.
2. Modify the node so it stops automatically once it returns to its starting position.

---

## Setup

Start the simulation like provided within the setup instructions, open your `src` folder and place the krr_navigator package in it. 

<!--TODO: update this download so that the zip can be updated more easily-->
<!-- see also: https://gitlab.tudelft.nl/cor/ro47014/ro47014_krr_manual/-/merge_requests/14#note_362653 -->
[Download the krr_navigator package](resource/krr_navigator.zip)

Rebuild your workspace and source it.
```bash
colcon build --symlink-install --packages-select krr_navigator
source install/setup.bash
```

Now, in a different singularity terminal, you can run the `krr_navigator_node` node with: 
```bash
ros2 run krr_navigator krr_navigator_node
```

---

As you can see the robot loops and navigates to 4 waypoints within the room. This is behavior we would like to adjust for gathering information for our KB.

## Exercise 1 – Configure waypoints using a YAML file


1. Inside the `krr_navigator` package, create a new folder called `config`.
2. Inside the `config` folder, create a file named `params.yaml`.
3. Open `config/params.yaml` and add the following content:
```yaml
       krr_navigator:
         ros__parameters:
           frame_id: "map"
           loop: true

           waypoints:
             - "[0.0, 0.0, 0]"
             - "[-4.0, 0.0, 0]"
             - "[-4.0, 3.5, 0]"
             - "[0.0, 3.5, 0]"
             - "[0.0, 0.0, 0]"
```
4. Build your workspace if needed:
```bash
colcon build --symlink-install --packages-select krr_navigator
source install/setup.bash
```

5. Run the node with the parameter file:
```bash
ros2 run krr_navigator krr_navigator_node --ros-args --params-file src/krr_navigator/config/params.yaml
```

As you can see the robot now goes to the same spots as before, now we can try to adjust the waypoints the robot can move to. 

## Exercise 2 – Stop navigation after returning to the start position

In Exercise 1, the robot continuously loops through the configured waypoints.  
In this exercise, you will modify the node so that it **stops automatically** once it has returned to its starting position.

1. Open the file `krr_navigator_node.py` inside the `krr_navigator` package.

2. Identify where the list of waypoints is stored in the node.

   The **first waypoint** in this list should be considered the **starting position**.

3. Store this starting position when the node starts.

   You will need this later to determine when the robot has completed its route.

4. Modify the navigation logic so that:
   - The robot navigates through all waypoints **once**
   - When the robot reaches the final waypoint **and**
   - That waypoint corresponds to the starting position

   the node stops sending new navigation goals.

5. When the robot has returned to its starting position:
   - Print an informational log message
   - Shut down the node cleanly

   Example log output:
```
[INFO] [krr_navigator]: Returned to start position
[INFO] [krr_navigator]: Navigation complete, shutting down
```
6. Build your workspace again:
```bash
colcon build --symlink-install --packages-select krr_navigator
source install/setup.bash
```
7. Run the node with the same parameter file as before:
```bash
ros2 run krr_navigator krr_navigator_node --ros-args --params-file src/krr_navigator/config/params.yaml
```
If implemented correctly, the robot should:
- Visit all configured waypoints
- Return to its starting position
- Stop automatically without looping forever