Semestral work - Frontier-based April-tag localization

The goal of the semestral work is to implement high-level exploration node explorer.py, which will utilize algorithms implemented during the semester to explore an unknown maze and localize a hidden Apriltag. The exploration node will employ nodes implemented during regular labs (factorgraph localization, ICP SLAM, frontier detection, planning and path following). The simulation semestral work will be carried out individually (no teams). Teams of up to 4 will be allowed for the second part.

The goal of the algorithm is to first localize the robot relative to the given absolute marker with known position, and then explore the world, find the relative marker, localize it and return to the starting position.

Details about what Apriltags are and how they work are given in Lab 05.

You can start implementing your high-level exploration node in aro_exploration/nodes/exploration/explorer.py.

A template script is provided in the 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 explorer.py — overview of a possible implementation

1. Pick a frontier, if any, or a random traversable 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.
6. Check for localization of the goal Apriltag ◃ aro_localization.py (pick any ID ≠ 7)
7. Repeat from line 1 if Apriltag not localized / return to beginning if localized.

You can obtain 15 points from the two following milestones:

Milestone I (max 10 points): Upload all codes to BRUTE before deadline. Later uploads are penalized 1 pt/day. Use the provided submission script create_sw01_package.sh as with previous homeworks. The evaluation will be based on running the run_eval.py node (see “Evaluation details” section for details) ⇒ please make sure that:

• running roslaunch aro_exploration aro_exploration_sim.launch runs everything what is needed on your side.
• the roslaunch aro_exploration run_eval.launch doesn't fail.
• 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 have ever been uploaded to BRUTE (including other courses and years).

Milestone II (max 5 points): Transfer and fine-tune your code on real Turtlebots. Demonstrate exploration and tag localization functionality: robot should successfully localize the tag (2 pt, mandatory) and return to starting position (2 pt) without a collision with the arena (1 pt). Time for the demonstrations will be during labs 12 and 13. You will be given access to the lab with the real robots earlier so that you can tune the solutions during the semester.

Evaluation will be performed by an automatic evaluation script, which will run the simulation for at most 120 seconds and then stop the simulation. A simplified version is provided for local testing with simple testing maps. The evaluation script listens to topic /relative_marker_pose for the position of the relative marker (message type geometry_msgs/PoseStamped). Evaluation node also monitors ground truth of robot position to determine if it returned to the starting position.

Please, make sure your solution publishes the appropriate data on this topic. Otherwise, your solution might not be accepted!

To test your solution, you can run the run_eval.launch file. Use the argument world to change the map (e.g. “world:=aro_eval_1”) and argument marker_config to change placement of the markers. The launch file starts the robot simulation and outputs the awarded points. The results are also stored into the folder ~/aro_evaluation. 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_exploration run_eval.launch world:=aro_eval_1 marker_config:=2

This launch file internally starts a different launch file with these parameters:

roslaunch aro_exploration aro_exploration_sim.launch world:=$WORLD marker_config:=$MARKER_CONFIG ground_truth:=false mr_use_gt:=false tf_metrics:=false rviz:=false gui:=false localization_visualize:=false joy_teleop:=false run_mode:=eval

The latter one might be more suitable for local debugging. But if you want to test your code as in Brute, run that command exactly as is.

The number of points from the semestral work will be determined by number of successful tag localizations (up to 1pt) and returns of the robot to starting position (up to 1pt). Fifteen simulation experiments with randomized tag locations will be performed for each submission to determine the final score.

Successful localization and return to home are each awarded 1 pt if the pose is less than 0.25 meter from the ground truth position. The awarded points then linearly decrease to the distance of 1 meter. Only the x,y position of the robot/marker is evaluated, not its orientation. If the marker was not localized, no points will be awarded for robot position.

The evaluation will be done on worlds similar to aro_easy_* or aro_medium_*. The worlds will be simple maze-like corridors.

The evaluation is run 3 times on each world and the result is averaged per world:

* This world is not publicly available.

The last 2 labs of the semester are reserved for demonstration of your code on real robots. You can work in teams of up to 4.

You should not be directly using any 'aro_sim' configuration/script/launch file on the robot. Your pipeline execution on the real robot should be performed from aro_exploration/launch/exploration/aro_exploration_real.launch.

DO NOT launch any _sim.launch file on the real robot or your notebook connected to it.

See TurtleBot Lab Guide for details describing how to work with the real robots.

Your team will be awarded points according to the following rules:

• 1 pt if the robot drives so that it can actually see the relative marker and looks in its direction (mandatory)
• 1 pt if the robot localizes the relative marker with 1 m or better accuracy
• 2 pt if the robot successfully returns to the starting position (with 0.5 m tolerance)
• 1 pt if all the previous were performed without a collision with the arena

You have multiple tries when showing your solution to a teacher, but each teacher has a (possibly unknown) limit of tries per team, so try not overshooting this limit.

Please note that we will evaluate performance of the whole system in terms of the localization accuracy, 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 localization of the markers and the robot.
• 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 factorgraph 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.

• Tune the costs of the individual measurement sources to play nicely together. Please note that exaggerating some of the costs will have very bad effects on the optimization.
• Tune the optimizer parameters, such as number of iterations and loss function.
• Try to detect adverse control (fast velocity or acceleration) or bumping into obstacles and decrease accordingly the costs of the related measurements.
• Try to avoid the adverse control or bumping into obstacles.
• If the optimization is too slow, you can test with lower real_time_factor to see whether the problem is in the localization itself or just its speed.
• Look at the Apriltag detections and implement an even better filter of the false positive ones.
• If you find the relative marker and return to the start and there is still a lot of time, try to plan a direct path to the relative marker and drive there again - this will help make the localization more accurate.
• If you struggle too much with integrating the factorgraph localization with the rest of the pipeline, use the ICP SLAM odometry for frontier exploration and control and only use the factorgraph as a means to localize the marker.

• 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.

• Utilize the 'visible_occupancy' topic to include unseen wall segments to the frontier search space. Utilize robot orientation to cover wall segments during exploration.
• Consider different heuristics for frontier selection such as the number of unknown voxels in frontier's neighborhood, or the physical distance which has to be traveled.

• 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 un-traversable 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.

• Common problems using Singularity with ROS.
• Real robot troubleshooting.
• It works for me locally, but Brute doesn't like it.
  • Check How to start for the exact commands Brute uses to test your work.
  • Carefully inspect the console logs that Brute provides to you and search for any exceptions or other error outputs. Please note that because of buffering, the logs in Brute are not exactly sequentially aligned, so that a print that happened later can appear before some print that happened earlier in a different node. You can safely ignore any exceptions like “master shutdown”, “publishing to closed topic” etc. These are normal exceptions seen when your script is shut down.
  • If you still get different results, try checking out deploy/workspace into a new directory and just unpack the tar.gz file you are submitting to Brute over the downloaded workspace. Then run the test commands from this new workspace.
• I don't get any results in Brute. I wait 20 minutes and the evaluation is still not available.
  • In this case, the console logs of your submission were so large that Brute could not store them and thus cannot show them to you. The limit is around 5-10 MB. You can help this case by reducing the number of logged text.
  • It can especially happen if you get an exception with every incoming lidar scan/image. These exceptions are written to the console logs.
• No map is displayed in rviz - Try checking whether icp_slam and aro_localization nodes work okay and publish tf transforms. Publishing map-to-odom transforms is their job. In the process it also calls your icp function, so make sure it's working too. If not, try to debug/fix it.
• 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).
• The “numpy has no attribute float” error from ros_numpy package - Clone the https://github.com/qboticslabs/ros_numpy repository to your src directory or downgrade numpy to pre-1.24.