Lab 04

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

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.

Fig. 1: Example of Apriltags. Image courtesy of https://april.eecs.umich.edu/software/apriltag.

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:

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

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

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

Follow the assignment of the homework HW3.