Deleting layers from world and loading new layers instead

[Ducanor]

Hi everyone,
im currently working on the simulation_manager.cpp. My goal is to load new layers from bitmap png files on every episode while training and deleting the old layers. For this purpose i wrote a ros::Subscriber inside the simulation_manager.cpp and defined a callback function.

ros::Subscriber goal_sub = n.subscribe("/goal", 1, &SimulationManager::callback_goal, this);

void SimulationManager::callback_goal(geometry_msgs::PoseStamped goal_msg) {
    YamlReader world_reader = YamlReader(map_layer_yaml_file_);
    YamlReader layers_reader = world_reader.Subnode("layers", YamlReader::LIST);
    for (auto &layer : world_->layers_) { // delete layers
      if (layer->body_ != nullptr) {
        layer->body_->physics_body_ = nullptr;
      }
      delete layer;
    }

    world_->cfr_.layer_id_table_.clear(); // empty the layer_id_table

    world_->LoadLayers(layers_reader); // load "map" layer from png file
    world_->DebugVisualize();
  }

During roslaunch i start the simulation with an empty map

properties:
  velocity_iterations: 10
  position_iterations: 10
layers:
- name: static
  map: empty.yaml
  color: [0, 1, 0, 1]

and afterwards use world_->LoadLayers(layers_reader); to load the real map for training via this yaml file:

properties:
  velocity_iterations: 10
  position_iterations: 10
layers:
- name: map
  map: map.yaml
  color: [0, 0, 1, 1]

This works for the first episode of training and the visualization works as well, but flatland server process dies after the first episode.

[flatland_server-2] process has died [pid 9515, exit code -11, cmd /home/ducanor/catkin_ws/devel/lib/flatland_server/flatland_server __name:=flatland_server __log:=/home/ducanor/.ros/log/978f8eb0-d9aa-11eb-8ae5-00155d6d5b70/flatland_server-2.log].
log file: /home/ducanor/.ros/log/978f8eb0-d9aa-11eb-8ae5-00155d6d5b70/flatland_server-2*.log

My guess is that there is a problem with the deleting during the callback. Maybe "map" layer is not deleted properly and this causes the process to die.

P.S.: I tried without deleting, but this causes the process to die after loading layers the maximum amount of times (16). The layers will not be overwritten.

[josephduchesne]

The code as-is isn't really designed to handle dynamic map loading like that, although there's no reason why it can't be fixed.

I think I need to understand your use-case a bit better.

Are you trying to:

1. Run one simulation, but modify the map over time by reloading the map layers.
2. Run multiple different simulation worlds one after another without restarting flatland server?

If you're trying to do the first, maybe the map layers can be cleared and then loaded into the existing layers (presumably it would be the same layer names/ids, with maybe some additions). This is how dynamic model loading works, except the world objects would persist (with just the layers being replaced in-place, or added as needed). What I'd hope to happen if you call world_->LoadLayers(layers_reader) and a layer with the same name already exists is for the new layer to clear the old layer, and then load into it (not necessarily deleting the layer, just updating it). Right now on world.cpp line 197, it looks like this will throw a "Layer with name $foo already exists" exception. Instead, it should probably get a reference to the existing layer (in the layers_ vector), and update it's properties, names, colors, etc, and clear it's physics/Body objects before loading in the replacements. This would require a parallel to Layer::MakeLayer such as layer->ReplaceLayer(...same arguments as MakeLayer...). To do this cleanly, most of the MakeLayer behavior should probably be broken out into helper functions.

If you're trying to do the second, can you use case be satisfied by restarting flatland server for a new world + layers each time? Avidbots does this, using a single world.yaml symlinked into each different map directory. Since the layer file paths are relative, you can load whichever layer files you want by pointing to the folder that contains the layer files and the common world.yaml symlink. If restarting flatland_server isn't desired, can you explain what problems that's causing?

[Ducanor]

My use-case is the first. I tried to somewhat implement your idea with the following callback function:

void SimulationManager::callback(nav_msgs::OccupancyGrid msg) {
    YamlReader world_reader = YamlReader(map_layer_yaml_file_); // load layer information
    YamlReader layers_reader = world_reader.Subnode("layers", YamlReader::LIST);

    world_->layers_.clear(); // clearing layers vector from world_

    world_->cfr_.layer_id_table_.clear(); // clearing layer id table containing names

    world_->LoadLayers(layers_reader); // load new layers
    world_->DebugVisualize(); // visualize layer in rviz
  }

Currently the simulation runs without any processes dying. But it appears that the layers are stacked on top of each other and after some episodes there is no free space for the agent to be spawned and of course move around.

My idea was to clear the layers_ vector and load a layer with the same name and property over and over into world_. The only change would be the png file from which the layer is loaded. I dont get the "Layer with name $foo already exists" exception because I cleared the layer_id_table_ which contains all layer names and ids.

Im still missing something. The layers seem to remain in in the simulation.

[josephduchesne]

Clearing layer id table isn't going to quite work all the time since if the layers in the world file/layer file are in a different order than they were before, layers will switch IDs but existing stuff (models) will still have the old layer ids which are now backwards. This is fine if the order doesn't change but not ideal.

I'm not sure why the layers are stacked on top of eachother, since I'd have expected the Layer destructor to delete the Layer->body_, which should delete all of the physics objects. It might just be the debug visualization?

Looking at the code though, Layer::DebugVisualize should clear the old layer if it has the same name. I'm also missing something. Can you push a branch to a clone of the repo with your changes, and maybe an example layer file or two so that I can reproduce the issue?

It would be a lot easier for me to debug that way than to try to manually figure out what's going on without testing the problem.

[Ducanor]

Here is a our copy of the flatland repo:
This is our github for the whole simulator:
Im working on the task_generation branch.
If necessary the whole setup can be installed very quick following:
This guide

In here you can find the yaml files i use to load the layers.
map.world.yaml to load the world with empty layer
map_layer.yaml to load the map layer
random_map.png will be overwritten to load another map layer

I use this script to create new random maps.
I will upload some random maps for you to test into the random map folder.

Currently i changed the callback function in the simulation_manager.cpp to subscribe to the topic /chatter2 and i manually publish to this topic to control the loading of layers. I did the same with map_generator_node.py with the topic /chatter to control the creation of random maps and the publishing of the topic /map.

Please tell me if you need anything else. I very much appreciate your help!

[josephduchesne]

I've created a branch of your branch, and have a proposed PR that may fix this issue:
ignc-research#6

One other thing I noticed while I was in the code was that you've changed the rosParam's like world_path from local (/flatland_server/world_path) to global (/world_path).

In order to maintain compatibility I'd recommend not making that change, and instead passing in the world_path arg from a parent roslaunch file. Currently the roslaunch files in your flatland repo are broken since the entries would have to be moved out of the flatland_server into the parent so that they'd be set correctly. If you keep the change, then your flatland roslaunch files won't be compatible with mainline (or other branches of flatland), which could cause some headache for your repo's users in the future. Additionally, the examples and documentation won't be correct unless they're changed as well.