Semestral work - Frontier-based Exploration

The main goal of the semestral work is to implement high-level exploration node explorer, which autonomously explore an unknown environment and localize barbie doll. The node should regularly publish the occupancy grid to topic /occupancy and the position of the detected barbie to topic /final_barbie_point. These messages will be used for the final evaluation. The exploration node will employ nodes implemented during regular labs (SLAM, planning, object detection). Due to the current situation, the semestral work will be carried out individually (no teams), using a simulator instead of the real robots in the labs.

The template consists of five 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. This should be replaced by your own implementation from regular labs (HW 3: ICP function, possible improvements to the icp_slam_2d node).
• Object detection (barbie_detection), which contains configuration and launch files of the object detection and localization nodes. This node should be replaced by your own implementation from regular labs.
• Exploration (exploration), which contains configuration and launch files of the exploration nodes. Node frontier.py providing services related to finding frontiers, node planner.py providing a path planning service and node path_follower.py responsible for path execution should be replaced by your own implementation from regular labs. Node explorer providing high-level exploration planning and control should be implemented, see following paragraphs for our suggestions.

You can start by replacing icp_slam_2d, frontier.py, planner.py, path_follower.py, detector.py, network.py by the nodes you have implemented during the regular labs. Then you can start implementing your high-level exploration node in explorer..

The node—executable Python script— should be located at exploration/scripts/explorer. An empty template is provided in the student 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 22 points from the two following milestones:

Milestone I (max 7 points): Upload video demonstrating “a reasonable” functionality of your exploration pipeline before 29 April to BRUTE. It is enough to upload only video (no codes are needed for this milestone). The reasonable functionality is that the robot autonomously explores the selected map (regardless of the exploration efficiency or accuracy of barbie detections).

Milestone II (max 15 points): Upload all codes to BRUTE before 2021-05-19 00:00:00. You should upload whole pipeline to the BRUTE system before this deadline. 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).

Evaluation will 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 these two topics:

• /occupancy for the occupancy map, with message type nav_msgs/OccupancyGrid (map frame = “map”, odom frame = “odom”, more specification below)
• /final_barbie_point for the detected positions of the barbie target, message type geometry_msgs/PointStamped

Please, make sure your solution publishes the appropriate data on these topics (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 exploration/launch/exploration.launch file, which is included in 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 points for published occupancy grids and barbie positions, 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_points” = 1/6 * (map_points_30 + map_points_60 + map_points_90 + map_points_120 + map_points_150 + map_points_180) + barbie_points

The number of points from the semestral work will be determined as a saturated linear function of the “final_points” summed overall evaluation maps and starting positions. The exact shape of this function will depend on the performance of all submitted solutions and the performance of teacher's solution. In particular, we first evaluate teacher's solution with temporal handicap of 30sec (i.e. it will start running after 30 seconds from the start) and subtract additional 15.000 points. Everyone who submits a working solution, which achieves lower number of “final_points” than Handicapped Teacher's Solution (HTS), will obtain 4 points. The solutions, which achieve the same or higher number of “final_points” than HTS, will obtain 6 points. Remaining 9 points will be distributed based on the relative performance of the solutions, which were better than HTS. In particular, we will order these solutions according to achieved “final_points”, and express the relative ordering in terms of probability that your solution is better than another solution chosen at random. For example, if we have 100 submitted solutions and your solution was 37th (in terms of achieved “final_points”), your relative performance is 63%. We assign points according to the following table,

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 zero. For publishing an empty map, you obtain -14400 map_points, since the unpublished cells are assumed to be from class “Unknown”. Similarly, you obtain -14400 map_points for publishing arbitrarily large map containing only cells with class “Unknown”. Nevertheless, you can obtain arbitrarily low number of map_points (lower than -14400) for publishing sufficiently large maps, which contains only “Empty” or “Occupied” cells.

You can (but do not have to) publish the position of the barbie doll. Only last three messages from the topic /final_barbie_point, received by the end of the evaluation run, are accepted. You can publish more messages during the run but only the last three are kept. Out of these three messages, the closes to the actual barbie position is taken and the difference between the estimated and actual position is used for barbie_points assignment, as follows. If the difference between published position and ground truth position (i.e. the localization error) is smaller than 50cm, you will obtain 10000 barbie_points. If the difference between published position and ground truth position is between 50-100cm, you will obtain +5000 barbie_points. If the difference between published position and ground truth position is bigger than 100cm, you will obtain -5000 barbie_points. If the position is not published or contains NaNs, you obtain 0 barbie_points.

Please note that we will evaluate performance of the whole system in terms of the published occupancy grids and barbie positions, 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.

• Stop ICP early once no more improvement can be made (“While not done”)
• Surfel map with point-to-plane metric
  • Allow storing normals in/along the map
  • After kNN search, the nearest point on the corresponding plane would be used instead of point itself
  • Absolute orientation would align to these points on plane
  • Keep backward compatibility of absolute_orientation and icp functions
    • e.g. icp(…, metric='point-to-point') default would keep the original semantics
    • for icp(…, metric='point-to-plane')
      • x, y, y_index could have a different semantics, e.g. x[3:6, :] could be normals, or
      • additional 'y_normal' parameter could hold normals
• Try different initial estimates for ICP
  • Use constant velocity motion model
    1. May work very well with a smooth controller ensuring slow velocity changes
  • Use IMU orientation with constant velocity motion model
  • Keep odom→base_footprint TF as is: ignore it as input, but consider it when outputing map→odom TF: T_r2m = T_o2m * T_r2o
  • IMU message (excerpt):

                    orientation: 
                      x: -0.0011246201996952122
                      y: -0.003690314694296936
                      z: 0.29388865004485826
                      w: -0.9558318775714033
                    angular_velocity: 
                      x: -0.00011878946665939603
                      y: 4.463900499701507e-05
                      z: 0.0007740670965644818

• Use two occupancy thresholds b_empty and b_occupied
  • Remove points with $b_i < b_\mathrm{empty}$
  • In localization, consider only points with occupancy $b_i \geq b_\mathrm{occupied}$
• Try different methods to filter out noise in the point map
  • Assess occupied/empty points based on signed distance from reconstructed surface
  • Reconstruct surface from input measurements (e.g., line segments from pair of points in 2D, polygons in 3D) and compare map points with possibly occluding line segments
  • Similar but comparing measured point with surfels in the map: a surfel may need to filtered out if it occludes a measured point

• 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.
• Consider adjusting the heading of the robot at the end of the path when it is beneficial.

• We suggest you implement a detection aggregator, which will subscribe all the detections from the topic /barbie_point from your detector node and publish only the most probable barbie position to the topic /final_barbie_point.

• 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. weights_path = rospy.get_param('~weights_path', 'trained_weights')) - 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).