## Chapter description​

In this tutorial you will learn what path planning is, which will be implemented with the package move_base. Thanks to the knowledge gained in this and previous chapters, functionality will be implemented that will enable the implementation of an autonomous robot that will be able to reach any place on the map. The chapter is quite difficult, so I encourage you to work through this material when you are rested.

You can run this tutorial on:

Repository containing the final effect after doing all the ROS Tutorials you can find here

We have also prepared ready to go virtual environment with end effect of following this tutorial. It is available on ROSDS:

## Introduction​

Task of path planning for mobile robot is to determine sequence of manoeuvrers to be taken by robot in order to move from starting point to destination avoiding collision with obstacles. Some of the most popular scheduling algorithms are: Dijkstra’s algorithm, A*, D*, Artificial potential field method, Visibility graph method. All of these path planning algorithms are based on one of two approaches.

### Graph methods​

Method that is using graphs, defines places where robot can be and possibilities to traverse between these places. In this representation graph vertices define places e.g. rooms in building while edges define paths between them e.g. doors connecting rooms. Moreover each edge can have assigned weights representing difficulty of traversing path e.g. door width or energy required to open it. Finding the trajectory is based on finding the shortest path between two vertices while one of them is robot current position and second is destination.

### Occupancy grid methods​

Method that is using occupancy grid divides area into cells (e.g. map pixels) and assign them as occupied or free. One of cells is marked as robot position and another as a destination. Finding the trajectory is based on finding shortest line that do not cross any of occupied cells.

### Path planning in ROS​

In ROS it is possible to plan a path based on the occupancy map created in previous chapter, created with the slam_gmapping node. The next step is to add path planning, which will allow you to determine the cost map from which the optimal trajectory will be selected. Knowing the best trajectory, we can choose the control that will allow us to travel the designated path as quickly as possible. In ROS, both of these functionalities: determining the optimal path and determining the appropriate speed values ​​can be calculated by the move_base node from the move_base package.

The rest of the tutorial will be based on the knowledge from the previous chapters, so make sure that the following topics are familiar to you.

• Tutorial 4: How to control the robot?
• Tutorial 7: How information about the mutual position of objects is published?
• Tutorial 8: What is and how to receive data from a lidar/laser scanner?
• Tutorial 8: How to create and upload a map?
• Tutorial 8: How to locate the robot on the map?

## Introduction to move_base​

The move_base node is quite a complicated component because it implements as many as 5 closely related functionalities:

1. Creates a global cost map.
2. Determines the optimal trajectories based on the global cost map.
3. Creates a local cost map.
4. Performs temporary control based on the local cost map.
5. Recovery mode - determines how the robot should behave when the robot gets stuck.

Below is a diagram that contains all the elements discussed inside. On the right and left are the components that must (blue rectangles) or should be provided (grey rectangles) for the path planning algorithm to work. At the top we have information about the name of the message that starts the path planning procedure. This message triggers the so-called action - asynchronous call to another node's function (asynchronous means you don't have to wait for the procedure to finish, so you can do other things at any time). The output is a geometry_msgs/Twist message returning a velocity vector.

For all these functionalities to work properly, dozens of parameters must be properly configured. If you don't, your robot may move very counter-intuitively and even refuse to follow a certain trajectory. Configuring all these parameters is not easy, but who does not fight, does not drink champagne... or in our case, does not drive a ROSbot! 🏎️

### Configure move_base​

It is a known and good practice to store all parameters in a separate configuration folder. It used to be called config. Inside this folder I will place 6 configuration files with the extension .yaml. They will be:

• costmap_common.yaml - contains parameters for local and global maps
• costmap_global.yaml - contains parameters only for the global cost map
• costmap_local.yaml - contains parameters only for the local cost map
• planner_global.yaml - contains parameters for planning the local trajectory
• planner_local.yaml - contains parameters for planning the global trajectory
• move_base.yaml - contains parameters for the general behavior of the package including recovery behaviour.
Configuration note

Note that the configuration files shown below are dedicated to ROSbot 2R and may be slightly changed depending on the user's preferences or the environment in which the robot moves. For other robots, there may be a need to fine-tune these parameters individually.

## Cost map​

A cost map is a grid map where each cell is assigned a specific value - cost. The cost is related to the distance from the obstacle, the closer the object is, the higher the cost of a given cell on the map. We will create two cost maps, global and local. The global map is needed by the global planner and will be used to determine the shortest path from point A to point B based on historical data provided from the map and sensors. The local map, in turn, is needed by the local planner to find the best control over a section of the path.

note

If you ask yourself: "Okay, but why two maps? Wouldn't the algorithm work on one map?". I'm in a hurry to answer now. That's right, the algorithm could work. However, by splitting it into two maps, the local map can react differently to objects that appear. In addition, breaking the algorithm into two maps can improve the speed of the algorithm by reducing the local map and reducing the frequency of updating the global map.

Let's configure cost maps. To do this, create 3 configuration files in the tutorial pkg/config directory. These will be: costmap_common.yaml, costmap_global.yaml and costmap_local.yaml.

### Cost map - common parameters​

The first should contain common parameters for both maps, such as:

~/ros_ws/src/tutorial_pkg/config/costmap_common.yaml
# Reff: http://wiki.ros.org/costmap_2dglobal_frame: maprobot_base_frame: base_linkfootprint: [[0.14, 0.14], [0.14, -0.14], [-0.14, -0.14], [-0.14, 0.14]]rolling_window: trueinflation_radius: 0.5cost_scaling_factor: 4.0track_unknown_space: trueobservation_sources: scanscan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

The code explained

• global_frame - defines name of frame which contains occupancy grid map (crated in previous tutorial).
• robot_base_frame - defines name of robot base.
• footprint - defines the outline of the robot base (robot size).
• rolling_window - parameter setting parameter to true select robot_base_frame as a middle of the cost map.
• inflation_radius - determines from what distance from the obstacle, take into account the cost - any distance less than indicated will be included as an additional cost.
• cost_scaling_factor - defines the rate of cost decay with distance. Increasing the value causes the cost at the same distance from the obstacle to have a higher cost value
• track_unknown_space - true value add a third value on the cost map unknown, if the data from the sensor does not cover part of the area e.g. an unknown value will appear around the corner, as long as we do not cross the corner of the wall.
• observation_sources - define properties of used sensor, these are:
• sensor_frame - name of sensor frame
• data_type - type of message published by sensor (supported: PointCloud, PointCloud2, LaserScan)
• topic - name of topic where sensor data is published
• marking - true if sensor can be used to mark area as occupied
• clearing - true if sensor can be used to mark area as clear

### Cost map - global & local parameters​

Next, we configure specific parameters for the local map and separate ones for global maps. The costmap_global.yaml should contains:

~/ros_ws/src/tutorial_pkg/config/costmap_global.yaml
# Reff: http://wiki.ros.org/costmap_2dglobal_costmap:    update_frequency: 2.0    publish_frequency: 1.0    obstacle_range: 5.0    raytrace_range: 5.0    static_map: true    width: 15.0    height: 15.0

And costmap_local.yaml should contains:

~/ros_ws/src/tutorial_pkg/config/costmap_local.yaml
# Reff: http://wiki.ros.org/costmap_2dlocal_costmap:    update_frequency: 5.0    publish_frequency: 2.0    obstacle_range: 2.5    raytrace_range: 2.5    static_map: false        width: 2.5    height: 2.5    resolution: 0.02

The code explained

• local_costmap/global_costmap - specifies a namespace to specify local and global map parameters. By using namespaces, it is possible to combine two files into one.
• update_frequency - defines frequency in Hz for the map to be updated.
• publish_frequency - defines frequency in Hz for the map to be publish display information.
• obstacle_range - determines the maximum range sensor reading that will result in an obstacle being put into the costmap.
• raytrace_range - defines the maximum range in which the map data will be included in the cost map.
• static_map - reads the data transferred from the occupancy map.
• width & height - parameters define size of map (in meters).
• resolution - resolution of the cost map.

How to interprete vizualization

When we run our final project, we will see a cost map. An example look can be seen in the picture below. As you can see, there are several colors on it and each of them has a specific meaning.

• pink means a certain collision
• cyan indicates probable collision
• colors from red to blue indicate the value of the cost. High value red, low value blue.

The cost map should look like this.

## Planner​

The task of the planner is to attempt to reach the target position within the tolerance specified by the user. The planner is based on the cost map and creates a trajectory that goes through the cells with the lowest cost. In node move_base two planners are used: local and global. According to the global one, it is to find the optimal path based on the created global cost matrix. The local planner selects control in such a way as to get as close to the local minimum as possible.

### Global planner​

navfn/NavfnROS will be used as the global planner. This global planner is based on the Dikstra algorithm and is relatively simple, so there is no need to change the parameters. In the planner_global.yaml file, we will only specify the choice of the appropriate solver. For the rest of the solvers, see the nav_core documentation.

~/ros_ws/src/tutorial_pkg/config/planner_global.yaml
# Reff: http://wiki.ros.org/navfnbase_global_planner : navfn/NavfnROS

#### Local planner​

base_local_planner/TrajectoryPlannerROS will be used as the local planner. This local scheduler is based on the Dynamic Window Approach (DWA) algorithm. This is a relatively simple algorithm that first creates random velocity vectors and then simulates which velocity vector will give the best result. This result will be transferred to the wheels. The figure below illustrates the principle of operation of the algorithm:

Configuring this local scheduler is a much more difficult task due to the number of parameters available. planner_local.yaml should contain:

~/ros_ws/src/tutorial_pkg/config/planner_local.yaml
# Reff: http://wiki.ros.org/base_local_plannerbase_local_planner: base_local_planner/TrajectoryPlannerROSTrajectoryPlannerROS:    min_vel_x: 0.1    max_vel_x: 0.3    min_vel_theta: -0.7    max_vel_theta: 0.7    min_in_place_vel_theta: 0.4    escape_vel: -0.1      acc_lim_theta: 1.0    acc_lim_x: 1.0        holonomic_robot: false    xy_goal_tolerance: 0.1    yaw_goal_tolerance: 0.2    meter_scoring: true    path_distance_bias: 20    goal_distance_bias: 15    occdist_scale:  0.01    sim_time: 2.0

The code explained

• TrajectoryPlannerROS - namespace depends on the type of planner.
• min_vel_x - defines minimum linear velocity that will be set by trajectory planner. This should be adjusted to overcome rolling resistance and other forces that may suppress robot from moving.
• max_vel_x - defines maximum linear velocity that will be set by trajectory planner.
• max_vel_theta & min_vel_theta - defines maximum and minimum angular velocity that will be set by trajectory planner.
• min_in_place_vel_theta - defines minimum angular velocity that will be set by trajectory planner. This should be adjusted to overcome rolling resistance and other forces that may suppress robot from moving.
• acc_lim_theta & acc_lim_x - parameters define maximum values of accelerations used by trajectory planner.
• holonomic_robot - defines if robot is holonomic.
• meter_scoring - defines if cost function arguments are expressed in map cells or meters (if true, meters are considered).
• xy_goal_tolerance & yaw_goal_tolerance - parameters define how far from destination it can be considered as reached. Linear tolerance is in meters, angular tolerance is in radians.
• path_distance_bias, goal_distance_bias & occdist_scale - a group of parameters defines how much the aspect related to keeping to the path, following to the local minimum and avoiding objects should affect the selection of the trajectory.
• sim_time - the amount of time to forward-simulate trajectories in seconds

How to interprete vizualization

Once we run our project, we will see 3 paths in RViz. You can see them in the picture below. The global path is marked in blue. The local path is also marked in green, but only the part of it that is on the local map. In red is the local path that the robot is following at the moment.

## Configuration move_base​

The last thing is to configure the move_base node itself. It contains basic information about the node as well as parameters related to recovery mode. So we create the last file move_base.yaml and enter the following parameters into it.

~/ros_ws/src/tutorial_pkg/config/move_base.yaml
# Reff: http://wiki.ros.org/move_baserecovery_behavior_enabled: truecontroller_frequency: 10.0controller_patience: 10.0planner_frequency: 5.0planner_patience: 5.0conservative_reset_dist: 6.0oscillation_timeout: 10.0oscillation_distance: 0.15

The code explained

• recovery_behavior_enabled - enable or disable the move_base recovery behaviors to attempt to clear out space.

• controller_frequency - defines the rate in Hz at which to run the control loop and send velocity commands to the base.

• controller_patience - defines how long the controller will wait in seconds without receiving a valid control before space-clearing operations are performed.

• planner_frequency - defines the rate in Hz at which to run the global planning loop.

• planner_patience - how long the planner will wait in seconds in an attempt to find a valid plan before space-clearing operations are performed.

• conservative_reset_dist - defines the distance away from the robot in meters beyond which obstacles will be cleared from the costmap when attempting to clear space in the map.

• oscillation_timeout - defines how long in seconds to allow for oscillation before executing recovery behaviors.

• oscillation_distance - defines how far in meters the robot must move to be considered not to be oscillating. Moving this far resets the timer.

## Launching path planning node​

Ugh, I know that it was not easy to go through all these parameters, but thanks to this we are getting closer to the most pleasant one, i.e. starting autonomous driving in the robot. To test above configuration you will need to run move_base node along with localization.launch, whitch we created in previous tutorial.

To sum up, you will need to run following nodes:

• ROSbot/Gazebo - turn on the robot, calculate odometry, and publish and subscribe all necessary to move the robot,
• rplidarNode - driver for rpLidar laser scanner,
• amcl - localiztion node,
• map_server - to load the map,
• move_base - trajectory planner,
• rviz - visualization tool.

For the move_base node you will need to specify paths to already created .yaml configuration files. This is shown in the following launch file:

~/ros_ws/src/tutorial_pkg/launch/move_base.launch
<launch>   <arg name="use_gazebo" default="false" />   <arg name="map_name" default="map.yaml"/>   <!-- Localization -->   <include file="$(find tutorial_pkg)/launch/localization.launch"> <arg name="use_gazebo" value="$(arg use_gazebo)" />      <arg name="map_name" default="$(arg map_name)"/> </include> <!-- Path planning --> <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> <rosparam file="$(find tutorial_pkg)/config/move_base.yaml" command="load" />      <rosparam file="$(find tutorial_pkg)/config/costmap_common.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find tutorial_pkg)/config/costmap_common.yaml" command="load" ns="local_costmap" />      <rosparam file="$(find tutorial_pkg)/config/costmap_global.yaml" command="load" /> <rosparam file="$(find tutorial_pkg)/config/costmap_local.yaml" command="load" />      <rosparam file="$(find tutorial_pkg)/config/planner_global.yaml" command="load" /> <rosparam file="$(find tutorial_pkg)/config/planner_local.yaml" command="load" />   </node>   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find tutorial_pkg)/rviz/move_base.rviz"/></launch>  The code explained <rosparam file="$(find tutorial_pkg)/config/move_base.yaml" command="load" />

Load move_base parameters.

<rosparam file="$(find tutorial_pkg)/config/costmap_common.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find tutorial_pkg)/config/costmap_common.yaml" command="load" ns="local_costmap" />

Load common parameters to global and local cost maps with appropriate namespace.

<rosparam file="$(find tutorial_pkg)/config/costmap_global.yaml" command="load" /> <rosparam file="$(find tutorial_pkg)/config/costmap_local.yaml" command="load" /><rosparam file="$(find tutorial_pkg)/config/planner_global.yaml" command="load" /><rosparam file="$(find tutorial_pkg)/config/planner_local.yaml" command="load" />

Load the remaining parameters for: global & local cost map and for: global & local planner.

### Setting goal for trajectory planner​

Before we run move_base.launch, we have configured RViz for you. Download this RViz configuration move_base.rviz. Name the file as move_base.rviz and place it in the tutorial_pkg/rviz directory. Now we are ready to launch our autonomous driving program.

husarion@husarion:~\$
docker compose up -d rosbot ros-master rplidarroslaunch tutorial_pkg move_base.launch

After you launched trajectory planner with all accompanying nodes, your robot still will not be moving anywhere. You have to specify target position first. To set goal position for robot click on 2D nav goal button from Toolbar, then click a place in Visualization window, that will be destination for your robot. Observe as path is generated (you should see a line from your robot pointing to direction) and robot is moving to its destination. It should look like this:

Add an obstacle and watch how the robot copes.

We encourage you to change the parameters and check the result. If you are not sure what parameter are worth to change try some of these: inflation_radius, sim_time, static_map, min_in_place_vel_theta.
Congratulations, all the knowledge gained so far has allowed you to create your own autonomous robot! After completing this tutorial, you should be able to configure the move_base node to plan trajectories for your robot. You should also understand how to interpret the results in RViz. Most importantly, you can set a target target for your robot and it will do the rest! I hope you managed to configure everything correctly despite the hardships. In the next and final tutorial we will use ROSbot to patrol our area.