====== Lab 04 ======

During this lab, you should learn how to work with factorgraph-based localization and how to run it in the simulator.


===== Additional ROS Concept - Simulation Time =====

An important concept in ROS is the so-called simulation time. On real robots, time always advances according to the rules of universe. However, in simulation, you can (or have to) slow down or speed up the pace of time. Slowing down is useful if you have a compute-intensive node and you want to test how it works, but you don't yet have resources for refactoring the node into a faster realtime-capable version. Sometimes you also have to slow down - e.g. if you simulate exteroceptive sensors (lidars, cameras), it often happens that the computer is not powerful enough to render all the sensor data in realtime.

Working with simulation time in ROS is easy - just be sure to never use ''time.time()'' in your code, and that you always use ''rospy.Time.now()''. That's it. ROS automatically manages the time internals so that on real robots, you get the system clock time (what ''time.time()'' would give), but in simulation, you will get the properly slowed-down time. All the other time-related features like ''rospy.Rate'' or ''rospy.Timer'' work with this simulation time.

Internally, ROS decides between realtime clock and simulation time based on the value of global parameter ''/use_sim_time''. It set to True, every node creates a subscriber to topic ''/clock'' and updates the simulation time based on whatever is published to this topic. If the parameter is False, it automatically uses system clock. The Gazebo simulator is a publisher of the ''/clock'' topic and publishes by default every 1 ms. So that is the clock resolution you get when running from simulation. The ''real_time_factor'' argument of the ''aro_sim'' launch files allows you to slow down the simulation. By default, it runs with RTF 1.0.

For example:

  roslaunch aro_sim sim_with_markers.launch real_time_factor:=0.3

It is also usually a good practice to use simulation time when replaying bag files. Just set ''/use_sim_time'' to true before playing the bag file, and add ''%%--clock%%'' option to ''rosbag play''. The play node will then publish the ''/clock'' topic on 1 kHz. If you then use the ''%%--rate%%'' parameter, all nodes listening to the messages from bag file will be properly slowed down.


===== Apriltags =====

<imgcaption apriltags|Example of Apriltags. Image courtesy of https://april.eecs.umich.edu/software/apriltag.>{{:courses:aro:tutorials:tagformats_web.png?400|}}</imgcaption>



Apriltags are visual markers designed specifically so that it is easy to estimate their pose in full 6 DOF (x, y, z, roll, pitch, yaw). Thus they are very comfortable for being used as absolute localization markers.

Here is an example of a tag localized by the ROS node from package ''apriltag_ros'':

{{:courses:aro:tutorials:rviz_screenshot_2023_03_13-13_55_19.png?400|}}

To play with it yourself, you can get it running by launching these two commands:

  roslaunch aro_sim sim_with_markers.launch  # terminal 1
  roslaunch aro_localization apriltags_rgb.launch camera_name:=rgb  # terminal 2

The launcher ''sim_with_markers.launch'' knows several worlds (given as argument ''world''), and it also knows several positions of the markers through which you can cycle (argument ''marker_config'').

===== Factorgraph in action =====

https://www.youtube.com/live/YCE1Aj0k1UA?feature=share&t=2045
===== Lab task =====

During the lab, we will derive and compute practical tasks on some factorgraphs. We will also review the localization theory from the lectures.

Start with downloading {{ :courses:aro:tutorials:hw_03_pkgs.zip |}}. Add the folders to your workspace (overriding the aro_sim package). Then ''catkin build'' and ''source devel/setup.bash'' as usual. If ''catkin build'' results in an error, try ''catkin clean'' and then ''catkin build'' again. This may fix your problem.

You can play with the factorgraph without ROS by calling

  python3 src/aro_localization/src/aro_localization/factor_graph.py

Running it with the simulator and full ROS integration is then done as:

  roslaunch --sigint-timeout 1 --sigterm-timeout 1 aro_localization sim.launch fuse_icp_slam:=false

or

  roslaunch --sigint-timeout 1 --sigterm-timeout 1 aro_localization sim.launch keyboard:=true fuse_icp_slam:=false  # enable keyboard control of robot
===== Homework 3 assignment =====

Follow the assignment of the homework [[courses:aro:tutorials:homework03|HW3]].