costmap cspace: add layer type check (#101)

* Add layer type flag * Add layer type check
at-wat · Jan 22, 2018 · dce5294 · dce5294
1 parent bae0158
commit dce5294
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 14 deletions.
diff --git a/costmap_cspace/include/costmap_3d_layer_base.h b/costmap_cspace/include/costmap_3d_layer_base.h
@@ -84,6 +84,7 @@ class Costmap3dLayerBase
   float linear_expand_;
   float linear_spread_;
   map_overlay_mode overlay_mode_;
+  bool root_;
 
   CSpace3Cache cs_template_;
   int range_max_;
@@ -100,6 +101,7 @@ class Costmap3dLayerBase
     , linear_expand_(0.0)
     , linear_spread_(0.0)
     , overlay_mode_(map_overlay_mode::MAX)
+    , root_(true)
     , map_(new CSpace3DMsg)
     , map_overlay_(new CSpace3DMsg)
   {
@@ -115,9 +117,16 @@ class Costmap3dLayerBase
     linear_spread_ = linear_spread;
     overlay_mode_ = overlay_mode;
   }
+  void setChild(Costmap3dLayerBase::Ptr child)
+  {
+    child_ = child;
+    child_->setMap(getMapOverlay());
+    child_->root_ = false;
+  }
   virtual void generateCSpaceTemplate(const MapMetaData3D &info) = 0;
   void setBaseMap(const nav_msgs::OccupancyGrid &base_map)
   {
+    ROS_ASSERT(root_);
     ROS_ASSERT(base_map.data.size() >= base_map.info.width * base_map.info.height);
     map_base_ = base_map;
 
@@ -154,13 +163,9 @@ class Costmap3dLayerBase
     if (child_)
       child_->setBaseMapChain();
   }
-  void registerChild(Costmap3dLayerBase::Ptr child)
-  {
-    child_ = child;
-    child_->setMap(getMapOverlay());
-  }
   CSpace3DUpdate processMapOverlay(const nav_msgs::OccupancyGrid &msg)
   {
+    ROS_ASSERT(!root_);
     const int ox = lroundf((msg.info.origin.position.x - map_->info.origin.position.x) / map_->info.linear_resolution);
     const int oy = lroundf((msg.info.origin.position.y - map_->info.origin.position.y) / map_->info.linear_resolution);
     *map_overlay_ = *map_;

diff --git a/costmap_cspace/include/costmap_3d_layer_footprint.h b/costmap_cspace/include/costmap_3d_layer_footprint.h
@@ -64,9 +64,6 @@ class Costmap3dLayerFootprint : public Costmap3dLayerBase
   Polygon footprint_p_;
 
 public:
-  Costmap3dLayerFootprint()
-  {
-  }
   bool setFootprint(const XmlRpc::XmlRpcValue footprint_xml_const)
   {
     XmlRpc::XmlRpcValue footprint_xml = footprint_xml_const;

diff --git a/costmap_cspace/include/costmap_3d_layer_plain.h b/costmap_cspace/include/costmap_3d_layer_plain.h
@@ -45,9 +45,6 @@ class Costmap3dLayerPlain : public Costmap3dLayerBase
   using Ptr = std::shared_ptr<Costmap3dLayerPlain>;
 
 public:
-  Costmap3dLayerPlain()
-  {
-  }
   void generateCSpaceTemplate(const MapMetaData3D &info)
   {
     range_max_ = ceilf((linear_expand_ + linear_spread_) / info.linear_resolution);

diff --git a/costmap_cspace/src/costmap_3d.cpp b/costmap_cspace/src/costmap_3d.cpp
@@ -178,7 +178,7 @@ class Costmap3DOFNode
       throw std::runtime_error("Invalid footprint");
     }
     costmaps_[1] = costmap_overlay;
-    costmaps_[0]->registerChild(costmaps_[1]);
+    costmaps_[0]->setChild(costmaps_[1]);
 
     sub_map_ = nh_.subscribe("map", 1, &Costmap3DOFNode::cbMap, this);
     sub_map_overlay_ = nh_.subscribe("map_overlay", 1, &Costmap3DOFNode::cbMapOverlay, this);

diff --git a/costmap_cspace/test/src/test_costmap_3d.cpp b/costmap_cspace/test/src/test_costmap_3d.cpp
@@ -382,7 +382,7 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceOverwrite)
   // Settings: 4 angular grids, no expand/spread
   cm->setCSpaceConfig(4, 0.0, 0.0, costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::OVERWRITE);
   cm_over->setCSpaceConfig(4, 0.0, 0.0, costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::OVERWRITE);
-  cm->registerChild(cm_over);
+  cm->setChild(cm_over);
   cm_ref.setCSpaceConfig(4, 0.0, 0.0, costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::OVERWRITE);
   cm_base.setCSpaceConfig(4, 0.0, 0.0, costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::OVERWRITE);
 
@@ -522,7 +522,7 @@ TEST(Costmap3dLayerFootprintTest, testCSpaceOverlayMove)
   // Settings: 4 angular grids, no expand/spread
   cm->setCSpaceConfig(4, 0.0, 0.0, costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::MAX);
   cm_over->setCSpaceConfig(4, 0.0, 0.0, costmap_cspace::Costmap3dLayerFootprint::map_overlay_mode::MAX);
-  cm->registerChild(cm_over);
+  cm->setChild(cm_over);
 
   // Generate sample map
   nav_msgs::OccupancyGrid map;