def __init__(self):
		# defines
		self.update_rate = 10 # set update frequency [Hz]

		rospy.loginfo(rospy.get_name() + ": Start")

		# get parameters
		#self.debug = rospy.get_param("~print_debug_information", 'true') 
 		#if self.debug:
		#	rospy.loginfo(rospy.get_name() + ": Debug enabled")
		#self.status_publish_interval = rospy.get_param("~status_publish_interval", '0') 

		# get topic names
		self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		self.polygon_map_topic = rospy.get_param("~polygon_map_pub",'/fmKnowledge/polygon_map')

		# setup subscription topic callbacks
		rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)

		# setup publish topics
		self.polygon_change_pub = rospy.Publisher(self.polygon_map_topic, IntStamped)
		self.polygon_change = IntStamped()

		# initialize the polygon map
		self.polymap = polygon_map()
		self.polymap.set_nearby_threshold (5.0) 
		self.polymap.set_polygons_per_update (100)

		# import polygons 
		file = open('polygon_map.txt', 'r')
		file_content = csv.reader(file, delimiter='\t')
		for name,e1,n1,e2,n2,e3,n3,e4,n4 in file_content:
			polygon = [[float(e1),float(n1)],[float(e2),float(n2)],[float(e3),float(n3)],[float(e4),float(n4)]]
			self.polymap.add_polygon (name, polygon)
		file.close()
		rospy.loginfo(rospy.get_name() + ": Loaded %ld polygons" % self.polymap.poly_total)

		# call updater function
		self.r = rospy.Rate(self.update_rate)
		self.updater()
	def __init__(self):
		rospy.loginfo(rospy.get_name() + ": Start")

		# get parameters
		self.update_rate = rospy.get_param("~update_rate", "10") # update frequency [Hz]
		self.polygons_per_update = rospy.get_param("~polygons_per_update", "100")
		self.nearby_threshold = rospy.get_param("~nearby_threshold", "5.0") # [m]
		rospy.loginfo(rospy.get_name() + ": Update rate: %ld Hz", self.update_rate)
		rospy.loginfo(rospy.get_name() + ": Polygons per update: %ld", self.polygons_per_update)
		rospy.loginfo(rospy.get_name() + ": Nearby threshold: %.3f m", self.nearby_threshold)

		# get topic names
		self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		self.polygon_map_topic = rospy.get_param("~polygon_map_pub",'/fmKnowledge/polygon_map')

		# setup subscription topic callbacks
		rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)

		# setup publish topics
		self.polygon_change_pub = rospy.Publisher(self.polygon_map_topic, IntStamped)
		self.polygon_change = IntStamped()

		# initialize the polygon map
		self.polymap = polygon_map()
		self.polymap.set_nearby_threshold (self.nearby_threshold) 
		self.polymap.set_polygons_per_update (self.polygons_per_update)

		# import polygons 
		file = open('polygon_map.txt', 'r')
		file_content = csv.reader(file, delimiter='\t')
		for name,e1,n1,e2,n2,e3,n3,e4,n4 in file_content:
			polygon = [[float(e1),float(n1)],[float(e2),float(n2)],[float(e3),float(n3)],[float(e4),float(n4)]]
			self.polymap.add_polygon (name, polygon)
		file.close()
		rospy.loginfo(rospy.get_name() + ": Loaded %ld polygons" % self.polymap.poly_total)

		# call updater function
		self.r = rospy.Rate(self.update_rate)
		self.updater()
Example #3
0
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#*****************************************************************************
"""py
Revision
2013-05-31 KJ First version
"""

from polygon_map import polygon_map
from polygon_map_plot import polygon_map_plot
import csv
import copy

# initialize the polygon map
polymap = polygon_map()
polymap.set_nearby_threshold (10.0) 
polymap.set_polygons_per_update (1600)

# initialize the polygon map plot
map_title = "Field"
map_window_size = 10.0 # [inches]
easting_offset = 0.0 # [m]
northing_offset = 0.0 # [m]
easting_offset = -588815.0 # [m]
northing_offset = -6137299.0 # [m]
polyplot = polygon_map_plot(map_title, map_window_size, easting_offset, northing_offset)

# import polygons 
file = open('TekInnerParcelCornersExtended.csv', 'r')
file_content = csv.reader(file, delimiter='\t')