diff --git a/region_finder/include/voronoi_updater.h b/region_finder/include/voronoi_updater.h index 2b8063c996e85b7238c33e35a3aa3301caf362d1..82e5d3d128bb7104c06e7242a52a7e108f3a6abe 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 32924245ae9798161ca9e8f7dcd4d1771a50a5c3..f5bfd35245e59247e11ab9fac7187e11389c035a 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 > 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 82cc72a485dbcce243691612f75f0d72e72c61c2..f14992f1ee14963a312e7b0ff0b8bb9e530a0018 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 cf254f200eda6d007206d3dfadc246b5290427f4..454671dc7dc1851a1130716cbe6a907cdaede887 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 (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(*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 f109449a5afa3db8f18b7790cfb4a656f4d61034..b60cd7590749364076b23cbb94a58718fb293f04 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::clone() const } */ } + nrm->intersect_ = intersect_; return nrm; }