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()
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('TekInnerParcelCornersExtended.csv', '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() # call updater function self.r = rospy.Rate(self.update_rate) self.updater()
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()
# 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')
# 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')