diff --git a/region_viewer/.cproject b/region_viewer/.cproject
new file mode 100644
index 0000000000000000000000000000000000000000..5147f1ab51dd7fddc9e5936c4d54c1d281aec35a
--- /dev/null
+++ b/region_viewer/.cproject
@@ -0,0 +1,292 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/region_viewer/.project b/region_viewer/.project
new file mode 100644
index 0000000000000000000000000000000000000000..85cf9bf1d39034f29c827392ecf6857fd4613ea2
--- /dev/null
+++ b/region_viewer/.project
@@ -0,0 +1,78 @@
+
+
+ Region Finder
+
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.genmakebuilder
+ clean,full,incremental,
+
+
+ ?name?
+
+
+
+ org.eclipse.cdt.make.core.append_environment
+ true
+
+
+ org.eclipse.cdt.make.core.autoBuildTarget
+ all
+
+
+ org.eclipse.cdt.make.core.buildArguments
+
+
+
+ org.eclipse.cdt.make.core.buildCommand
+ make
+
+
+ org.eclipse.cdt.make.core.cleanBuildTarget
+ clean
+
+
+ org.eclipse.cdt.make.core.contents
+ org.eclipse.cdt.make.core.activeConfigSettings
+
+
+ org.eclipse.cdt.make.core.enableAutoBuild
+ false
+
+
+ org.eclipse.cdt.make.core.enableCleanBuild
+ true
+
+
+ org.eclipse.cdt.make.core.enableFullBuild
+ true
+
+
+ org.eclipse.cdt.make.core.fullBuildTarget
+ all
+
+
+ org.eclipse.cdt.make.core.stopOnError
+ true
+
+
+ org.eclipse.cdt.make.core.useDefaultBuildCmd
+ true
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder
+
+
+
+
+
+ org.eclipse.cdt.core.cnature
+ org.eclipse.cdt.core.ccnature
+ org.eclipse.cdt.managedbuilder.core.managedBuildNature
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigNature
+
+
diff --git a/region_viewer/CMakeLists.txt b/region_viewer/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..724354eb00c9cdc94cbfffbde1f2b5c83738d7dd
--- /dev/null
+++ b/region_viewer/CMakeLists.txt
@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 2.8.0)
+project(region_viewer)
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+#find_package(catkin REQUIRED COMPONENTS rospy std_msgs)
+find_package(catkin REQUIRED COMPONENTS rospy region_msgs)
+
+ catkin_package(
+ CATKIN_DEPENDS rospy region_msgs
+)
+
+
+#############
+## Install ##
+#############
+
+catkin_python_setup()
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+
+install(PROGRAMS scripts/region_viewer.pl
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+install(
+ DIRECTORY launch/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/
+)
diff --git a/region_viewer/README.txt b/region_viewer/README.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6cc3b4f30f3df5d6cae21697e2e1d9689c256fab
--- /dev/null
+++ b/region_viewer/README.txt
@@ -0,0 +1,15 @@
+Region finder inputs an occupancy grid and dynamically builds
+regions. There are two versions:
+
+- region_finder:
+Publishes the full region list (will all region data) at every pass.
+
+- region_finder_serv
+Publishes only the region id list at every pass. The user can then
+query a service for only the regions that have changed.
+
+Options:
+- gui - uses OpenCV for some basic display. Useful for debugging.
+
+rosrun region_finder region_finder _gui:=true
+
diff --git a/region_viewer/launch/region_viewer.launch b/region_viewer/launch/region_viewer.launch
new file mode 100644
index 0000000000000000000000000000000000000000..5eae613c279a10a2aa843eb8502dd7d1c4e99985
--- /dev/null
+++ b/region_viewer/launch/region_viewer.launch
@@ -0,0 +1,4 @@
+
+
+
+
diff --git a/region_viewer/package.xml b/region_viewer/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..371d522c6de9046bd8e64d5d6716fd0665becd2f
--- /dev/null
+++ b/region_viewer/package.xml
@@ -0,0 +1,21 @@
+
+ region_viewer
+ 1.0.0
+
+
+ Simple viewer for regions.
+
+
+ David Rajaratnam
+ BSD
+
+ catkin
+ rospy
+ region_msgs
+
+ rospy
+ region_msgs
+
+
+
+
diff --git a/region_viewer/scripts/region_viewer.pl b/region_viewer/scripts/region_viewer.pl
new file mode 100755
index 0000000000000000000000000000000000000000..61d3a8f19bc8f3b23d96581fde72af91fdf26e4d
--- /dev/null
+++ b/region_viewer/scripts/region_viewer.pl
@@ -0,0 +1,129 @@
+#!/usr/bin/env python
+
+#--------------------------------------------------------------------
+# Command-line input of user commands to display all regions
+#
+# -------------------------------------------------------------------
+
+import sys, os, exceptions
+
+import roslib; roslib.load_manifest('golog_demos')
+import roslib.packages
+import rospy
+from region_msgs.msg import Regions as RRegions, Region as RREgions, Tag as RTag, Tags as RTags
+from geometry_msgs.msg import Point as RPoint, Point32 as RPoint32, Polygon as RPolygon, Pose as RPose
+import cv
+
+class RegionDisplay:
+ def __init__(self, regions_topic, tag_topic):
+ print "Subscribing to regions topic: '{0}', and named points topic: '{1}'".format(regions_topic, tag_topic)
+ self.m_width = 200
+ self.m_height = 200
+ self.m_resolution = 0.1
+ self.m_origin = RPoint()
+ self.m_origin.x = -10
+ self.m_origin.y = -10
+ self.m_regions = []
+ self.m_namedpoints = []
+ self.m_mat = None
+ self.m_white = (255,255,255)
+ self.m_blue = (255,0,0)
+ self.m_black = (0,0,0)
+ self.m_green = (0,255,0)
+ self.m_fontRegionScale = 0.45
+ self.m_fontNPointScale = 0.30
+ self.m_fontRegion = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX, self.m_fontRegionScale, self.m_fontRegionScale)
+ self.m_fontNPoint = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX, self.m_fontNPointScale, self.m_fontNPointScale)
+
+ self.m_windowName = "Regions"
+# self.m_windowName = "Grouped Regions"
+# cv.NamedWindow(self.m_windowName, cv.CV_WINDOW_NORMAL | cv.CV_WINDOW_FREERATIO)
+ cv.NamedWindow(self.m_windowName, cv.CV_WINDOW_NORMAL)
+ self._resize_window()
+# cv.NamedWindow(self.m_windowName)
+ self.m_regions_sub = rospy.Subscriber(regions_topic, RRegions, self._on_update_regions)
+ self.m_namedpoints_sub = rospy.Subscriber(tag_topic, RTags, self._on_update_namedpoints)
+
+ def _rpoint2cvpoint(self, rpoint):
+ newx = (rpoint.x - self.m_origin.x)/self.m_resolution
+ newy = self.m_height - 1 -(rpoint.y - self.m_origin.y)/self.m_resolution
+ return (int(newx),int(newy))
+
+ def _on_update_regions(self, data):
+ self.m_regions = []
+ self.m_regions = data.regions
+ self._update_image()
+
+ def _on_update_namedpoints(self, data):
+ self.m_namedpoints = []
+ self.m_namedpoints = data.points
+ self._update_image()
+
+ def _resize_window(self):
+ self.m_mat = cv.CreateMat(self.m_height, self.m_width, cv.CV_8UC3)
+ self._update_image()
+
+ # draw a white background
+ def _draw_background(self):
+ for x in range (0, self.m_height-1):
+ for y in range (0, self.m_width-1):
+ self.m_mat[x,y] = self.m_white
+
+ def _draw_polygon(self, rpoints):
+ num = len(rpoints)
+ if num < 3: return
+ first = self._rpoint2cvpoint(rpoints[0])
+ next = first
+ for i in range(1, num-1):
+ cvp = self._rpoint2cvpoint(rpoints[i])
+ cv.Line(self.m_mat, next, cvp, self.m_blue)
+ next = cvp
+ if next != first:
+ cv.Line(self.m_mat, next, first, self.m_blue)
+
+ # Only call if the cv.matrix is valid
+ def _draw_region(self, region):
+ midpoint = self._rpoint2cvpoint(region.midpoint)
+ if len(region.name) > 0:
+ textpoint = (midpoint[0]-25, midpoint[1]-5)
+ cv.PutText(self.m_mat, region.name, textpoint, self.m_fontRegion, self.m_black)
+# cv.Circle(self.m_mat, midpoint, 1, self.m_black, 2)
+ self._draw_polygon(region.polygon.points)
+
+ def _draw_namedpoint(self, namedpoint):
+ if len(namedpoint.name) <= 0: return
+ point = self._rpoint2cvpoint(namedpoint.point)
+ textpoint = (point[0] - 10, point[1]-5)
+ cv.PutText(self.m_mat, namedpoint.name, textpoint, self.m_fontNPoint, self.m_green)
+ cv.Circle(self.m_mat, point, 1, self.m_green, 2)
+
+ def _update_image(self):
+ if self.m_mat is None: return
+ self._draw_background()
+ for namedpoint in self.m_namedpoints:
+ self._draw_namedpoint(namedpoint)
+ for region in self.m_regions:
+ self._draw_region(region)
+ cv.ShowImage(self.m_windowName, self.m_mat)
+
+if __name__ == '__main__':
+
+ try:
+ rospy.init_node('show_regions', log_level=rospy.DEBUG)
+
+ # Displays the high-level grouped regions
+# regions_topic = rospy.get_param('regions', 'regions_grouped')
+
+ # Displays the low-level regions
+ regions_topic = rospy.get_param('regions', 'regions')
+
+ tags_topic = rospy.get_param('tags', 'tags')
+ rd = RegionDisplay(regions_topic, tags_topic)
+ done = False
+ while not rospy.is_shutdown():
+ cv.WaitKey(2)
+
+ except:
+ print "Unexpected error", sys.exc_info()[0]
+ raise
+ cv.DestroyAllWindows()
diff --git a/region_viewer/scripts/show_regions_serv.py b/region_viewer/scripts/show_regions_serv.py
new file mode 100755
index 0000000000000000000000000000000000000000..b3d604daaede64653d51969b322bfeb387bd4cb4
--- /dev/null
+++ b/region_viewer/scripts/show_regions_serv.py
@@ -0,0 +1,145 @@
+#!/usr/bin/env python
+
+#--------------------------------------------------------------------
+# Command-line input of user commands to display all regions
+#
+# -------------------------------------------------------------------
+
+import sys, os, exceptions
+
+import roslib; roslib.load_manifest('golog_demos')
+import roslib.packages
+import rospy
+from region_msgs.msg import Regions, Region, Tag, Tags, Point2D
+from region_msgs.msg import SimplePolygon2D, PolygonWithHoles2D
+from geometry_msgs.msg import Pose as RPose
+from nav_msgs.msg import OccupancyGrid, MapMetaData
+import cv
+
+class RegionDisplay:
+ def __init__(self, map_topic, regions_serv, namedpoints_topic):
+ print "Subscribing to map topic: '{0}', and regions service: '{1}', and named points topic: '{2}'".format(map_topic, regions_serv, namedpoints_topic)
+ self.m_width = 0
+ self.m_height = 0
+ self.m_resolution = 0.0
+ self.m_origin = Point2D()
+ self.m_regions = []
+ self.m_namedpoints = []
+ self.m_mat = None
+ self.m_white = (255,255,255)
+ self.m_blue = (255,0,0)
+ self.m_black = (0,0,0)
+ self.m_green = (0,255,0)
+ self.m_fontRegionScale = 0.45
+ self.m_fontNPointScale = 0.30
+ self.m_fontRegion = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX,
+ self.m_fontRegionScale, self.m_fontRegionScale)
+ self.m_fontNPoint = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX, self.m_fontNPointScale, self.m_fontNPointScale)
+
+ self.m_windowName = "Regions"
+# self.m_windowName = "Grouped Regions"
+# cv.NamedWindow(self.m_windowName, cv.CV_WINDOW_NORMAL | cv.CV_WINDOW_FREERATIO)
+ cv.NamedWindow(self.m_windowName)
+ self.m_map_sub = rospy.Subscriber(map_topic, OccupancyGrid, self._on_update_map)
+ self.m_regions_sub = rospy.Subscriber(regions_serv, Regions, self._on_update_regions)
+ self.m_namedpoints_sub = rospy.Subscriber(namedpoints_topic, Tags, self._on_update_namedpoints)
+
+ def _rpoint2cvpoint(self, rpoint):
+ newx = (rpoint.x - self.m_origin.x)/self.m_resolution
+ newy = self.m_height - 1 -(rpoint.y - self.m_origin.y)/self.m_resolution
+ return (int(newx),int(newy))
+
+ def _on_update_regions(self, data):
+ self.m_regions = []
+ self.m_regions = data.regions
+ self._update_image()
+
+ def _on_update_namedpoints(self, data):
+ self.m_namedpoints = []
+ self.m_namedpoints = data.points
+ self._update_image()
+
+ def _on_update_map(self, data):
+ changed = False
+ if self.m_height != data.info.height:
+ self.m_height = data.info.height
+ changed = True
+ if self.m_width != data.info.width:
+ self.m_width = data.info.width
+ changed = True
+ if self.m_resolution != data.info.resolution:
+ self.m_resolution = data.info.resolution
+ changed = True
+ if self.m_origin != data.info.origin.position:
+ self.m_origin = data.info.origin.position
+ changed = True
+ if changed:
+ self.m_mat = cv.CreateMat(self.m_height, self.m_width, cv.CV_8UC3)
+ self._update_image()
+# self.m_mat[self.m_height-1 - x]
+
+ # draw a white background
+ def _draw_background(self):
+ for x in range (0, self.m_height-1):
+ for y in range (0, self.m_width-1):
+ self.m_mat[x,y] = self.m_white
+
+ def _draw_polygon(self, rpoints):
+ num = len(rpoints)
+ if num < 3: return
+ first = self._rpoint2cvpoint(rpoints[0])
+ next = first
+ for i in range(1, num-1):
+ cvp = self._rpoint2cvpoint(rpoints[i])
+ cv.Line(self.m_mat, next, cvp, self.m_blue)
+ next = cvp
+ if next != first:
+ cv.Line(self.m_mat, next, first, self.m_blue)
+
+ # Only call if the cv.matrix is valid
+ def _draw_region(self, region):
+# midpoint = self._rpoint2cvpoint(region.midpoint)
+# if len(region.name) > 0:
+# textpoint = (midpoint[0]-25, midpoint[1]-5)
+# cv.PutText(self.m_mat, region.name, textpoint, self.m_fontRegion, self.m_black)
+## cv.Circle(self.m_mat, midpoint, 1, self.m_black, 2)
+ self._draw_polygon(region.polygon.outer.points)
+
+ def _draw_namedpoint(self, namedpoint):
+ if len(namedpoint.name) <= 0: return
+ point = self._rpoint2cvpoint(namedpoint.point)
+ textpoint = (point[0] - 10, point[1]-5)
+ cv.PutText(self.m_mat, namedpoint.name, textpoint, self.m_fontNPoint, self.m_green)
+ cv.Circle(self.m_mat, point, 1, self.m_green, 2)
+
+ def _update_image(self):
+ if self.m_mat is None: return
+ self._draw_background()
+ for namedpoint in self.m_namedpoints:
+ self._draw_namedpoint(namedpoint)
+ for region in self.m_regions:
+ self._draw_region(region)
+ cv.ShowImage(self.m_windowName, self.m_mat)
+
+if __name__ == '__main__':
+
+ try:
+ rospy.init_node('show_regions', log_level=rospy.DEBUG)
+ map_topic = rospy.get_param('map','map')
+
+ # Displays the high-level grouped regions
+# regions_serv = rospy.get_param('regions_grouped', 'regions_grouped')
+
+ # Displays the low-level regions
+ regions_serv = rospy.get_param('regions_grouped', 'regions')
+
+ namedpoints_topic = rospy.get_param('named_points', 'named_points')
+ rd = RegionDisplay(map_topic, regions_serv, namedpoints_topic)
+ done = False
+ while not rospy.is_shutdown():
+ cv.WaitKey(2)
+
+ except:
+ print "Unexpected error", sys.exc_info()[0]
+ raise
+ cv.DestroyAllWindows()
diff --git a/region_viewer/setup.py b/region_viewer/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..302c61df65a81363799d817afea3412e2bb53f53
--- /dev/null
+++ b/region_viewer/setup.py
@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+
+d = generate_distutils_setup(
+# packages=['ggp_rqt'],
+# package_dir={'': 'src'}
+)
+
+setup(**d)
+