def update_geo_path(self):
if (self.waypoints and self.zone and self.band):
waypoints_geo = []
for point in self.waypoints:
x, y = point
geo = UTMPoint(x, y, zone=self.zone, band=self.band).toMsg()
wp = WayPoint()
wp.id = unique_id.toMsg(unique_id.fromRandom())
wp.position = geo
waypoints_geo.append(wp)
segments_geo = []
last_wp = waypoints_geo[0].id
for wp in waypoints_geo:
seg = RouteSegment()
seg.id = unique_id.toMsg(unique_id.fromRandom())
seg.start = last_wp
seg.end = wp.id
last_wp = wp.id
segments_geo.append(seg)
self.route_network.points = waypoints_geo
self.route_network.segments = segments_geo
else:
rospy.logwarn('[update_geo_path] waypoints, zone, and/or band\n' +
'\thas not been set/updated.\n\tIgnoring...')
def makeWayPoint(id, lat, lon):
w = WayPoint()
w.id = UniqueID(id)
w.position = GeoPoint(latitude=lat, longitude=lon)
return w
def makeWayPoint(id, lat, lon):
w = WayPoint()
w.id = UniqueID(id)
w.position = GeoPoint(latitude=lat, longitude=lon)
return w