Semestral work - Frontier-based Exploration

The main goal of the semestral work is to implement high-level exploration node explorer in aro_exploration package, which autonomously explore an unknown environment. The published occupancy grid on topic /occupancy will be used for the final evaluation by comparison with ground truth data. The exploration node will employ nodes implemented during regular labs (SLAM, frontier, planning, following). The simulation semestral work will be carried out individually (no teams). Teams of up to 4 will be allowed for the second part.

Download aro_semestral.zip containing exploration package with pre-configured launch file, simulation package with a minor fix and an example evaluation package. Place it in a common workspace with packages implemented for previous homeworks. Your task is to implement ROS communication between packages in the exploration node.

Your workspace should consist of several packages:

• Simulator (aro_sim), which contains configuration and launch files of the simulated environment. This package should stay as provided. Any solution tampering with the simulation environment during evaluation will be denoted as unacceptable. Notice that there are few changes in the sensor configuration from the defaults provided by turtlebot3 packages: (i) the source for the odometry are wheel encoders instead of ground-truth pose, therefore the odometry must be corrected by the ICP SLAM node, otherwise the resulting localization of the robot would be completely inaccurate.(ii) The lidar noise level is increased and the lidar range is accordingly adjusted.
• Evaluathor (evaluathor), which contains evaluation node. This package should stay as provided.
• Localization and mapping (aro_slam), which contains implementation of the ICP-based SLAM with configuration and launch files for testing. This should be replaced by your own implementation from regular labs (HW 3: ICP function, possible improvements to the icp_slam node).
• Exploration (aro_explorer), which contains configuration and launch files of the exploration nodes.
• Frontier detection (aro_frontier) providing services related to finding frontiers, n
• Path planning (aro_planner) providing a path planning service.
• Path following (aro_control) responsible for path execution.

You can start by adding the implementations you have worked on during the regular labs to a single workspace. Then you can start implementing your high-level exploration node in aro_explorer.

The node—executable Python script— should be located at aro_exploration/scripts/explorer. An empty template is provided package. Algorithm below provides an overview of a possible implementation. An exploration experiment with such a node is shown in the video above.

Exploration node—overview of a possible implementation

1. Pick a frontier, if any, or a random position as a goal. ◃ frontier.py
2. Plan a path to the goal, or go to line 1 if there is no path. ◃ planner.py
3. Delegate path execution to low-level control. ◃ path_follower.py
4. Monitor execution of the plan.
5. Invoke a recovery behavior if needed. ◃ path_follower.py
6. Repeat from line 1.

You can obtain 20 points from the two following milestones:

Milestone I (max 10 points): Upload all codes to BRUTE before 2022-05-02 00:00:00. You should upload your whole pipeline to the BRUTE system before this deadline ie. all packages from the workspace in a single zip file. The evaluation will be based on running the Evaluathor node on our own maps (see “Evaluation details” section for details) ⇒ please make sure that:

• running the evaluathor launchfile runs everything what is needed on your side.
• focus on implementing functionalities which generalize well rather than tuning something for a single map.
• avoid sharing your codes with your colleagues, since all uploaded codes will be checked by global plagiat detection system. The system compares your codes with any other codes which has ever been uploaded to BRUTE (including other courses and years).

Milestone II (max 10 points): Transfer and fine-tune your code on real turtle-bots. Demonstrate exploration functionality, robot should successfully explore the testing area. Points will be assigned to working solutions.

Evaluation will also be performed by an automatic evaluation script, which will run the simulation for 180 seconds and then it will stop the simulation. A simplified version of it is provided in the aro_evaluathor package. The evaluation script launches the aro_sim/launch/turtlebot3.launch file and then listens to /occupancy topic for the occupancy map, with message type nav_msgs/OccupancyGrid (map frame = “map”, odom frame = “odom”, more specification below).

Please, make sure your solution publishes the appropriate data on this topic (more details below). Otherwise, your solution might not be accepted! Additionally, make sure your entire solution is started via the turtlebot3.launch file. Do not modify this file! The entire aro_sim package will be replaced with a “clean” one. Any modifications you perform in the aro_sim package will be lost! If you need to launch additional nodes or otherwise modify the launch procedure, you should modify the aro_exploration/launch/exploration.launch file in your exploration package, which is then included from the turtlebot3.launch file.

To test your solution, you can run the aro_evaluathor/launch/eval_world.launch file. Use the argument world to change the map (e.g. “world:=aro_maze_8” or “world:=brick_maze_1”). The launch file starts the robot simulation and opens an OpenCV window where you can see the evaluation process (score and map to ground truth comparison). The results is also stored into the folder ~/aro_evaluation as a table and a video. Make sure your solution can be tested by the evaluation script without any errors before submission!

Example of how to start the evaluation: roslaunch aro_evaluathor eval_world.launch world:=aro_maze_8

You can get map points for published occupancy grids, see paragraphs below for detailed description of the point assignment procedure. In order to capture the temporal progress of your exploration, we will evaluate published maps in the uniformly distributed temporal intervals: 30sec, 60sec, 90sec, 120sec, 150sec, 180sec, and estimate:
“final_map_points” = 1/6 * (map_points_30 + map_points_60 + map_points_90 + map_points_120 + map_points_150 + map_points_180)

The number of points from the semestral work will be determined as a function of the “final_map_points” for each evaluation run. We assign points according to the following table from percentage of maximum ground truth score for each evaluation run. Final performance used to determine points from semestral work is an average performance over all evaluation runs:

The evaluation will be done on worlds similar to aro_maze_8. They won’t contain open areas as in house. The worlds will be maze-like with long corridors and thick walls. Only the burger robot will be used in evaluation.

The occupancy grid consists of 5x5cm cells. Each cell is classified into one out of three classes based on its occupancy confidence value in the int8[] data field: “Empty” (with occupancy confidence <0,25)), “Occupied” (occupancy confidence <25,100>) and “Unknown” (occupancy confidence -1). The part of the scene, which has not been covered by the published occupancy grid is considered to be “Unknown”. Given a ground truth occupancy grid and a published occupancy grid, the number of map_points will be determined according to the following table:

Consider the following example: the 6x6m world, where all cells are observable (i.e. their ground truth class is either “Empty” or “Occupied”). Since the resolution of the occupancy grid is 5cm, the ground truth map will consists of 14400 cells which are either “Empty” or “Occupied”, anything else is “Unknown”. The maximum number of map_points, which you can obtain for publishing the map identical to the ground truth map is given by number of observable obstacles and size of empty space. For publishing an empty map, you obtain 0 map_points, since the unpublished cells are assumed to be from class “Unknown”. Similarly, you obtain 0 map_points for publishing arbitrarily large map containing only cells with class “Unknown”.

Please note that we will evaluate performance of the whole system in terms of the published occupancy, so the nodes must not only work individually but also work well with other nodes to fulfill their role in the whole system. Things to consider:

• Inaccurate localization will result in distorted maps and wrong cells being compared to the ground truth during evaluation.
• Slow localization will have a negative impact on low-level motion control. Low-level motion control can be adjusted as well if needed.
• As all experiments are run in simulator, possible recovery behaviors can be quite aggressive. Nevertheless, if the maneuvers are too aggressive and robot is hitting obstacles, it will adversely affect odometry and initial pose estimates for ICP.
• Choosing inappropriate goals and visiting already covered areas repeatedly will slow down exploration.
• Having no recovery or fallback behaviors can lead the system to halt in the very beginning.
• Consider selecting important parameters such as max robot speed or obstacle margin and tune the pipeline as a black-box (e.g. random search, grid search, or CMA-ES).
• A general advice is to focus on performance bottlenecks.

• Due to different noise characteristics in the virtual and real environment, choose optimal configuration separately for each. The parameters found in the evaluation done as a part of homework 3 should give you a good starting point. You can try experimenting with the following parameters:
  • alignment: frame-to-frame / frame-to-map,
  • loss: point-to-point / point-to-plane,
  • descriptor: position / position and normal,
  • odometry: with / without (odometry should be used on the real robot).
• High accelerations and fast maneuvers may reduce localization accuracy, especially on the real robots. Try limit the acceleration or the maximum velocity in the control node if that seems to be the problem.

• Consider different heuristics for frontier selection such as the number of unknown voxels in frontier's neighbourhood, the distance which has to be travelled.

• Apply path straightening for obtaining shorter paths with a lower number of turns.
• Be aware that due to imprecision in path following, localization and mapping, the robot can appear in occupied space although the path leads exclusively through the unoccupied space. Make use of the fact that there is usually no untraversable obstacle at the robot's current position.

• Consider dynamical adjustment of the forward velocity based on turning angle.
• Consider dynamical adjustment of the forward velocity based on the distance to obstacles.
• Improve path following by providing paths with a lower number of turns.
• Adjust the limits on control inputs to avoid negative effects on the precision of localization and mapping.
• Try applying turning at a spot with zero forward velocity if the difference between current and desired orientation is too high.

• No map is displayed in rviz - Try checking whether icp_slam_2d works okay and publishes tf transforms, publishing map-to-odom transforms is its job. In the process it also calls your icp function, so make sure it's working too. If not, try to debug/fix it.
• changing a parameter value in the code has no effect (e.g. robotDiameter = rospy.get_param('~robot_diameter', 0.8)) - This is likely because the parameter is already defined in a launchfile. The second argument of the get_param function is used only if the parameter has not already been defined. Try looking up the parameter in the turtlebot3.launch or exploration.launch files (or other launch files, if you use them). Use set_param if you want to change a parameter value from the code.
• frame error(s) In RViz, you can get “Frame does not exist” error. This happens because the requested frame (e.g., the one set as a global fixed frame) was not published, yet. For example, the node publishing the occupancy grid, did not publish the necessary transform, yet. In case of other frame errors, check that the names of the frames in both the messages (i.e. <some_msg>/header/frame_id) and transforms, are correct.
• spawn error during launch Sometimes, when starting the evaluation pipeline, the URDF model spawning node can die. Simply try restarting it again.
• occupancy map changes size and causes crash The size of the occupancy map can (and most likely will) change during mapping. You should always look at the map metadata contained within the occupancy grid message (upon reception of every message).