Your task in this homework is to implement algorithms for occupancy grid exploration, path planning and path following. You are given nodes that publish an existing occupancy grid and that subscribe to robot movement control in simplified simulation. A launch file and several scripts are provided to test your implementations, and interfaces of your implementations should be compatible with the Semestral work. The template files can be downloaded through link template_files. The detailed instructions are provided in Instructions section at the end of this page.

Control logic which connects all the parts together is executed by the node explorer implemented in the script exploration.py. The node should handle communication with other parts of your semestral work, and exploration of the environment by coordinating data transfer between subordinate ROS nodes.

The implementation is up to you. It is needed to connect all the parts of this homework working together.

Detection of frontier will be done by node frontier in frontier.py. Two services should be implemented:

• Return random frontier
• Return closest frontier

Is a BFS algorith to search for frontiers.

• outer BFS - search for frontier points connected to the robot position
• inner BFS - “frontier assembly”, search for contiguous frontier points

Make sure to filter the frontiers, i.e. make sure they are reachable. You can test that with path planning, or run the WFD on a map with inflated obstacles. Selecting random frontier will probably work in some finite time. Better approach is to select the closest frontier, either by distance (Euclidean, Manhattan) or path distance.

You can find pseudocode and more information in [1].

Let's rehearse:

• Path - an ordered set of points
• Trajectory - Path plus velocities, accelerations etc. at each point

We will not concert ourselves with trajectory for now, just the path. Planning will be performed on a 2D occupancy grid.

Grid is then represented as an un-oriented graph. Some well known graph search algorithms are BFS, DFS, Dikstra and A*. Pick the one that will give you minimal path length.

Paths close to the obstacles are infeasible, robot would collide. Robot diameter is also given in the task, see the launch file example.launch. Solution is to add a safety margin, inflate obstacles in the grid. Use scipy, either approach will work:

https://docs.scipy.org/doc/scipy/reference/generated/scipy.ndimage.binary_dilation.html

https://docs.scipy.org/doc/scipy/reference/generated/scipy.ndimage.grey_dilation.html

Path planning in the occupancy grid will be implemented by node planner in planner.py. Implement a service which takes origin and goal poses as input and returns array of poses, as is given. Mind that planner should be aware of occupancy grid, which will change during the future map exploration.

Defined in: nav_msgs/OccupancyGrid

data (int[8])

• 0 == empty cell
• 1 .. 100 == (probably) occupied cell
• -1 == unknown (unseen) cell

info (nav_msgs/MapMetaData)

• resolution (float32) - “size” of cell in meters
• width - width of grid in cells
• height - height of grid in cells
• origin (geometry_msgs/Pose) - relation of origin [0,0] of the grid to the real world (e.g. map tf)

Robot position needs to be transformed to fit into the grid - translation and scaling.

Although the path planner produces a safe collision-free path leading the robot towards the goal, it is only a sequence of waypoints that does not explicitly says how the path should be followed. To navigate the robot through the environment, the path has to be transformed in the sequence of actuators commands leading to following the path towards the goal. In case of the robot used within this assignment, it means a sequence of commands consisting of forward linear velocity and the robot's angular velocity. The most straight forward approach to path following is the “turn and move” strategy that consists of a sequence of turns with a zero forward velocity and a sequence of moves forwards with a zero angular velocity. However, due to frequent stops this approach results in a very slow path following.

The more effective approach, which is often used for differential wheel robots, is a “pure pursuit law” that is often called the “carrot following” approach. For simplicity, we will assume that the forward linear velocity of the robot is constant for the whole path, and we are searching only for the angular velocity commands. In this case, the pure pursuit law consists of two steps:

1. Find a position $P_d$ on the path in a look-ahead distance $d_l$ from the robot's current position.
2. Compute desired angular velocity $\omega$ that given the constant forward velocity $v$ results in reaching position $P_d$.

The curve $k$ that leads the robot to desired position $P_d$ is part of the circle with diameter $R$. From the presented scheme, we can derive that $R$ can be computed based on equation $$ R = \frac{{d_l}^2}{2\Delta y}. $$ With a given constant velocity $v$ and computed radius $R$, the desired angular velocity $\omega$ that will result in a following of curve $k$ is given by $$ \omega = \frac{v}{R}. $$

Another approach for path following (again assuming constant forward velocity) is applying PID (P, PI, PD) controller for orientation/heading control. The reference heading for this controller is again computed based on the position of the look-ahead point and current position of the robot. The control error is then given as a deviation from this reference heading, and the control input is computed based on equation

$$ \omega_n = k_p e_n + k_I \sum_{i=1}^n (e_n t_s) + k_d \frac{1}{t_s} (e_n - e_{n-1}), $$ where $e_n$ is the control error at step $n$, $t_s$ is a control period, and $k_p$, $k_I$ and $k_d$ are tuning coefficients. Note that the pure pursuit controller is a variant of proportional controller.

Your task will be to implement and tune a path following approach using the pure pursuit controller or PID controller (2 points). For your implementation you can use provided template path_follower.py or start with your own implementation. The uploaded solution has to subscribe the path represented by nav_msgs/Path message on topic /path and publish velocity commands for robot on topic /cmd_vel (type: geometry_msgs/Twist). The current pose of the robot in a map can be obtained from transformations on topic /tf (an example provided in path_follower.py). Your commands has to lead to safe path-following that will not result in a collision with obstacles. If the path_follower receives new path, the following of the previous one is interrupted and a new path is followed. If the new path is empty, the robot should stop. The solution will be tested on paths generated in a map with a minimum path-obstacle distance 0.4 m and robot diameter equal to 0.2 m.

Possible improvements and hints:

• adjust velocity based on the required angle to turn
• beware that the following of the path is usually not precise, and you have to consider possible deviations from a planned path while setting the obstacle growing parameter

Enter Singularity shell:

singularity shell --nv path/to/robolab_melodic.simg

Configure workspace:

ws=~/workspace/aro
mkdir -p "${ws}/src"
cd "${ws}/src"
curl -sSL https://cw.fel.cvut.cz/wiki/_media/courses/aro/tutorials/aro_hw_04.zip -O
unzip *.zip
cd "${ws}"
catkin init
catkin config --extend /opt/ros/aro
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build -c

Packages contained in the zip are dummy_grid and exploration. You can implement your homework using the templates provided in the exploration package. The dummy_grid package is used as a substitute for running Gazebo simulation and can help you visualize the task and debug your code.

The dummy_grid package contains a launch file, which will visualize the occupancy grid, robot pose, planned and travelled path using Rviz. You can also manually set the robot pose by entering $x$, $y$ coordinates in the dialog window.

The dummy_grid/scripts folder contains some additional ROS nodes which can help you with testing your code. Similar scripts will be used in automatic evaluation, so try experimenting with it.

Please submit the whole exploration package directory in a single .zip file. Scripts will be evaluated using dummy_grid simulation. Robot diameter will be kept the same during our testing. 2 points will be awarded for planner.py, 2 for frontier.py and 2 for path_follower.py. 1 point will be awarded for submitting a working exploration.py which connects them together and navigates the robot to closest frontier in a simple maze.

In template files planner.py and frontier.py, there is a mistake in frame name “base_footprint”, which should be renamed to “robot”. While not needed in planner.py, it is useful in the frontier.py. Issue has been corrected in recent templates.

[1] TOPIWALA, Anirudh; INANI, Pranav; KATHPAL, Abhishek. Frontier Based Exploration for Autonomous Robot. arXiv preprint arXiv:1806.03581, 2018.