From 5d63caf7832327490b8d8feb92ef296847723914 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Mon, 18 Jun 2018 14:41:05 +0200 Subject: [PATCH 1/2] Add test to frame_info for failing behaviour --- rviz_default_plugins/CMakeLists.txt | 10 +++ .../displays/tf/frame_info_test.cpp | 87 +++++++++++++++++++ 2 files changed, 97 insertions(+) create mode 100644 rviz_default_plugins/test/rviz_default_plugins/displays/tf/frame_info_test.cpp diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 7f2c1e824..dff059945 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -295,6 +295,16 @@ if(BUILD_TESTING) target_link_libraries(fps_view_controller_test ${TEST_LINK_LIBRARIES}) endif() + ament_add_gmock(frame_info_test + test/rviz_default_plugins/displays/tf/frame_info_test.cpp + test/rviz_default_plugins/displays/display_test_fixture.cpp + test/rviz_default_plugins/scene_graph_introspection_helper.cpp + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_INSTALL_PREFIX} PATH=${CMAKE_INSTALL_PREFIX}/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_assimp_vendor/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_yaml_cpp_vendor/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_ogre_vendor/bin) + if(TARGET frame_info_test) + target_include_directories(frame_info_test PUBLIC ${TEST_INCLUDE_DIRS}) + target_link_libraries(frame_info_test ${TEST_LINK_LIBRARIES}) + endif() + ament_add_gmock(grid_cells_display_test test/rviz_default_plugins/displays/grid_cells/grid_cells_display_test.cpp test/rviz_default_plugins/displays/display_test_fixture.cpp diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/tf/frame_info_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/tf/frame_info_test.cpp new file mode 100644 index 000000000..8f211540f --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/tf/frame_info_test.cpp @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable : 4996) +#endif + +#include +#include +#include + +#ifdef _WIN32 +# pragma warning(pop) +#endif + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_common/properties/float_property.hpp" + +#include "rviz_default_plugins/displays/tf/frame_info.hpp" + +#include "test/rviz_rendering/scene_graph_introspection.hpp" +#include "../display_test_fixture.hpp" +#include "../../scene_graph_introspection_helper.hpp" + +using namespace ::testing; // NOLINT + +class FrameInfoTestFixture : public DisplayTestFixture +{ +public: + FrameInfoTestFixture() + { + frame_info_ = std::make_unique(nullptr); + } + + std::unique_ptr frame_info_; +}; + +TEST_F(FrameInfoTestFixture, updateArrow_makes_arrow_invisible_if_transform_has_no_translation) { + auto arrow = std::make_shared(scene_manager_); + frame_info_->parent_arrow_ = arrow.get(); + frame_info_->distance_to_parent_ = 0; + auto property = std::make_shared(); + property->setValue(true); + frame_info_->enabled_property_ = property.get(); + + auto arrows = rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()); + EXPECT_THAT(arrows, SizeIs(1)); + EXPECT_TRUE(rviz_default_plugins::arrowIsVisible(arrows[0])); + + frame_info_->updateParentArrow(Ogre::Vector3::ZERO, Ogre::Vector3::ZERO, 1); + + auto invisible_arrows = rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()); + EXPECT_THAT(invisible_arrows, SizeIs(1)); + EXPECT_FALSE(rviz_default_plugins::arrowIsVisible(invisible_arrows[0])); +} From f00f8bdd4a75839b11560dc65cebc48d47ef0faa Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Mon, 18 Jun 2018 14:08:01 +0200 Subject: [PATCH 2/2] Don't rely on failed orientation but look at distance explicitly --- .../src/rviz_default_plugins/displays/tf/frame_info.cpp | 5 ++++- .../src/rviz_default_plugins/displays/tf/tf_display.cpp | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.cpp index 1ed9a654e..a4c1f930d 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.cpp @@ -217,7 +217,8 @@ void FrameInfo::updateParentArrow( Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(direction); - if (!orient.isNaN()) { + if (direction.squaredLength() > 0 && !orient.isNaN()) { + setParentArrowVisible(true); distance_to_parent_ = distance; float head_length = (distance < 0.1f * scale) ? (0.1f * scale * distance) : 0.1f * scale; float shaft_length = distance - head_length; @@ -227,6 +228,8 @@ void FrameInfo::updateParentArrow( parent_arrow_->setPosition(position); parent_arrow_->setOrientation(orient); + } else { + setParentArrowVisible(false); } } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp index d41a31f07..614b6eca4 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp @@ -545,8 +545,8 @@ void TFDisplay::updateParentArrowIfTransformExists( { logTransformationException(frame->parent_, frame->name_); } else { - frame->updateParentArrow(position, parent_position, scale_property_->getFloat()); frame->setParentArrowVisible(show_arrows_property_->getBool()); + frame->updateParentArrow(position, parent_position, scale_property_->getFloat()); } }