Merge pull request #18 from avidbots/support-empty-layers

Added support for empty layers
avidbots · Sep 28, 2017 · a403801 · a403801
2 parents 113c43e + 541b04a
commit a403801
Show file tree

Hide file tree

Showing 5 changed files with 73 additions and 46 deletions.
diff --git a/flatland_server/include/flatland_server/layer.h b/flatland_server/include/flatland_server/layer.h
@@ -67,7 +67,7 @@ class Layer : public Entity {
   std::vector<std::string> names_;  ///< list of layer names
   std::string viz_name_;            ///< for visualization
   CollisionFilterRegistry *cfr_;    ///< collision filter registry
-  Body *body_;
+  Body *body_ = nullptr;
 
   /**
    * @brief Constructor for the Layer class for initialization using a image
@@ -107,6 +107,17 @@ class Layer : public Entity {
         const Pose &origin, const std::vector<LineSegment> &line_segments,
         double scale);
 
+  /**
+  * @brief Constructor for the Layer class for initialization with no static
+  * map in it
+  * @param[in] physics_world Pointer to the box2d physics world
+  * @param[in] cfr Collision filter registry
+  * @param[in] names A list of names for the layer, the first name is used
+  * for the name of the body
+  */
+  Layer(b2World *physics_world, CollisionFilterRegistry *cfr,
+        const std::vector<std::string> &names, const Color &color);
+
   /**
    * @brief Destructor for the layer class
    */

diff --git a/flatland_server/src/layer.cpp b/flatland_server/src/layer.cpp
@@ -96,6 +96,10 @@ Layer::Layer(b2World *physics_world, CollisionFilterRegistry *cfr,
   }
 }
 
+Layer::Layer(b2World *physics_world, CollisionFilterRegistry *cfr,
+             const std::vector<std::string> &names, const Color &color)
+    : Entity(physics_world, names[0]), names_(names), cfr_(cfr) {}
+
 Layer::~Layer() { delete body_; }
 
 const std::vector<std::string> &Layer::GetNames() const { return names_; }
@@ -107,54 +111,59 @@ Layer *Layer::MakeLayer(b2World *physics_world, CollisionFilterRegistry *cfr,
                         const std::string &map_path,
                         const std::vector<std::string> &names,
                         const Color &color) {
-  YamlReader reader(map_path);
-  reader.SetErrorInfo("layer " + Q(names[0]));
+  if (map_path.length() > 0) {  // If there is a map in this layer
+    YamlReader reader(map_path);
+    reader.SetErrorInfo("layer " + Q(names[0]));
+
+    std::string type = reader.Get<std::string>("type", "");
+
+    if (type == "line_segments") {
+      double scale = reader.Get<double>("scale");
+      Pose origin = reader.GetPose("origin");
+      boost::filesystem::path data_path(reader.Get<std::string>("data"));
+      if (data_path.string().front() != '/') {
+        data_path = boost::filesystem::path(map_path).parent_path() / data_path;
+      }
 
-  std::string type = reader.Get<std::string>("type", "");
+      ROS_INFO_NAMED("Layer",
+                     "layer \"%s\" loading line segments from path=\"%s\"",
+                     names[0].c_str(), data_path.string().c_str());
 
-  if (type == "line_segments") {
-    double scale = reader.Get<double>("scale");
-    Pose origin = reader.GetPose("origin");
-    boost::filesystem::path data_path(reader.Get<std::string>("data"));
-    if (data_path.string().front() != '/') {
-      data_path = boost::filesystem::path(map_path).parent_path() / data_path;
-    }
+      std::vector<LineSegment> line_segments;
 
-    ROS_INFO_NAMED("Layer",
-                   "layer \"%s\" loading line segments from path=\"%s\"",
-                   names[0].c_str(), data_path.string().c_str());
+      ReadLineSegmentsFile(data_path.string(), line_segments);
 
-    std::vector<LineSegment> line_segments;
+      return new Layer(physics_world, cfr, names, color, origin, line_segments,
+                       scale);
 
-    ReadLineSegmentsFile(data_path.string(), line_segments);
+    } else {
+      double resolution = reader.Get<double>("resolution");
+      double occupied_thresh = reader.Get<double>("occupied_thresh");
+      Pose origin = reader.GetPose("origin");
 
-    return new Layer(physics_world, cfr, names, color, origin, line_segments,
-                     scale);
+      boost::filesystem::path image_path(reader.Get<std::string>("image"));
+      if (image_path.string().front() != '/') {
+        image_path =
+            boost::filesystem::path(map_path).parent_path() / image_path;
+      }
 
-  } else {
-    double resolution = reader.Get<double>("resolution");
-    double occupied_thresh = reader.Get<double>("occupied_thresh");
-    Pose origin = reader.GetPose("origin");
+      ROS_INFO_NAMED("Layer", "layer \"%s\" loading image from path=\"%s\"",
+                     names[0].c_str(), image_path.string().c_str());
 
-    boost::filesystem::path image_path(reader.Get<std::string>("image"));
-    if (image_path.string().front() != '/') {
-      image_path = boost::filesystem::path(map_path).parent_path() / image_path;
-    }
+      cv::Mat map = cv::imread(image_path.string(), CV_LOAD_IMAGE_GRAYSCALE);
+      if (map.empty()) {
+        throw YAMLException("Failed to load " + Q(image_path.string()) +
+                            " in layer " + Q(names[0]));
+      }
 
-    ROS_INFO_NAMED("Layer", "layer \"%s\" loading image from path=\"%s\"",
-                   names[0].c_str(), image_path.string().c_str());
+      cv::Mat bitmap;
+      map.convertTo(bitmap, CV_32FC1, 1.0 / 255.0);
 
-    cv::Mat map = cv::imread(image_path.string(), CV_LOAD_IMAGE_GRAYSCALE);
-    if (map.empty()) {
-      throw YAMLException("Failed to load " + Q(image_path.string()) +
-                          " in layer " + Q(names[0]));
+      return new Layer(physics_world, cfr, names, color, origin, bitmap,
+                       occupied_thresh, resolution);
     }
-
-    cv::Mat bitmap;
-    map.convertTo(bitmap, CV_32FC1, 1.0 / 255.0);
-
-    return new Layer(physics_world, cfr, names, color, origin, bitmap,
-                     occupied_thresh, resolution);
+  } else {  // If the layer has no static obstacles
+    return new Layer(physics_world, cfr, names, color);
   }
 }
 
@@ -287,9 +296,11 @@ void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh,
 
 void Layer::DebugVisualize() const {
   DebugVisualization::Get().Reset(viz_name_);
-  DebugVisualization::Get().Visualize(viz_name_, body_->physics_body_,
-                                      body_->color_.r, body_->color_.g,
-                                      body_->color_.b, body_->color_.a);
+  if (body_ != nullptr) {
+    DebugVisualization::Get().Visualize(viz_name_, body_->physics_body_,
+                                        body_->color_.r, body_->color_.g,
+                                        body_->color_.b, body_->color_.a);
+  }
 }
 
 void Layer::DebugOutput() const {
@@ -302,7 +313,9 @@ void Layer::DebugOutput() const {
                   this, physics_world_, name_.c_str(), names.c_str(),
                   category_bits);
 
-  body_->DebugOutput();
+  if (body_ != nullptr) {
+    body_->DebugOutput();
+  }
 }
 
 };  // namespace flatland_server
diff --git a/flatland_server/src/world.cpp b/flatland_server/src/world.cpp
@@ -76,7 +76,9 @@ World::~World() {
   // them since the AABB tree gets restructured everytime a fixture is removed
   // The memory will later be freed by deleting the world
   for (int i = 0; i < layers_.size(); i++) {
-    layers_[i]->body_->physics_body_ = nullptr;
+    if (layers_[i]->body_ != nullptr) {
+      layers_[i]->body_->physics_body_ = nullptr;
+    }
     delete layers_[i];
   }
 
@@ -176,7 +178,7 @@ void World::LoadLayers(YamlReader &layers_reader) {
           ", max allowed is " + std::to_string(cfr_.MAX_LAYERS));
     }
 
-    boost::filesystem::path map_path(reader.Get<std::string>("map"));
+    boost::filesystem::path map_path(reader.Get<std::string>("map", ""));
     Color color = reader.GetColor("color", Color(1, 1, 1, 1));
     reader.EnsureAccessedAllKeys();
 
@@ -186,7 +188,7 @@ void World::LoadLayers(YamlReader &layers_reader) {
       }
     }
 
-    if (map_path.string().front() != '/') {
+    if (map_path.string().front() != '/' && map_path.string().length() > 0) {
       map_path = world_yaml_dir_ / map_path;
     }
 

diff --git a/flatland_server/test/load_world_test.cpp b/flatland_server/test/load_world_test.cpp
@@ -521,7 +521,7 @@ TEST_F(LoadWorldTest, simple_test_A) {
   EXPECT_EQ(w->physics_velocity_iterations_, 11);
   EXPECT_EQ(w->physics_position_iterations_, 12);
 
-  ASSERT_EQ(w->layers_.size(), 3);
+  ASSERT_EQ(w->layers_.size(), 4);
 
   // check that layer 0 settings are loaded correctly
   EXPECT_STREQ(w->layers_[0]->name_.c_str(), "2d");

diff --git a/flatland_server/test/load_world_tests/simple_test_A/world.yaml b/flatland_server/test/load_world_tests/simple_test_A/world.yaml
@@ -9,6 +9,7 @@ layers:
     map: "map3d.yaml"
   - name: ["lines"]
     map: "map_lines.yaml"
+  - name: "robot"
 models:  
   - name: turtlebot1 
     model: "turtlebot.model.yaml"