From 711d378596de116bc188647679530fe96cf295e6 Mon Sep 17 00:00:00 2001 From: Stefan Kaiser Date: Wed, 17 Jun 2020 15:38:33 +0200 Subject: [PATCH] Fix cleanup bug braking the navsat-tile transforms (#84) --- CHANGELOG.rst | 4 ++++ src/aerialmap_display.cpp | 14 ++++++++------ src/aerialmap_display.h | 2 +- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 33d3bb0..fa67131 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package rviz_satellite ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +------------ +* Fix cleanup bug breaking the navsat-tile transforms (#84, #85) + 3.0.0 (2020-05-26) ------------------ * Code cleanup (#76, #75) diff --git a/src/aerialmap_display.cpp b/src/aerialmap_display.cpp index 26170bc..90213ab 100644 --- a/src/aerialmap_display.cpp +++ b/src/aerialmap_display.cpp @@ -374,7 +374,7 @@ void AerialMapDisplay::updateCenterTile(sensor_msgs::NavSatFixConstPtr const& ms } // check if update is necessary - auto const tile_coordinates = fromWGSCoordinate({ msg->latitude, msg->longitude }, zoom_); + TileCoordinate const tile_coordinates = fromWGSCoordinate({ msg->latitude, msg->longitude }, zoom_); TileId const new_center_tile_id{ tile_url_, tile_coordinates, zoom_ }; bool const center_tile_changed = (!center_tile_ || !(new_center_tile_id == *center_tile_)); @@ -636,15 +636,17 @@ void AerialMapDisplay::transformTileToMapFrame() return; } - auto const center_tile = center_tile_->coord; + // FIXME: note the template! this is different from center_tile_.coord, otherwise we could just use that + // since center_tile_ and ref_fix_ are in sync + auto const ref_fix_tile_coords = fromWGSCoordinate({ ref_fix_->latitude, ref_fix_->longitude }, zoom_); // In assembleScene() we shift the AerialMap so that the center tile's left-bottom corner has the coordinate (0,0). // Therefore we can calculate the NavSatFix coordinate (in the AerialMap frame) by just looking at the fractional part // of the coordinate. That is we calculate the offset from the left bottom corner of the center tile. - auto const center_tile_offset_x = center_tile.x - std::floor(center_tile.x); + auto const center_tile_offset_x = ref_fix_tile_coords.x - std::floor(ref_fix_tile_coords.x); // In assembleScene() the tiles are created so that the texture is flipped along the y coordinate. Since we want to // calculate the positions of the center tile, we also need to flip the texture's v coordinate here. - auto const center_tile_offset_y = 1 - (center_tile.y - std::floor(center_tile.y)); + auto const center_tile_offset_y = 1 - (ref_fix_tile_coords.y - std::floor(ref_fix_tile_coords.y)); double const tile_w_h_m = getTileWH(ref_fix_->latitude, zoom_); ROS_DEBUG_NAMED("rviz_satellite", "Tile resolution is %.1fm", tile_w_h_m); @@ -653,7 +655,7 @@ void AerialMapDisplay::transformTileToMapFrame() auto const t_centertile_navsat = Ogre::Vector3(center_tile_offset_x * tile_w_h_m, center_tile_offset_y * tile_w_h_m, 0); - t_centertile_map = t_navsat_map - t_centertile_navsat; + t_centertile_map_ = t_navsat_map - t_centertile_navsat; } bool AerialMapDisplay::getMapTransform(std::string const& query_frame, ros::Time const& timestamp, @@ -718,7 +720,7 @@ void AerialMapDisplay::transformMapTileToFixedFrame() setStatus(::rviz::StatusProperty::Ok, "Transform", "Transform OK"); // the translation of the tile w.r.t. the fixed-frame - auto const t_centertile_fixed = t_fixed_map + o_fixed_map * t_centertile_map; + auto const t_centertile_fixed = t_fixed_map + o_fixed_map * t_centertile_map_; scene_node_->setPosition(t_centertile_fixed); scene_node_->setOrientation(o_fixed_map); diff --git a/src/aerialmap_display.h b/src/aerialmap_display.h index 4c317eb..5751653 100644 --- a/src/aerialmap_display.h +++ b/src/aerialmap_display.h @@ -186,7 +186,7 @@ protected Q_SLOTS: /// Last request()ed tile id (which is the center tile) boost::optional center_tile_{ boost::none }; /// translation of the center-tile w.r.t. the map frame - Ogre::Vector3 t_centertile_map{ Ogre::Vector3::ZERO }; + Ogre::Vector3 t_centertile_map_{ Ogre::Vector3::ZERO }; /// the map frame, rigidly attached to the world with ENU convention - see https://www.ros.org/reps/rep-0105.html#map std::string static const MAP_FRAME; };