@@ -567,22 +567,29 @@ def _update_and_publish(self) -> None:
567
567
assert self .camera_data is not None
568
568
max_map_radius = self .get_parameter ('max_map_radius' ).get_parameter_value ().integer_value
569
569
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.' )
584
573
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.' )
586
593
587
594
if self ._ortho_image_3d_msg is not None :
588
595
self ._ortho_image_3d_pub .publish (self ._ortho_image_3d_msg )
0 commit comments