# Long-Range Navigation in Complex and Dynamic Environments with Full-Stack S-DOVS +
## 3 Approach
### Trajectory Waypoint Generator
This costmap is dinamycally updated as the robot navigates and senses the environment. On the one hand, if the LIDAR measurements sense a static obstacle that had not been previously considered, it is added to the costmap, so that the new proposed trajectories and waypoints do not intersect with the obstacles. On the other hand, if no obstacle is detected in a place where an obstacle was supposed to be, the costmap in that positions is cleared, so that the robot may navigate there. This functionality allows the robot to, for example, detect whether a door is open or closed and to choose that path or an alternative one. If there is not any possible path at all, the waypoint generator does not generate any point. In addition, this algorithm assumes that the static map is known in advance, but it may work when there is a significant difference between the map and the environment. The static map is used to initialize the costmap, but, as discussed before, this costmap is dinamycally updated if the environment is not the same as the costmap. Thus, as the robot navigates in the environment, the costmap will be more precise and the performance will improve. Furthermore, the approach could be easily adapted when the map is unknown, by replacing the map and AMCL localization combination with a Simultaneous Localization and Mapping (SLAM) algorithm, such as Gmapping [32], which is publicly available as a ROS node, to build the whole map while the robot navigates. We provided replanning capabilities to the waypoint generator to ensure that a new plan is generated if the robot is too far away from the global planner's plan. This is detected by measuring the distance from the robot to the segment \(path_{aux}\) joining the last point reached and the next to be reached. In addition, a new plan is produced if the robot is stuck. The criteria followed to decide whether the robot is stuck is to check whether its linear velocity is null for some number of timesteps. Moreover, replanning is conducted the first time the robot reaches a velocity lower than the threshold. If the robot keeps the same low velocity, it does not perform additional replanning, as there are situations in which low velocities are necessary to avoid collisions. This is intended to avoid situations in which the robot is completely stuck before they can occur. It is important to note that the new plan may be similar to the previous one. Therefore, if the robot reaches a low velocity due to dynamic obstacles which are not included in the cost map, the new plan sent will not influence the current trajectory of the robot. Furthermore, if it deviates from the original path due to dynamic obstacles, the new plan recomputed would be similar to the previous one, except now accounting for the new starting position of the robot in order to adapt the waypoints to it. With this approach, the robot follows points that adapt to the S-DOVS trajectory and replans when the map is updated with new static obstacles that block its way. A high-level algorithm of the design of the waypoint generator is shown in Algorithm 1. ```
Input: Global path \(path_{g}\), robot distance to goal \(d_{goal}\) Output: Next subgoal \(g\) Data:\(d_{long}\),\(d_{short}\) /* Long and short lookahead distances */ \(path_{aux}\gets subsample\_and\_remove\_straight(path_{g})\) \(g\gets get\_first\_point(path_{g})\) while\(g\
eq get\_last\_point(path_{g})\)do if\(g\in path_{aux}\)then \(d_{ahead}\gets d_{short}\) else \(d_{ahead}\gets d_{long}\) end if\(d_{goal}\leq d_{ahead}\)then \(g\gets get\_next\_point(path_{g})\) end if\(robot\_is\_stuck()\lor robot\_ deviates(path_{aux})\lor robot\_low\_velocity()\)then \(path_{g},path_{aux}\gets generate\_new\_path()\)  end if  end while
```  
**Algorithm 1**Waypoint generator  
In Algorithm 1, the \(robot\_is\_stuck()\) function checks whether the robot is completely stopped due to being trapped. It has a timeout to send a new plan every few time steps until the robot can move again due to a change in the environment.