diff --git a/flatland_server/include/flatland_server/layer.h b/flatland_server/include/flatland_server/layer.h index 1e7b7c46..ee976291 100644 --- 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 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 &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 &names, const Color &color); + /** * @brief Destructor for the layer class */ diff --git a/flatland_server/src/layer.cpp b/flatland_server/src/layer.cpp index 9582ca31..2f422bcc 100644 --- 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 &names, const Color &color) + : Entity(physics_world, names[0]), names_(names), cfr_(cfr) {} + Layer::~Layer() { delete body_; } const std::vector &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 &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("type", ""); + + if (type == "line_segments") { + double scale = reader.Get("scale"); + Pose origin = reader.GetPose("origin"); + boost::filesystem::path data_path(reader.Get("data")); + if (data_path.string().front() != '/') { + data_path = boost::filesystem::path(map_path).parent_path() / data_path; + } - std::string type = reader.Get("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("scale"); - Pose origin = reader.GetPose("origin"); - boost::filesystem::path data_path(reader.Get("data")); - if (data_path.string().front() != '/') { - data_path = boost::filesystem::path(map_path).parent_path() / data_path; - } + std::vector 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 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("resolution"); + double occupied_thresh = reader.Get("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("image")); + if (image_path.string().front() != '/') { + image_path = + boost::filesystem::path(map_path).parent_path() / image_path; + } - } else { - double resolution = reader.Get("resolution"); - double occupied_thresh = reader.Get("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("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 index 99239513..c2184869 100644 --- 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("map")); + boost::filesystem::path map_path(reader.Get("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 index 1a2bfbb2..b7431db1 100644 --- 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 index fe681678..c096505f 100644 --- 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"