Path planning

Robot path planning is a fundamental part of the motion of mobile robots: robots move following trajectories that drive the from one place to another. Therefore, when operating autonomously, a robot must select safe and optimal trajectories to navigate on its environment. Later, the navigation system must transform the desired trajectory in a sequence of velocities that can drive the robot following such path.

The optimality of the trajectory can respond to many factors: distance to be traveled, arrival time, or risk of collisions. The physical embodiment of the robot difficult the computation of optimal paths to reach a specific location from another—indeed, the kinematics restrictions of the robot must be taken into account in the path planning process.

In the simplest case, one might consider an off-line planning of the motion of the robot. Starting from a known configuration of the world, it is possible to pre-program a set of velocities that can drive a robot following a set of waypoints that connect the beginning and end of a desired path.

Objective

The objective of this practical session is to design the control software that can drive a mobile robot visiting pre-defined waypoints on its environment.

General remarks

In this practical session you will fist plan an optimal path for driving the robot across multiple waypoints on its workspace, and later, you will implement the control software that can drive the robot following such path.

You can enter the directory of this practical session by entering the commands

cd ~/catkin_ws/src/path_planning/scripts

There you will find the file path_planning_node.py—the script that you will use develop the control software for the robot.

IMPORTANT: 1 - Set the properties of the file for being executable.

chmod +x path_planning_node.py

2 - Execute this extra command to patch the script to run in Ubuntu 20.04

sed -i 's/python/python3/g' path_planning_node.py

3 - Add the following lines into the script path_planning_node.py. They must be placed with the rest of pre-defined global variables.

# Pre-defined global variables

_model_index = 0       # New line to be added
_model_found = False   # New line to be added
_truth_pose = Pose2D()
_cmd_vel = Twist()

4 - In the script path_planning_node.py, replace the following line in the while loop of the main function

# pub_odom.publish(_cmd_vel)  # Line must be removed
pub_vel.publish(_cmd_vel)     # Line must be added

By mistake, they are missing in the script that you downloaded.

Exercise

In this exercise, you must find the pose of the waypoints that the robot must visit, and afterwards, you must program a set of velocities that can drive the robot to the waypoints. You can get the pose of the waypoints using the pose provided by the simulator, as you did in the practical session P1.2.

Launching the experiment

1 - Open a terminal and enter the directory that contains the materials for this practical session.

cd ~/catkin_ws/src/path_planning/scripts

2 - Run the simulation in Gazebo.

roslaunch path_planning path_planning.launch

Experimental setup

At the beginning of the experiment, the Summit-XL is spawned in an scenario cluttered with construction cones. The initial pose of the robot is at the coordinates (x,y,z) (0,0,0), with an orientation theta of 0.0°. The cones have been arranged to form small alleys. The scenario for this practical session is shown below.

Path planning scneario

There are 11 waypoints that are marked by red arrows.

Planning the path

In this exercise you must plan a path that drives the robot into as many alleys as possible, and in the shortest time. To do so, you must select an optimal trajectory for the robot, that must be collision free too.

You are free to select the method you want to use to plan the path for the robot.

Developing the control software

In the simples approach, you can design the control software for the robot in open loop. In this navigation method, you first drive the robot manually into the waypoints, meanwhile you register the velocity of the robot at each time step, and later, you try to preproduce the movements of the robot by setting those velocities.

In a more smart approach, you can design the control software in closed loop. As you learned in the practical session P1.2, the robot can get its pose from the information provided by the simulator—like a GPS. Likewise the open loop method, you must register first the position of the waypoints in the scenario. After, you can design a control strategy that drives the robot into the waypoints by considering its current position.

While registering the pose of the waypoints, you can move the robot using the Linux terminal or the interface you developed in the practical session,P1.0.

Testing your control software

You can test your implementation by running the script with the command

rosrun path_planning path_planning_node.py

If no error is reported, the robot will move according to the velocities you set. If you wish to try something else, click on the terminal running the teleoperation interface and press Ctrl + C until the process stops.

If you wish to check the velocity that you are sending to the robot, open a new terminal and enter the command

rostopic echo /robot/cmd_vel/

You can also get the pose of the robot with the command

rostopic echo /robot/mapping/truth_pose

To be considered

Consider the advantages and drawbacks of the open-loop and closed-loop strategy. Which do you think performs the best? Which is easier to implement?

Proposed solution

A proposed solution for the exercise will be available to download after midnight.

Objective

General remarks

By mistake, they are missing in the script that you downloaded.

Exercise

Further readings