From 742b4f893246ae079113cd5a86231790dc1ee53a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 11 May 2022 14:11:07 -0700 Subject: [PATCH 1/4] add support for lidar visibility mask Signed-off-by: Ian Chen --- ogre/src/OgreGpuRays.cc | 2 +- ogre2/src/Ogre2GpuRays.cc | 2 +- test/integration/gpu_rays.cc | 143 +++++++++++++++++++++++++++++++++++ 3 files changed, 145 insertions(+), 2 deletions(-) diff --git a/ogre/src/OgreGpuRays.cc b/ogre/src/OgreGpuRays.cc index 32e0e4d31..29673fc23 100644 --- a/ogre/src/OgreGpuRays.cc +++ b/ogre/src/OgreGpuRays.cc @@ -434,7 +434,7 @@ void OgreGpuRays::CreateGpuRaysTextures() vp->setSkiesEnabled(false); vp->setBackgroundColour( Ogre::ColourValue(this->dataMaxVal, 0.0, 1.0)); - vp->setVisibilityMask(IGN_VISIBILITY_ALL & + vp->setVisibilityMask(this->VisibilityMask() & ~(IGN_VISIBILITY_GUI | IGN_VISIBILITY_SELECTABLE)); } diff --git a/ogre2/src/Ogre2GpuRays.cc b/ogre2/src/Ogre2GpuRays.cc index 846cc33d3..76714acc2 100644 --- a/ogre2/src/Ogre2GpuRays.cc +++ b/ogre2/src/Ogre2GpuRays.cc @@ -961,7 +961,7 @@ void Ogre2GpuRays::Setup1stPass() passScene->setAllLoadActions(Ogre::LoadAction::Clear); passScene->setAllClearColours(Ogre::ColourValue(0, 0, 0)); // set camera custom visibility mask when rendering laser retro - passScene->mVisibilityMask = IGN_VISIBILITY_ALL & + passScene->mVisibilityMask = this->VisibilityMask() & ~Ogre2ParticleEmitter::kParticleVisibilityFlags; } diff --git a/test/integration/gpu_rays.cc b/test/integration/gpu_rays.cc index bd0d08864..f7b95846a 100644 --- a/test/integration/gpu_rays.cc +++ b/test/integration/gpu_rays.cc @@ -67,6 +67,9 @@ class GpuRaysTest: public testing::Test, // Test single ray box intersection public: void SingleRay(const std::string &_renderEngine); + + // Test and verify lidar visibilty mask and visual visibility flags + public: void Visibility(const std::string &_renderEngine); }; ///////////////////////////////////////////////// @@ -834,6 +837,140 @@ void GpuRaysTest::SingleRay(const std::string &_renderEngine) rendering::unloadEngine(engine->Name()); } +///////////////////////////////////////////////// +void GpuRaysTest::Visibility(const std::string &_renderEngine) +{ +#ifdef __APPLE__ + ignerr << "Skipping test for apple, see issue #35." << std::endl; + return; +#endif + + if (_renderEngine == "optix") + { + igndbg << "GpuRays visibility mask not supported yet in rendering engine: " + << _renderEngine << std::endl; + return; + } + + // Test GPU rays with 3 boxes in the world. + // then move all 3 boxes out of range and verify range values + + const double hMinAngle = -IGN_PI/2.0; + const double hMaxAngle = IGN_PI/2.0; + const double minRange = 0.1; + const double maxRange = 10.0; + const int hRayCount = 320; + const int vRayCount = 1; + + // create and populate scene + RenderEngine *engine = rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ScenePtr scene = engine->CreateScene("scene"); + ASSERT_TRUE(scene != nullptr); + +#if IGNITION_RENDERING_MAJOR_VERSION <= 6 + // HACK: Tell ign-rendering6 to listen to SetTime calls + scene->SetTime(std::chrono::nanoseconds(-1)); +#endif + + VisualPtr root = scene->RootVisual(); + + // Create first ray caster + ignition::math::Pose3d testPose(ignition::math::Vector3d(0, 0, 0.1), + ignition::math::Quaterniond::Identity); + + GpuRaysPtr gpuRays = scene->CreateGpuRays("gpu_rays_1"); + gpuRays->SetWorldPosition(testPose.Pos()); + gpuRays->SetWorldRotation(testPose.Rot()); + gpuRays->SetNearClipPlane(minRange); + gpuRays->SetFarClipPlane(maxRange); + gpuRays->SetAngleMin(hMinAngle); + gpuRays->SetAngleMax(hMaxAngle); + gpuRays->SetRayCount(hRayCount); + gpuRays->SetVisibilityMask(0x1011); + + gpuRays->SetVerticalRayCount(vRayCount); + root->AddChild(gpuRays); + + // Create a second ray caster rotated + ignition::math::Pose3d testPose2(ignition::math::Vector3d(0, 0, 0.1), + ignition::math::Quaterniond(IGN_PI/2.0, 0, 0)); + + // Create testing boxes + // box in the center + // GpuRays should see box because default flags have all bits set to 1 + ignition::math::Pose3d box01Pose(ignition::math::Vector3d(3, 0, 0.5), + ignition::math::Quaterniond::Identity); + VisualPtr visualBox1 = scene->CreateVisual("UnitBox1"); + visualBox1->AddGeometry(scene->CreateBox()); + visualBox1->SetWorldPosition(box01Pose.Pos()); + visualBox1->SetWorldRotation(box01Pose.Rot()); + root->AddChild(visualBox1); + + // box on the right of the first gpu rays caster + // GpuRays should see box because mask & flags evalutes to non-zero + ignition::math::Pose3d box02Pose(ignition::math::Vector3d(0, -5, 0.5), + ignition::math::Quaterniond::Identity); + VisualPtr visualBox2 = scene->CreateVisual("UnitBox2"); + visualBox2->AddGeometry(scene->CreateBox()); + visualBox2->SetWorldPosition(box02Pose.Pos()); + visualBox2->SetWorldRotation(box02Pose.Rot()); + visualBox2->SetVisibilityFlags(0x0010); + root->AddChild(visualBox2); + + // box on the left of the rays caster + // GpuRays should not see box because mask & flags evalutes to 0 + ignition::math::Pose3d box03Pose( + ignition::math::Vector3d(0, 5, 0.5), + ignition::math::Quaterniond::Identity); + VisualPtr visualBox3 = scene->CreateVisual("UnitBox3"); + visualBox3->AddGeometry(scene->CreateBox()); + visualBox3->SetWorldPosition(box03Pose.Pos()); + visualBox3->SetWorldRotation(box03Pose.Rot()); + visualBox3->SetVisibilityFlags(0x0100); + root->AddChild(visualBox3); + + // Verify rays caster range readings + // listen to new gpu rays frames + unsigned int channels = gpuRays->Channels(); + float *scan = new float[hRayCount * vRayCount * channels]; + common::ConnectionPtr c = + gpuRays->ConnectNewGpuRaysFrame( + std::bind(&::OnNewGpuRaysFrame, scan, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + + gpuRays->Update(); + scene->SetTime(scene->Time() + std::chrono::milliseconds(16)); + + int mid = static_cast(hRayCount/2) * channels; + int last = (hRayCount - 1) * channels; + double unitBoxSize = 1.0; + double expectedRangeAtMidPointBox1 = abs(box01Pose.Pos().X()) - unitBoxSize/2; + double expectedRangeAtMidPointBox2 = abs(box02Pose.Pos().Y()) - unitBoxSize/2; + + // rays caster 1 should see box01 and box02 + EXPECT_NEAR(scan[mid], expectedRangeAtMidPointBox1, LASER_TOL); + EXPECT_NEAR(scan[0], expectedRangeAtMidPointBox2, LASER_TOL); + EXPECT_FLOAT_EQ(scan[last], ignition::math::INF_F); + + c.reset(); + + delete [] scan; + + scan = nullptr; + + // Clean up + engine->DestroyScene(scene); + rendering::unloadEngine(engine->Name()); +} + ///////////////////////////////////////////////// TEST_P(GpuRaysTest, Configure) { @@ -864,6 +1001,12 @@ TEST_P(GpuRaysTest, SingleRay) SingleRay(GetParam()); } +///////////////////////////////////////////////// +TEST_P(GpuRaysTest, Visibility) +{ + Visibility(GetParam()); +} + INSTANTIATE_TEST_CASE_P(GpuRays, GpuRaysTest, RENDER_ENGINE_VALUES, From 5d619db8750c3bd0bcebe9d17a9bf68d6498fb88 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 11 May 2022 14:18:50 -0700 Subject: [PATCH 2/4] update test comment Signed-off-by: Ian Chen --- test/integration/gpu_rays.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/gpu_rays.cc b/test/integration/gpu_rays.cc index f7b95846a..434b02782 100644 --- a/test/integration/gpu_rays.cc +++ b/test/integration/gpu_rays.cc @@ -853,8 +853,8 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) } // Test GPU rays with 3 boxes in the world. - // then move all 3 boxes out of range and verify range values - + // One of the boxes has visibility flags set to a value that + // makes it invisible to the sensor const double hMinAngle = -IGN_PI/2.0; const double hMaxAngle = IGN_PI/2.0; const double minRange = 0.1; From 680bc44ee7124be8e93ee51e6e9eed6ff7d874f2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 11 May 2022 15:51:38 -0700 Subject: [PATCH 3/4] fix comment Signed-off-by: Ian Chen --- test/integration/gpu_rays.cc | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/test/integration/gpu_rays.cc b/test/integration/gpu_rays.cc index 434b02782..eea8c3009 100644 --- a/test/integration/gpu_rays.cc +++ b/test/integration/gpu_rays.cc @@ -881,7 +881,7 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) VisualPtr root = scene->RootVisual(); - // Create first ray caster + // Create ray caster ignition::math::Pose3d testPose(ignition::math::Vector3d(0, 0, 0.1), ignition::math::Quaterniond::Identity); @@ -898,10 +898,6 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) gpuRays->SetVerticalRayCount(vRayCount); root->AddChild(gpuRays); - // Create a second ray caster rotated - ignition::math::Pose3d testPose2(ignition::math::Vector3d(0, 0, 0.1), - ignition::math::Quaterniond(IGN_PI/2.0, 0, 0)); - // Create testing boxes // box in the center // GpuRays should see box because default flags have all bits set to 1 @@ -914,7 +910,7 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) root->AddChild(visualBox1); // box on the right of the first gpu rays caster - // GpuRays should see box because mask & flags evalutes to non-zero + // GpuRays should see box because mask & flags evaluates to non-zero ignition::math::Pose3d box02Pose(ignition::math::Vector3d(0, -5, 0.5), ignition::math::Quaterniond::Identity); VisualPtr visualBox2 = scene->CreateVisual("UnitBox2"); @@ -925,7 +921,7 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) root->AddChild(visualBox2); // box on the left of the rays caster - // GpuRays should not see box because mask & flags evalutes to 0 + // GpuRays should not see box because mask & flags evaluates to 0 ignition::math::Pose3d box03Pose( ignition::math::Vector3d(0, 5, 0.5), ignition::math::Quaterniond::Identity); @@ -955,7 +951,7 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) double expectedRangeAtMidPointBox1 = abs(box01Pose.Pos().X()) - unitBoxSize/2; double expectedRangeAtMidPointBox2 = abs(box02Pose.Pos().Y()) - unitBoxSize/2; - // rays caster 1 should see box01 and box02 + // rays caster should see box01 and box02 but not box03 EXPECT_NEAR(scan[mid], expectedRangeAtMidPointBox1, LASER_TOL); EXPECT_NEAR(scan[0], expectedRangeAtMidPointBox2, LASER_TOL); EXPECT_FLOAT_EQ(scan[last], ignition::math::INF_F); @@ -963,7 +959,6 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) c.reset(); delete [] scan; - scan = nullptr; // Clean up From d32ecb84233748efcebbe5ba8e965df835f40e54 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 12 May 2022 09:46:38 -0700 Subject: [PATCH 4/4] style Signed-off-by: Ian Chen --- test/integration/gpu_rays.cc | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/test/integration/gpu_rays.cc b/test/integration/gpu_rays.cc index eea8c3009..ecea19cd3 100644 --- a/test/integration/gpu_rays.cc +++ b/test/integration/gpu_rays.cc @@ -848,15 +848,15 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) if (_renderEngine == "optix") { igndbg << "GpuRays visibility mask not supported yet in rendering engine: " - << _renderEngine << std::endl; + << _renderEngine << std::endl; return; } // Test GPU rays with 3 boxes in the world. // One of the boxes has visibility flags set to a value that // makes it invisible to the sensor - const double hMinAngle = -IGN_PI/2.0; - const double hMaxAngle = IGN_PI/2.0; + const double hMinAngle = -IGN_PI / 2.0; + const double hMaxAngle = IGN_PI / 2.0; const double minRange = 0.1; const double maxRange = 10.0; const int hRayCount = 320; @@ -867,7 +867,7 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) if (!engine) { igndbg << "Engine '" << _renderEngine - << "' is not supported" << std::endl; + << "' is not supported" << std::endl; return; } @@ -945,11 +945,13 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine) gpuRays->Update(); scene->SetTime(scene->Time() + std::chrono::milliseconds(16)); - int mid = static_cast(hRayCount/2) * channels; + int mid = static_cast(hRayCount / 2) * channels; int last = (hRayCount - 1) * channels; double unitBoxSize = 1.0; - double expectedRangeAtMidPointBox1 = abs(box01Pose.Pos().X()) - unitBoxSize/2; - double expectedRangeAtMidPointBox2 = abs(box02Pose.Pos().Y()) - unitBoxSize/2; + double expectedRangeAtMidPointBox1 = + abs(box01Pose.Pos().X()) - unitBoxSize / 2; + double expectedRangeAtMidPointBox2 = + abs(box02Pose.Pos().Y()) - unitBoxSize / 2; // rays caster should see box01 and box02 but not box03 EXPECT_NEAR(scan[mid], expectedRangeAtMidPointBox1, LASER_TOL);