def triangle_graph():
'initialize test data: three points with one-way segments between them'
r = RouteNetwork()
r.points.append(makeWayPoint('da7c242f-2efe-5175-9961-49cc621b80b9',
30.3840168, -97.72821))
r.points.append(makeWayPoint('812f1c08-a34b-5a21-92b9-18b2b0cf4950',
30.385729, -97.7316754))
r.points.append(makeWayPoint('2b093523-3b39-5c48-a11c-f7c65650c581',
30.3849643,-97.7269564))
s = makeSegment('41b7d2be-3c37-5a78-a7ac-248d939af379',
'da7c242f-2efe-5175-9961-49cc621b80b9',
'812f1c08-a34b-5a21-92b9-18b2b0cf4950')
geodesy.props.put(s, 'oneway', 'yes')
r.segments.append(s)
s = makeSegment('2df38f2c-202b-5ba5-be73-c6498cb4aafe',
'812f1c08-a34b-5a21-92b9-18b2b0cf4950',
'2b093523-3b39-5c48-a11c-f7c65650c581')
geodesy.props.put(s, 'oneway', 'yes')
r.segments.append(s)
s = makeSegment('8f2c2df3-be73-5ba5-202b-cb4aafec6498',
'2b093523-3b39-5c48-a11c-f7c65650c581',
'da7c242f-2efe-5175-9961-49cc621b80b9')
geodesy.props.put(s, 'oneway', 'yes')
r.segments.append(s)
return r
def grid_graph(min_lat, min_lon, max_lat, max_lon, step=0.001):
"""Generate a fully-connected rectangular grid.
:param min_lat: Initial latitude [degrees].
:param min_lon: Initial longitude [degrees].
:param max_lat: Latitude limit [degrees].
:param max_lon: Longitude limit [degrees].
:param step: Step size [degrees].
:returns: RouteNetwork message.
"""
nid = geodesy.gen_uuid.makeUniqueID(PKG_URL + '/test_network')
r = RouteNetwork(id=nid)
prev_row = None
for latitude in float_range(min_lat, max_lat, step):
prev_col = None
this_row = len(r.points)
for longitude in float_range(min_lon, max_lon, step):
fake_url = 'fake://point/' + str(latitude) + '/' + str(longitude)
pt_id = geodesy.gen_uuid.generate(fake_url)
r.points.append(makeWayPoint(pt_id, latitude, longitude))
if prev_col is not None:
s = makeSeg(prev_col, pt_id)
r.segments.append(s)
s = makeSeg(pt_id, prev_col)
r.segments.append(s)
prev_col = pt_id
if prev_row is not None:
prev_id = r.points[prev_row].id.uuid
s = makeSeg(prev_id, pt_id)
r.segments.append(s)
s = makeSeg(pt_id, prev_id)
r.segments.append(s)
prev_row += 1
prev_row = this_row
return r
def tiny_oneway():
'initialize test data: two points with one segment from first to second'
r = RouteNetwork()
r.points.append(makeWayPoint('da7c242f-2efe-5175-9961-49cc621b80b9',
30.3840168, -97.72821))
r.points.append(makeWayPoint('812f1c08-a34b-5a21-92b9-18b2b0cf4950',
30.385729, -97.7316754))
s = makeSegment('41b7d2be-3c37-5a78-a7ac-248d939af379',
'da7c242f-2efe-5175-9961-49cc621b80b9',
'812f1c08-a34b-5a21-92b9-18b2b0cf4950')
geodesy.props.put(s, 'oneway', 'yes')
r.segments.append(s)
return r
def tiny_graph():
'initialize test data: two points with segments between them'
r = RouteNetwork()
r.points.append(makeWayPoint('da7c242f-2efe-5175-9961-49cc621b80b9',
30.3840168, -97.72821))
r.points.append(makeWayPoint('812f1c08-a34b-5a21-92b9-18b2b0cf4950',
30.385729, -97.7316754))
r.segments.append(makeSegment('41b7d2be-3c37-5a78-a7ac-248d939af379',
'da7c242f-2efe-5175-9961-49cc621b80b9',
'812f1c08-a34b-5a21-92b9-18b2b0cf4950'))
r.segments.append(makeSegment('2df38f2c-202b-5ba5-be73-c6498cb4aafe',
'812f1c08-a34b-5a21-92b9-18b2b0cf4950',
'da7c242f-2efe-5175-9961-49cc621b80b9'))
return r
def test_empty_route_network(self):
pl = Planner(RouteNetwork())
self.assertEqual(len(pl.points), 0)
self.assertEqual(len(pl.graph.segments), 0)
def __init__(self):
"""
Constructor for AgrosPathGenerator
"""
rospy.loginfo('Initializing AgrosPathGenerator...\n\n'
'\t(Note: this class polls rosparams on\n' +
'\tconstruction. If you are setting any parameters\n' +
'\tmanually, please review the API and call the\n' +
'\tright functions (e.x. call update_ab_line()\n' +
'\tafter setting a and b with geographic points).\n')
self.path_pub = rospy.Publisher('~path', Path, queue_size=1)
self.route_pub = rospy.Publisher('~route_network',
RouteNetwork,
queue_size=1)
self.path = Path()
self.route_network = RouteNetwork()
self.route_network.id = unique_id.toMsg(unique_id.fromRandom())
self.frame_id = rospy.get_param('~frame_id', 'odom')
# Check UTM params -----------------------------------------------------
self.zone = rospy.get_param('~utm_zone', None)
if not self.zone:
rospy.logwarn('The ROS param ~utm_zone is required by this node')
self.band = rospy.get_param('~utm_band', None)
if not self.zone:
rospy.logwarn('The ROS param ~utm_band is required by this node')
# Configure the AB line, if given --------------------------------------
self.ab = None
self.azimuth = None
temp_a = rospy.get_param('~a', {}) # temp dict
if ('latitude' in temp_a and 'longitude' in temp_a
and 'altitude' in temp_a):
lat = temp_a['latitude']
lon = temp_a['longitude']
alt = temp_a['altitude']
self.a_geo = fromLatLong(lat, lon, alt)
temp_b = rospy.get_param('~b', {}) # temp dict
if ('latitude' in temp_b and 'longitude' in temp_b
and 'altitude' in temp_b):
lat = temp_b['latitude']
lon = temp_b['longitude']
alt = temp_b['altitude']
self.b_geo = fromLatLong(lat, lon, alt)
# Update the AB line itself as a shapely LineString
self.update_ab_line() # ab and ab_angle should be set after calling
# Configure the boundaries, if given -----------------------------------
self.perimeter = None
temp_boundary = rospy.get_param('~boundary', None) # temp dict
if 'points' in temp_boundary:
self.boundary_geo = []
for point in temp_boundary['points']:
if ('latitude' in point and 'longitude' in point
and 'altitude' in point):
lat = point['latitude']
lon = point['longitude']
alt = point['altitude']
wp = fromLatLong(lat, lon, alt)
self.boundary_geo.append(wp)
# Update the perimeter itself as a shapely LinearRing
self.update_perimeter() # perimeter should be set after calling
# Configure headlands, if given ----------------------------------------
self.headlands = None
self.headland_width = rospy.get_param('~headland_width', None)
# Update the headlands as a shapely LinearRing
self.update_headlands() # headlands should be set after calling
# Attempt to generate boustrophedon path -------------------------------
self.waypoints = None
self.segments = None
self.tool_width = rospy.get_param('~tool_width', None)
temp_entry = rospy.get_param('~entry_position', None)
if ('latitude' in temp_entry and 'longitude' in temp_entry
and 'altitude' in temp_entry):
lat = temp_entry['latitude']
lon = temp_entry['longitude']
alt = temp_entry['altitude']
self.entry_geo = fromLatLong(lat, lon, alt)
# Generate a Boustrophedon patten if all other properties are updated
self.generate_boustrophedon()
# Convert the Euclidean path back to geographic coordinates ------------
self.update_path()
self.update_geo_path()
# Perform Visualization using matplotlib -------------------------------
self.visualize = rospy.get_param('~visualize', False)
if self.visualize:
self.plot()
