A 2D navigation stack that takes in information from odometry, sensor streams, and a goal pose and outputs velocity commands that are sent to a mobile base. As a prerequisite for navigation stack use, the robot must be running ROS, have a tf transform tree in place, and publish sensor data using the correct ROS Message types. Also, the navigation stack needs to be configured for the shape and dynamics of a robot. The algorithm was tested with a simulation of the Festo Robotino robot (RTO) in a virtual world in Gazebo.
 |
| GIF 1: NavPy in action. |
Fig.1 depicts the ROS computation graph of the NavPy navigation stack. It contains all necessary nodes and the topics that are used for communication between nodes. This should give a brief overview of the system's components.
 |
| Fig.1: ROS computation graph of the NavPy navigation stack. |
In the following, all packages within this repository are briefly explained.
Responsible for creating the local and the global costmap and adapting the costmaps to dynamic changes of the environment.
Transforms pgm files to occupancy grid maps and lets other nodes access these static maps via a service.
Creates a local path based on minimizing a cost function that leads the robot towards the goal.
Localizes the robot in the map.
Plans a path between the robot pose and an arbitrary valid goal.
Contains the launch file to start-up the NavPy navigation stack.
Contains world models that are used by the Gazebo simulation.
All other packages have been adapted from the following two repositories owned by the user dietro.
This repository contains everything needed to start-up the RTO like for example the robot's geometric description.
This repository contains additional packages in order to run the RTO in the Gazebo simulation environment.
In the following, all packages within this repository that are related to the NavPy navigation stack are briefly explained.
This package contains the 'costmap_generator_node', which is used for generating the local and the global costmap.
In order to allow the use of a point representation of the mobile robot for path planning, the static map from the map server has to be padded by the radius of the mobile robot and by an additional, optional safety distance. This leads to an area in that the robot should under no circumstances operate in. Therefore, the system will efficiently prevent the robot from crashing into obstacles that are positioned in the global costmap. This area is called the area of 'hard padding'. To additionally assign a higher cost to grid cells that are close to obstacles, a so called 'soft padding' area gets generated by the costmap generator. The robot is allowed to operate in this area but path planning through parts of the 'soft padded area' will lead to an additional cost that is taken into account by the global planner.
The global costmap is an occupancy grid map. Grid cells that are occupied by obstacles have the value 100, grid cells in the area of 'hard padding' the value 99 and grid cells in the area of 'soft padding' the values from 98 to 1. Free space is represented with the value 0 and unknown space with the value -1. Figures 2 - 4 will represent three costmaps with the same 'hard padding' area but different types and sizes of the 'soft padding' area.
 |
 |
 |
| Fig.2: 'Exponential' soft padding (0.2 m). | Fig.3: 'Exponential' soft padding (1.0 m). | Fig.4: 'Linear' soft padding (1.0 m). |
In a real world scenario it is not enough to make decisions based on a static global costmap, since dynamic changes in the surrounding might lead to significant changes in the global costmap. If these changes are not recognized by the system, the accuracy of the localization will be drastically reduced. Therefore, obstacles that are not taken into account by the current version of the global costmap have to be recognized and added in order to allow a smooth and stable navigation of the mobile robot. The local costmap serves this purpose by considering the current laser scan range measurements. Fig.5 depicts the local costmap and an obstacle that is currently not part of the global costmap.
 |
| Fig.5: Local costmap (green) detects a new obstacle. |
In order to add newly appearing obstacles, a service has been implemented that updates the global costmap based on the detections of the local costmap.
To receive the laser scan messages from the Hokuyo laser scanner, which is necessary for detecting local obstacles.
To receive the odometry messages from the odometry system.
To publish the occupancy grid of the padded, global costmap.
To publish the occupancy grid of the local costmap for visualization purposes.
To publish the point cloud of currently sensed obstacles, which has been transformed to the map frame.
Service that switches the static map that gets used by the costmap generator to generate the global costmap.
request: map_nr_switch [int8]
response: success [bool]
Service that resets the global costmap to the original state that only represents the padded static map. All additionally added obstacles will be deleted.
request: command [string] ('clear')
response: success [bool]
Service that adds elements from the local_obstacle point cloud to the global costmap.
request: command [string] ('stuck')
response: success [bool]
init_map_nr: Map to start the costmap generator with.
log_times: Log execution times of critical operations.
debug_mode: Return debugging messages to the terminal.
global_costmap:
robot_diameter: The diameter of the robot used for 'hard padding'.safety_distance: Additional distance used for 'hard padding'.padded_val: Value of the grid elements that are part of the 'hard padding' area.apply_soft_padding: Apply the area of 'soft padding' to the global costmap.decay_distance: Distance starting from the area of 'hard padding' that is affected by 'soft padding'.decay_type: Decay type of the area of 'soft padding' (linear, exponential, reciprocal).
local_costmap:
length: Width and height of the local costmap.frequency: Update frequency of the local costmap.frequency_scan: Frequency at which the laser scanner operates.
This package contains the rto_map_server node, which transforms a pgm file to an occupancy grid map and is able to store and switch between multiple maps. The map server also adds meta information that is stored in the corresponding yaml file to the OccupancyGrid message. The pgm and yaml files should be stored in the 'maps' folder of this package. To create the corresponding files for a new map, existing packages like the 'slam_toolbox' and the ROS navigation stack 'map_saver' can be utilized. The information of the yaml file should be added in the form of a dictionary to the rto_map_server config file.
none
To publish the OccupancyGrid message that has been constructed based on a pgm and yaml file.
Service that lets a caller get access to a OccupancyGrid based on a map number.
request: map_nr [int64]
response: map [nav_msgs/OccupancyGrid]
debug_mode: Return debugging messages to the terminal.
maps_nr: The number of maps being stored on the map server.
mapx:
image: Name of the pgm file stored in the 'maps' folder.resolution: Resolution of the pixels in meter.origin: List consisting of the x, y and z coordinate of the map's origin.occupied_thresh: Threshold of the grid value for being seen as occupied.free_thresh: Threshold of the grid value for being seen as free.
This package contains the local_planner_node, which creates a local path and allows the robot to follow the global path to reach the navigation goal.
The local planner implemented in this package is based on the dynamic window approach, which is an online collision avoidance strategy that samples trajectories from a generated, valid search space and selects the best trajectory for the current situation with the help of a cost function. The cost function consists of four different separate costs and is minimized in order to obtain the optimal control values. The four parts of the cost function are:
- cost based on linear velocity
- cost based on the angle towards the goal
- cost based on the proximity to the global path
- cost based on the proximity to obstacles
The overall cost for a control pair is 0 if the robot travels with its maximal linear velocity, looks directly towards the goal, is exactly on the global path and the distance to the closest obstacle is as big as possible. Based on the gain factors of the different costs the local planner will exhibit a certain behaviour. If the robot, for example, should dynamically avoid obstacles that are not part of the costmap, it would make sense to reduce the gain of the cost that is based on the proximity to the global path and increase the gain of the cost that is related to the proximity to obstacles. If the robot should however follow exactly the global path, different gain values might make more sense. The following GIFs show two completely different strategies for local planning.
 |
 |
| GIF 2: Local planner focuses on avoiding obstacles (gain values: 18 12 15 15). | GIF 3: Local planner focuses on staying on the global path (gain values: 28 2 80 1). |
Of course both strategies have advantages and disadvantages and it depends on the situation which version to use. Please be aware of the fact that the parameters are tuned for the robot to work in the Gazebo simulation environment. Applying the local planner to real world conditions might require additional parameter tuning.
The local planner node not only estimates the best control values, but also initialize a so called recovery behaviour that adds the local costmap to the global costmap via the service 'add_local_costmap' and publishes the current goal position again to initiate a re-planning of the global path. This is necessary in order to allow the system to react to changes of the world it operates in. Therefore, the local planner needs a couple of measures to decide if a recovery behaviour is necessary. The implemented measures are listed below:
- small linear velocity
- circling
- execution time of the current path
Based on the values of the related parameters a recovery behaviour will be carried out when the linear velocity is below a certain threshold for a certain amount of time. This can often be traced back to a global path blocked by an obstacle that is currently not included in the global costmap. This particular case can also lead to the robot circling in front of the obstacle. For this reason, a recovery behaviour is also initialized when the sign of the robot's angular velocity does not change for a specific time period. The last measure for initializing a recovery behaviour is the execution time of the current path. If the robot takes longer than expected to reach the goal position, this might also indicate that a recovery behaviour makes sense in this situation. The GIFs below demonstrate the typical scenarios that lead to an initialization and execution of a recovery behaviour.
 |
 |
 |
| GIF 4: Initializing a recovery behaviour based on a small linear velocity. | GIF 5: Initializing a recovery behaviour based on circling. | GIF 6: Initializing a recovery behaviour based on path execution time. |
To receive the current position in the odometry frame.
To calculate the distance of the robot to the global path and to estimate the angle to the goal.
To estimate the minimal distances of possible trajectories to the currently sensed obstacles.
To publish the best control pair in order to let the robot move as desired.
To publish a line strip marker that can be visualized in rviz.
To initialize a re-planning of the global path after new obstacles have been added to the costmap.
log_times: Log execution times of critical operations.
debug_mode: Return debugging messages to the terminal.
show_path: Show a representation of the local path in rviz.
lookahead: Time span (dt) used for the motion update and the estimation of the dynamic window.
robot_diameter: Diameter of the robot used to determine if a trajectory could result in a crash.
safety_distance: Distance that is additionally used with the robot diameter to estimate valid trajectories.
min_linear_vel: Minimum linear velocity of the robot.
max_linear_vel: Maximum linear velocity of the robot.
min_angular_vel: Minimum angular velocity of the robot.
max_angular_vel: Maximum angular velocity of the robot.
max_acc: Maximum acceleration of the robot.
max_dec: Maximum deceleration of the robot.
res_ang_vel_space: Resolution of the angular velocity in the dynamic window.
res_lin_vel_space: Resolution of the linear velocity in the dynamic window.
gain_vel: Gain factor of the cost based on linear velocity.
gain_goal_angle: Gain factor of the cost based on the angle towards the goal.
gain_glob_path: Gain factor of the cost based on the proximity to the global path.
gain_clearance: Gain factor of the cost based on the proximity to obstacles.
rec_min_lin_vel: Velocity threshold for a recovery based on small linear velocities.
rec_min_lin_vel_time: Time period in which the velocity has to be under the threshold.
rec_circling_time: Time period for circling after which the recovery behaviour gets initialized.
rec_path_time_factor: Factor that decides when to initialize a recovery behaviour based on overall path execution time.
rec_path_len: Minimal length of a global path to be considered for a recovery behaviour based on overall path execution time.
min_dist_goal: Distance to goal that is considered enough to declare that the goal has been reached.
This package contains the rto_localization node, which is responsible for localizing the robot in a map. When the rto_localization is launched it requests the map from the rto_map_server. For localizing the robot, a Monte Carlo localization algorithm is used. The navigation stack is able to work in a dynamic environment. Obstacles which are not included in the map will reduce the accuracy of the Monte Carlo localization drastically. Therefore, the map is updated by the rto_costmap_generator. The performance of the Monte Carlo localization is measured by the averaged error of all particles. This performance measurement is used to decide whether the pose of the robot is estimated by the Monte Carlo localization or the odometry. The Monte Carlo localization is used if the error is smaller than a given threshold. Otherwise, the estimated pose from the last iteration is updated according to the relative motion since then. The relative motion is received from the odometry. This is especially important when the robot senses a dynamic obstacle which is not yet included in the map. Such situations can be seen in the following. In GIF 7 the robot passes a dynamic obstacle solely by using the local planer. The obstacle is not included in the map. In GIF 8 the global path is recalculated by the global planer and the map is updated. In both situations the robot pose is estimated by the odometry as long as the robot senses the unknown obstacle. After updating the map or passing the obstacle the Monte Carlo localization is accurate again and can be used to estimate the robot pose.
 |
 |
| GIF 7: Pass dynamic obstacle solely by using local planer. | GIF 8: Update map of localization and recalculate the path. |
If the localization is not accurate for several iterations it might happen, that the particles drift away. By predicting the particles with a higher variance, the particles spread out. This allows the Monte Carlo localization to catch the pose of the robot again. It can be seen in GIF 9. To make sure, that the variance is not dominating the prediction of the particles, it is adapted to the angular and translational velocity of the robot.
 |
| GIF 9: Increased prediction variance when Monte Carlo localization is not accurate and odometry is used for estimate robot pose. |
Laser scan of the robot to update the particles.
Motion of the robot estimated by odometry to predict particles.
Update dynamic obstacles in the map.
Visualization of all particles in rviz as red arrows.
Visualization of the estimated pose of the localization in rviz as green arrow.
Estimated pose of the localization.
dynamics_translation_noise_std_dev: Each particle is predicted translational according to the odometry and a gaussian noise with this standard deviation.
dynamics_orientation_noise_std_dev: Each particle is predicted rotatory according to the odometry and a gaussian noise with this standard deviation.
num_particles: Number of particles used in the Monte Carlo localization.
num_beams: Number of laser beams used for updating the particles. The algorithm subsamples equally from all the laser beams of the robot's laser scanner
update_rate: Prediction and update rate of the Monte Carlo localization.
odom_noise: Noise can be included in the odometry which is used to predict the particles.
launch_style: The particles of the localization can be initialized randomly in the map or close to the position where the robot is spawned.
normalized_comulated_localization_error: Threshold which defines whether the localization or the odometry is used to estimate the robot's pose.
variance_increase_for_bad_localization: Defines how much the translation and orientation standard deviation is increased when the localization is not accurate and the odometry is used to estimate the pose of the robot .
This package contains the global_planner_node, which creates a global path that can be used by the local_planner to navigate the robot towards the goal.
This package contains the global_planner_node, which creates a global path that can be used by the local_planner to navigate the robot towards the goal. The global planner is based on a bidirectional A-Star algorithm. At the beginning, we start the search from both the robot's position and the goal position. After each iteration, the global planner will check whether there is an intersection between the open list of searching from the start point and that of the end point. If there is an intersection, then the global planner connects the path from the start point to the end point. It might be the case that the path looks like in Fig.6. So the global planner will then apply path smoothing to avoid unnecessary turns (see Fig.7). At last, to make sure that the local_planner will get a dense path that is represented by nodes next to each other, path augmentation is applied (see Fig.8).
 |
 |
 |
| Fig.6: Original path generated by the bidirectional A-Star algorithm. | Fig.7: Path after path smoothing has been applied. | Fig.8: Path after path augmentation has been applied. |
In some cases, the global planner might not find a path from the current position of the robot to the goal. In this situation, the global planner will make a call to the service /clear_map to reset the global costmap to its original state. This might delete obstacles that have been added to the global costmap at an earlier time that are not at this position anymore. Doing this might allow the global planner to find a valid path to the goal position. This procedure is also considered as recovery behaviour. A situation where this recovery behaviour helps to plan a valid path toward the goal (see GIF 10).
 |
| GIF 10: Recovery behaviour for the case that the global planner is unable to find a global path. |
To receive the padded costmap used to plan a feasible path.
To receive the current position of robot as the start point.
To receive the arranged point in map as the end point.
To publish the generated path message as a feasible path from current location to goal.
To publish a visualized plan in rviz.
In order to use the robot you first have to install Ubuntu Focal (20.04) as well as ROS Noetic, which currently is the only supported ROS version of this repository. Besides, you will need to install necessary dependencies of the Robotino robot. For details in installing ROS Noetic as well as all necessary dependencies, please refer to rto_core.
Afterwards you have to clone this repository to your catkin space and build it.
cd ~/catkin_ws/src
git clone https://github.com/KathiWinter/rto_Robot_Navigation.git
cd ~/catkin_ws
catkin buildTo use this robot in a simulated environment, you will have to export the following environment variables first:
export ROBOT=rto-1
export ROBOT_ENV=world1
To start up the robot in the Gazebo simulation environment, the line below should be entered into the terminal:
roslaunch rto_bringup_sim robot.launch
Afterwards, the NavPy navigation stack can be launched via the launch file navigation_navpy.launch:
roslaunch rto_navigation navigation_navpy.launch
To bring up rviz for visualization purposes, please enter the following line into the terminal:
roslaunch rto_bringup rviz.launch