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) +