From 9351f5c7868450a5f9041e76e4400bcc0221432d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 28 Jul 2020 07:47:55 +0200 Subject: [PATCH] [backport Gazebo7] Fixed crash when collision size is zero (#2769) * Fixed crash when collision size is zero Signed-off-by: ahcorde * Using ignition::math::isnan Signed-off-by: ahcorde * Improved error message Signed-off-by: ahcorde * fixed method to get the name of the visual Signed-off-by: ahcorde * Fixed else brackets Signed-off-by: ahcorde * [Gazebo 9] Added test to check collisions equal to zero (#2788) * Added test to check collisions equal to zero Signed-off-by: ahcorde * Included feedback Signed-off-by: ahcorde * make linters happy Signed-off-by: ahcorde * Update Visual_TEST.cc --- gazebo/rendering/Visual.cc | 15 +++++-- gazebo/rendering/Visual_TEST.cc | 36 +++++++++++++++ worlds/collision_zero.world | 79 +++++++++++++++++++++++++++++++++ 3 files changed, 127 insertions(+), 3 deletions(-) create mode 100644 worlds/collision_zero.world diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc index a200e71128..7ba675cf6f 100644 --- a/gazebo/rendering/Visual.cc +++ b/gazebo/rendering/Visual.cc @@ -793,9 +793,18 @@ void Visual::SetScale(const math::Vector3 &_scale) this->dataPtr->scale = _scale.Ign(); - this->dataPtr->sceneNode->setScale( - Conversions::Convert(math::Vector3(this->dataPtr->scale))); - + if (!ignition::math::isnan(this->dataPtr->scale.X()) + && !ignition::math::isnan(this->dataPtr->scale.Y()) + && !ignition::math::isnan(this->dataPtr->scale.Z())) + { + this->dataPtr->sceneNode->setScale( + Conversions::Convert(this->dataPtr->scale)); + } + else + { + gzerr << GetName() << ": Size of the collision contains one or several zeros." << + " Collisions may not visualize properly." << std::endl; + } // Scale selection object in case we have one attached. Other children were // scaled from UpdateGeomSize for (auto child : this->dataPtr->children) diff --git a/gazebo/rendering/Visual_TEST.cc b/gazebo/rendering/Visual_TEST.cc index 9803ed9913..6e03e8c43d 100644 --- a/gazebo/rendering/Visual_TEST.cc +++ b/gazebo/rendering/Visual_TEST.cc @@ -1514,6 +1514,42 @@ TEST_F(Visual_TEST, ConvertVisualType) rendering::Visual::ConvertVisualType(msgs::Visual::PHYSICS)); } +TEST_F(Visual_TEST, CollisionZero) +{ + // This test checks that there isn't a segmentation fault when inserting + // zero collision geometries. + // Load a world containing 3 simple shapes with collision geometry equals + // to zero. + Load("worlds/collision_zero.world"); + + // Get the scene + gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene(); + ASSERT_NE(scene, nullptr); + + // Wait until all models are inserted + int sleep = 0; + int maxSleep = 10; + rendering::VisualPtr box, sphere, cylinder; + while ((!box || !sphere || !cylinder) && sleep < maxSleep) + { + event::Events::preRender(); + event::Events::render(); + event::Events::postRender(); + + box = scene->GetVisual("box"); + cylinder = scene->GetVisual("cylinder"); + sphere = scene->GetVisual("sphere"); + common::Time::MSleep(1000); + sleep++; + } + scene->ShowCollisions(true); + // box + ASSERT_NE(box, nullptr); + // cylinder + ASSERT_NE(cylinder, nullptr); + // sphere + ASSERT_NE(sphere, nullptr); +} ///////////////////////////////////////////////// TEST_F(Visual_TEST, Scale) { diff --git a/worlds/collision_zero.world b/worlds/collision_zero.world new file mode 100644 index 0000000000..76e420119b --- /dev/null +++ b/worlds/collision_zero.world @@ -0,0 +1,79 @@ + + + + + model://ground_plane + + + model://sun + + + 0 0 0.5 0 0 0 + + + + + 0 0 0 + + + + + + + 1 1 1 + + + + + + + + + + 0 1.5 0.5 0 0 0 + + + + + 0 + + + + + + + 0.5 + + + + + + + + + + 0 -1.5 0.5 0 1.5707 0 + + + + + 0 + 0 + + + + + + + 0.5 + 1.0 + + + + + + + + + +