Skip to content

Commit 3138e00

Browse files
authored
Merge pull request #29 from hmakelin/hmakelin-map-node-update-publish-fix
Add max radius validation to map node _update_and_publish method
2 parents 0af8388 + 6b17a0b commit 3138e00

File tree

1 file changed

+22
-15
lines changed

1 file changed

+22
-15
lines changed

gisnav/nodes/map_node.py

Lines changed: 22 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -567,22 +567,29 @@ def _update_and_publish(self) -> None:
567567
assert self.camera_data is not None
568568
max_map_radius = self.get_parameter('max_map_radius').get_parameter_value().integer_value
569569
map_radius = get_dynamic_map_radius(self.camera_data, max_map_radius, self._DEM_REQUEST_ALTITUDE)
570-
xy = GeoPt(x=self._home_geopoint.position.longitude, y=self._home_geopoint.position.latitude)
571-
map_candidate = GeoSquare(xy, map_radius)
572-
573-
bbox = BBox(*map_candidate.bounds)
574-
if self.map_size_with_padding is not None:
575-
self.get_logger().info(f'Requesting DEM for home/local frame origin (assumed same!).')
576-
img, dem = self._get_map(bbox, self.map_size_with_padding)
577-
self._home_dem = MapData(bbox=bbox, image=Img(img), elevation=Img(dem))
578-
579-
# TODO: assumes that this local_frame_origin is the starting location, same that was used for the request
580-
# --> not strictly true even if it works for the simulation
581-
if self._origin_dem_altitude is None:
582-
if self._home_geopoint is not None:
583-
self._origin_dem_altitude = self._terrain_altitude_at_position(xy, local_origin=True)
570+
if map_radius <= 0:
571+
self.get_logger().warn(f'Could not determine valid map radius ({map_radius}), skipping requesting DEM '
572+
f'for home.')
584573
else:
585-
self.get_logger().warn('Required map size unknown, skipping requesting DEM for home.')
574+
assert -180 <= abs(self._home_geopoint.position.longitude) <= 180
575+
assert -90 <= abs(self._home_geopoint.position.latitude) <= 90
576+
assert_type(map_radius, float)
577+
xy = GeoPt(x=self._home_geopoint.position.longitude, y=self._home_geopoint.position.latitude)
578+
map_candidate = GeoSquare(xy, map_radius)
579+
580+
bbox = BBox(*map_candidate.bounds)
581+
if self.map_size_with_padding is not None:
582+
self.get_logger().info(f'Requesting DEM for home/local frame origin (assumed same!).')
583+
img, dem = self._get_map(bbox, self.map_size_with_padding)
584+
self._home_dem = MapData(bbox=bbox, image=Img(img), elevation=Img(dem))
585+
586+
# TODO: assumes that this local_frame_origin is the starting location, same that was used for the
587+
# request --> not strictly true even if it works for the simulation
588+
if self._origin_dem_altitude is None:
589+
if self._home_geopoint is not None:
590+
self._origin_dem_altitude = self._terrain_altitude_at_position(xy, local_origin=True)
591+
else:
592+
self.get_logger().warn('Required map size unknown, skipping requesting DEM for home.')
586593

587594
if self._ortho_image_3d_msg is not None:
588595
self._ortho_image_3d_pub.publish(self._ortho_image_3d_msg)

0 commit comments

Comments
 (0)