From 40a8d48489765236aa43a48861e164d6e713be84 Mon Sep 17 00:00:00 2001
From: David Rajaratnam <daver@cse.unsw.edu.au>
Date: Fri, 17 Nov 2017 10:27:57 +1100
Subject: [PATCH] Fixed bug in publishing the intersecting regions

---
 region_finder/include/voronoi_updater.h   |  2 --
 region_finder/src/opencv_tools.cpp        | 19 ++++++++++++++++++-
 region_finder/src/region_finder.cpp       |  4 ++--
 region_finder/src/regionmapper.cpp        |  6 ++----
 region_finder/src/voronoi_region_calc.cpp |  1 +
 5 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/region_finder/include/voronoi_updater.h b/region_finder/include/voronoi_updater.h
index 2b8063c..82e5d3d 100644
--- a/region_finder/include/voronoi_updater.h
+++ b/region_finder/include/voronoi_updater.h
@@ -236,8 +236,6 @@ public:
     if (skeletonize_)
       boundaries_.updatePoints(grid, begin, end);
 
-	ROS_INFO_STREAM("HERE");
-
     bushfire_.update(grid, begin, end);
   }
   const t_pointset& changes() const { return changedPoints_; }
diff --git a/region_finder/src/opencv_tools.cpp b/region_finder/src/opencv_tools.cpp
index 3292424..f5bfd35 100644
--- a/region_finder/src/opencv_tools.cpp
+++ b/region_finder/src/opencv_tools.cpp
@@ -354,7 +354,7 @@ void CVManager::DisplayVoronoiCell(const VoronoiCell& vc, const IntPoint2D& poin
   if (regionmap_->inRegion(point))
   {
     const VoronoiRegion& region = regionmap_->region(point);
-    std::cout << "\t ID: " << region.id_ << std::endl;
+    std::cout << "\t Region ID: " << region.id_ << std::endl;
     std::cout << "\t Midpoint: " << region.midpoint_ << std::endl;
     std::cout << "\t Related pointsets: ";
 
@@ -369,6 +369,23 @@ void CVManager::DisplayVoronoiCell(const VoronoiCell& vc, const IntPoint2D& poin
       std::cout << psp << " ";
     }
     std::cout << std::endl;
+
+
+    std::cout << "\t Intersecting regions: [";
+
+    typedef std::set<std::pair<unsigned long, unsigned long> > t_tmp;
+    t_tmp::const_iterator icurr = regionmap_->intersect_.begin();
+    t_tmp::const_iterator iend = regionmap_->intersect_.end();
+
+    if (regionmap_->intersect_.empty()) std::cout << "EMPTY";
+
+    while (icurr != iend)
+    {
+        if (icurr->first == region.id_) std::cout << " " << icurr->second << " ";
+        ++icurr;
+    }
+    std::cout << "]" << std::endl;
+
     drawRegion(imgclone, vmap_height, green, region);
   }
 
diff --git a/region_finder/src/region_finder.cpp b/region_finder/src/region_finder.cpp
index 82cc72a..f14992f 100644
--- a/region_finder/src/region_finder.cpp
+++ b/region_finder/src/region_finder.cpp
@@ -25,9 +25,9 @@ int main(int argc, char** argv)
   {
 
 //	  debugger_attach_hack();
-	  
+
 	  ros::init(argc, argv, "region_finder");
-	  
+
 	  RegionMapper rm;
 	  ros::spin();
 	  return (0);
diff --git a/region_finder/src/regionmapper.cpp b/region_finder/src/regionmapper.cpp
index cf254f2..454671d 100644
--- a/region_finder/src/regionmapper.cpp
+++ b/region_finder/src/regionmapper.cpp
@@ -114,12 +114,9 @@ RegionMapper::initMaps(const nav_msgs::OccupancyGridPtr& oc)
   // mapupdater_.resolution(mapmetadata_.resolution);
   // Create the map by transforming data from the occupancy grid and then guess unknowns where possible
   occmap_ = create_grid2d<t_occmap> (mapmetadata_.width, mapmetadata_.height);
-  ROS_INFO_STREAM("HERE3");
 
   occmapupdater_.update(*occmap_, *occgrid_);
 
-  ROS_INFO_STREAM("HERE4");
-
 
   // Create the voronoi map from the basic map
   //  voronoimap_ = create_grid2d<t_voronoimap, t_basicmap, BasicMap2VoronoiMap>(*basicmap_, voronoimapfilter_);
@@ -371,7 +368,8 @@ void RegionMapper::publishRegions(const VoronoiRegionMap& vrm, const nav_msgs::M
     t_tmp::const_iterator end = vrm.intersect_.end();
     while (curr != end)
     {
-        rregion.intersecting.push_back(curr->second);
+        if (curr->first == rregion.id)
+            rregion.intersecting.push_back(curr->second);
         ++curr;
     }
 
diff --git a/region_finder/src/voronoi_region_calc.cpp b/region_finder/src/voronoi_region_calc.cpp
index f109449..b60cd75 100644
--- a/region_finder/src/voronoi_region_calc.cpp
+++ b/region_finder/src/voronoi_region_calc.cpp
@@ -519,6 +519,7 @@ std::auto_ptr<VoronoiRegionMap> VoronoiRegionMap::clone() const
           }
         */
     }
+    nrm->intersect_ = intersect_;
     return nrm;
 }
 
-- 
GitLab