Merge pull request #6 from avidbots/task_generation_fix

Fixed map replacement crash, and added some extra debug output

flatland_server/src/body.cpp

@@ -66,7 +66,13 @@ Body::Body(b2World *physics_world, Entity *entity, const std::string &name,
 }
 
 Body::~Body() {
+  ROS_INFO_NAMED("Body",
+                 "body  \"%s\" destructor called",
+                 name_.c_str());
   if (physics_body_) {
+    ROS_INFO_NAMED("Body",
+                   "Deleting physics body for \"%s\"",
+                   name_.c_str());
     physics_body_->GetWorld()->DestroyBody(physics_body_);
   }
 }

flatland_server/src/layer.cpp

@@ -118,7 +118,12 @@ Layer::Layer(b2World *physics_world, CollisionFilterRegistry *cfr,
       cfr_(cfr),
       viz_name_("layer/" + names[0]) {}
 
-Layer::~Layer() { delete body_; }
+Layer::~Layer() { 
+  ROS_INFO_NAMED("Layer",
+                 "layer \"%s\" destructor called",
+                 names_[0].c_str());
+  delete body_; 
+}
 
 const std::vector<std::string> &Layer::GetNames() const { return names_; }
 

flatland_server/src/simulation_manager.cpp

@@ -238,13 +238,11 @@ bool SimulationManager::callback_StepWorld(
 // void SimulationManager::callback(nav_msgs::OccupancyGrid msg) {
 // void SimulationManager::callback(geometry_msgs::PoseStamped msg) {
 void SimulationManager::callback(const std_msgs::String::ConstPtr& msg) {
+    std::string map_layer_yaml_file_(msg->data);
     YamlReader world_reader = YamlReader(map_layer_yaml_file_);
     YamlReader layers_reader = world_reader.Subnode("layers", YamlReader::LIST);
-    world_->layers_name_map_.clear();
-    world_->layers_.clear();
-    ROS_INFO_NAMED("World", "Layer cleared");
-    world_->cfr_.layer_id_table_.clear();
-    ROS_INFO_NAMED("World", "Layer ID table cleared");
+
+    // replace the existing world layers with layers loaded from the provided yaml file
     world_->LoadLayers(layers_reader);
     world_->DebugVisualize(true);
     ROS_INFO_NAMED("World", "Map Layer loaded");

flatland_server/src/world.cpp

@@ -55,6 +55,7 @@
 #include <boost/filesystem.hpp>
 #include <map>
 #include <string>
+#include <set>
 
 namespace flatland_server {
 
@@ -165,6 +166,10 @@ World *World::MakeWorld(const std::string &yaml_path) {
 }
 
 void World::LoadLayers(YamlReader &layers_reader) {
+
+  // Used to delete existing layers not found in the layers reader
+  std::set<std::vector<std::string>> layer_ids_added;
+
   // loop through each layer and parse the data
   for (int i = 0; i < layers_reader.NodeSize(); i++) {
     YamlReader reader = layers_reader.Subnode(i, YamlReader::MAP);
@@ -177,6 +182,7 @@ void World::LoadLayers(YamlReader &layers_reader) {
     } else {
       names.push_back(name_reader.As<std::string>());
     }
+    layer_ids_added.insert(names);
 
     if (cfr_.LayersCount() + names.size() > cfr_.MAX_LAYERS) {
       throw YAMLException(
@@ -192,9 +198,23 @@ void World::LoadLayers(YamlReader &layers_reader) {
         reader.SubnodeOpt("properties", YamlReader::NodeTypeCheck::MAP).Node();
     reader.EnsureAccessedAllKeys();
 
-    for (const auto &name : names) {
-      if (cfr_.RegisterLayer(name) == cfr_.LAYER_ALREADY_EXIST) {
-        throw YAMLException("Layer with name " + Q(name) + " already exists");
+
+    if (layers_name_map_.count(names)>0) {  // this layer name already exists, delete it
+      ROS_INFO_NAMED("World", "Removing layer \"%s\" from path=\"%s\" prior to reloading",
+                    names[0].c_str(), map_path.string().c_str());
+
+      Layer* old_layer = layers_name_map_[names];
+
+      layers_.erase(std::remove(layers_.begin(), layers_.end(), old_layer), layers_.end()); // remove the layer from the layers_ vector
+      delete old_layer;  // delete the layer object, which should also delete it's world entities
+      layers_name_map_.erase(names);  // remove old entry from layers_names_map_. New layer will be re-added.
+
+    } else {  // otherwise, we need to add the layer to the collision filter registry
+
+      for (const auto &name : names) {
+        if (cfr_.RegisterLayer(name) == cfr_.LAYER_ALREADY_EXIST) {
+          throw YAMLException("Layer with name " + Q(name) + " already exists");
+        }
       }
     }
 
@@ -203,17 +223,33 @@ void World::LoadLayers(YamlReader &layers_reader) {
     }
 
     ROS_INFO_NAMED("World", "Loading layer \"%s\" from path=\"%s\"",
-                   names[0].c_str(), map_path.string().c_str());
+                  names[0].c_str(), map_path.string().c_str());
 
-    Layer *layer = Layer::MakeLayer(physics_world_, &cfr_, map_path.string(),
+    Layer* layer = Layer::MakeLayer(physics_world_, &cfr_, map_path.string(),
                                     names, color, properties);
     layers_name_map_.insert(
         std::pair<std::vector<std::string>, Layer *>(names, layer));
     layers_.push_back(layer);
 
     ROS_INFO_NAMED("World", "Layer \"%s\" loaded", layer->name_.c_str());
+
     layer->DebugOutput();
   }
+
+  // remove any  existing layers that were not present in the new world file
+  for( const auto& keyVal : layers_name_map_) {
+    if (layer_ids_added.count(keyVal.first) == 0)  {  // layer id not found in the new file
+      // delete physics body if present, clearing it
+      if (keyVal.second->body_ != nullptr) {
+        ROS_INFO_NAMED("World", "Clearing physics body of obsolete layer \"%s\"", keyVal.second->name_.c_str());
+        delete keyVal.second->body_;
+        keyVal.second->body_ = nullptr;
+      }
+
+    } else {
+      ROS_INFO_NAMED("World", "Leaving active layer \"%s\"", keyVal.second->name_.c_str());
+    }
+  }
 }
 
 void World::LoadModels(YamlReader &models_reader) {