Fix lidar heightmap detection

ogre2/src/Ogre2GpuRays.cc

@@ -960,9 +960,11 @@ void Ogre2GpuRays::Setup1stPass()
           colorTargetDef->addPass(Ogre::PASS_SCENE));
       passScene->setAllLoadActions(Ogre::LoadAction::Clear);
       passScene->setAllClearColours(Ogre::ColourValue(0, 0, 0));
-      // set camera custom visibility mask when rendering laser retro
-      passScene->mVisibilityMask = this->VisibilityMask() &
-          ~Ogre2ParticleEmitter::kParticleVisibilityFlags;
+      // set visibility mask and '&' it with IGN_VISIBILITY_ALL (0x0FFFFFFF)
+      // to make sure the fist 4 bits are 0 otherwise lidar will not be able
+      // to detect heightmaps
+      passScene->mVisibilityMask = (this->VisibilityMask() & IGN_VISIBILITY_ALL)
+          & ~Ogre2ParticleEmitter::kParticleVisibilityFlags;
     }
 
     Ogre::CompositorTargetDef *particleTargetDef =

test/integration/gpu_rays.cc

@@ -19,11 +19,13 @@
 
 #include <ignition/common/Console.hh>
 #include <ignition/common/Image.hh>
+#include <ignition/common/ImageHeightmap.hh>
 #include <ignition/common/Filesystem.hh>
 
 #include "test_config.h"  // NOLINT(build/include)
 
 #include "ignition/rendering/GpuRays.hh"
+#include "ignition/rendering/Heightmap.hh"
 #include "ignition/rendering/ParticleEmitter.hh"
 #include "ignition/rendering/RenderEngine.hh"
 #include "ignition/rendering/RenderingIface.hh"
@@ -70,6 +72,14 @@ class GpuRaysTest: public testing::Test,
 
   // Test and verify lidar visibilty mask and visual visibility flags
   public: void Visibility(const std::string &_renderEngine);
+
+  // Test heightmap detection
+  public: void Heightmap(const std::string &_renderEngine);
+
+  /// \brief Path to test media files.
+  public: const std::string TEST_MEDIA_PATH{
+        common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+        "test", "media")};
 };
 
 /////////////////////////////////////////////////
@@ -968,6 +978,188 @@ void GpuRaysTest::Visibility(const std::string &_renderEngine)
   rendering::unloadEngine(engine->Name());
 }
 
+/////////////////////////////////////////////////
+void GpuRaysTest::Heightmap(const std::string &_renderEngine)
+{
+#ifdef __APPLE__
+  ignerr << "Skipping test for apple, see issue #35." << std::endl;
+  return;
+#endif
+
+    // \todo(anyone) test fails on github action (Bionic) but pass on other
+    // builds. Need to investigate further.
+    // Github action sets the MESA_GL_VERSION_OVERRIDE variable
+    // so check for this variable and disable test if it is set.
+
+  if (_renderEngine == "optix")
+  {
+    igndbg << "GpuRays visibility mask not supported yet in rendering engine: "
+           << _renderEngine << std::endl;
+    return;
+  }
+  else if (_renderEngine == "ogre2")
+  {
+#ifdef __linux__
+    std::string value;
+    bool result = common::env("MESA_GL_VERSION_OVERRIDE", value, true);
+    if (result && value == "3.3")
+    {
+      igndbg << "Test is run on machine with software rendering or mesa driver "
+             << "Skipping test. " << std::endl;
+      return;
+    }
+#endif
+  }
+
+  // Test GPU rays heightmap detection
+  const double hMinAngle = -IGN_PI / 8.0;
+  const double hMaxAngle = IGN_PI / 8.0;
+  const double minRange = 1;
+  const double maxRange = 100.0;
+  const int hRayCount = 20;
+  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 ray caster oriented to look down at the heightmap
+  math::Pose3d testPose(math::Vector3d(0, 0, 20),
+      math::Quaterniond(math::Vector3d(0, 1.570796, 0)));
+
+  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);
+  // set visibility mask
+  // note this is not the same as GZ_VISIBILITY_MASK
+  // which is 0x0FFFFFFF
+  gpuRays->SetVisibilityMask(0xFFFFFFFF);
+
+  gpuRays->SetVerticalRayCount(vRayCount);
+  root->AddChild(gpuRays);
+
+  // create heightmap
+
+  // Heightmap data
+  auto heightImage = common::joinPaths(TEST_MEDIA_PATH, "heightmap_bowl.png");
+  math::Vector3d size{100, 100, 10};
+  math::Vector3d position{0, 0, 0};
+  auto textureImage = common::joinPaths(TEST_MEDIA_PATH, "materials",
+      "textures", "texture.png");
+  auto normalImage = common::joinPaths(TEST_MEDIA_PATH, "materials",
+      "textures", "flat_normal.png");
+
+  auto data = std::make_shared<common::ImageHeightmap>();
+  data->Load(heightImage);
+
+  EXPECT_EQ(heightImage, data->Filename());
+
+  HeightmapDescriptor desc;
+  desc.SetData(data);
+  desc.SetSize(size);
+  desc.SetPosition(position);
+  desc.SetUseTerrainPaging(true);
+  desc.SetSampling(4u);
+
+  HeightmapTexture textureA;
+  textureA.SetSize(0.5);
+  textureA.SetDiffuse(textureImage);
+  textureA.SetNormal(normalImage);
+  desc.AddTexture(textureA);
+
+  HeightmapBlend blendA;
+  blendA.SetMinHeight(2.0);
+  blendA.SetFadeDistance(5.0);
+  desc.AddBlend(blendA);
+
+  HeightmapTexture textureB;
+  textureB.SetSize(0.5);
+  textureB.SetDiffuse(textureImage);
+  textureB.SetNormal(normalImage);
+  desc.AddTexture(textureB);
+
+  HeightmapBlend blendB;
+  blendB.SetMinHeight(4.0);
+  blendB.SetFadeDistance(5.0);
+  desc.AddBlend(blendB);
+
+  HeightmapTexture textureC;
+  textureC.SetSize(0.5);
+  textureC.SetDiffuse(textureImage);
+  textureC.SetNormal(normalImage);
+  desc.AddTexture(textureC);
+
+  auto heightmap = scene->CreateHeightmap(desc);
+  ASSERT_NE(nullptr, heightmap);
+
+  // Add to a visual
+  auto vis = scene->CreateVisual();
+  vis->AddGeometry(heightmap);
+  EXPECT_EQ(1u, vis->GeometryCount());
+  EXPECT_TRUE(vis->HasGeometry(heightmap));
+  EXPECT_EQ(heightmap, vis->GeometryByIndex(0));
+  scene->RootVisual()->AddChild(vis);
+
+  // 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));
+
+  for (unsigned int i = 0; i < hRayCount * channels; i += channels)
+  {
+    // range readings should not be inf and far lower than the max range
+    // it should be between ~15m and 20m
+    double range = scan[i];
+    EXPECT_LT(14.9, range);
+    EXPECT_GT(20.0, range);
+  }
+
+  c.reset();
+
+  delete [] scan;
+  scan = nullptr;
+
+  // \todo(iche033) this should not be needed once Ogre2Heightmap::Destroy is
+  // implemented.
+  if (_renderEngine == "ogre2")
+  {
+    vis->Destroy();
+    heightmap.reset();
+  }
+
+  // Clean up
+  engine->DestroyScene(scene);
+  rendering::unloadEngine(engine->Name());
+}
+
 /////////////////////////////////////////////////
 TEST_P(GpuRaysTest, Configure)
 {
@@ -1004,6 +1196,11 @@ TEST_P(GpuRaysTest, Visibility)
   Visibility(GetParam());
 }
 
+/////////////////////////////////////////////////
+TEST_P(GpuRaysTest, Heightmap)
+{
+  Heightmap(GetParam());
+}
 
 INSTANTIATE_TEST_CASE_P(GpuRays, GpuRaysTest,
     RENDER_ENGINE_VALUES,