# Long-Range Navigation in Complex and Dynamic Environments with Full-Stack S-DOVS +
## 3 Approach
### Trajectory Waypoint Generator  
Reactive planners designed to navigate in dynamic environments use information about obstacles and their trajectories to avoid obstacles and reach a goal. They are typically designed to navigate in scenarios where the initial position and goal are connected by a straight line. However, to navigate in the real world it is useful to have access to a known map. The robot may encounter large walls that could surround it, or it may become trapped. A global planner can be used to overcome such problems, enabling long range navigation. While we used S-DOVS as the local planner here, our approach could be used jointly with other deep reinforcement learning-based or model-based planners designed to navigate in dynamic environments. S-DOVS only requires one point as a goal, which can be far from the initial pose, and plans the trajectory in advance using estimated obstacle velocities. Moreover, the goal should be far enough to ensure that the local planner has the freedom to perform collision avoidance maneuvers. Additionally, the original static map may be updated while the robot navigates. If the robot senses static obstacles, it should take them into account in the new global map. For example, the robot may attempt to enter a room through a door, find the door closed, and replan to find an alternate path. This work uses the A* implementation of the ROS standard move base as a preliminary global planner, computing an initial global path. The map is kept up to date, with the global cost map taking newly seen static obstacles into account and the A* algorithm planning accordingly. The cost map's inflation ratio is set to twice the robot's size, ensuring that the S-DOVS has adequate space to choose its direction. In our approach, we consider a spatial horizon \(d_{ahead}\) to send the next waypoint to the local motion planner. When the distance from the robot to the current subgoal is less than \(d_{ahead}\), the next subgoal is followed, allowing the motion planner space to maneuver and deviate from the original path in order to avoid dynamic obstacles. Therefore, the waypoint generator saves the initial global plan computed by A* as \(path_{g}\) and keeps sending waypoints regarding the spatial horizon. Nevertheless, there is a problem with this approach. If \(d_{ahead}\) is too low, the motion planner may not maneuver properly, as the robot may not deviate from the global plan. If \(d_{ahead}\) is too high, the motion planner may not benefit from the map information and may not avoid convex obstacles. To solve this problem, our approach computes an auxiliary path \(path_{aux}\subseteq path_{g}\), which selects important waypoints that should not be overlooked. The value of \(d_{ahead}\) is high for the points of \(path_{g}\) that do not belong to \(path_{aux}\) and low for the ones that belong to \(path_{aux}\); thus, that the robot explicitly reaches the points that make it navigate through sharp corners or rooms. To compute \(path_{aux}\), the waypoint generator performs a downsampling operation, selecting one point per square meter, ensuring that the path followed is sparse. Intermediate points in a straight line are removed, as they do not provide any extra information for motion planning and reaching them would result in suboptimal performance compared to computing the velocities required to reach the final point. For each set of three points, if they are in the same line, the point in the middle is removed. This is determined by testing whether the slope of the line passing through the first and second points is similar to that of the line passing through the first and third points, using the following equation:  
\[\frac{y_{1}-y_{2}}{x_{1}-x_{2}}\approx\frac{y_{1}-y_{3}}{x_{1}-x_{3}}\implies |(y_{1}-y_{2})(x_{1}-x_{3})-(y_{1}-y_{3})(x_{1}-x_{2})|<\epsilon, \tag{2}\]  
The robot keeps track of a costmap with the area occupied by the static obstacles, which is updated with the information received by the sensors. It is initialized using the static map of the environment.