Check if key exists in gpu lidar's user data

include/ignition/rendering/Node.hh

@@ -332,6 +332,11 @@ namespace ignition
       /// \return Value in any type. If _key does not exist for the node, an
       /// empty variant is returned (i.e., no data).
       public: virtual Variant UserData(const std::string &_key) const = 0;
+
+      /// \brief Check if node has custom data
+      /// \param[in] _key Unique key
+      /// \return True if node has custom data with the specified key
+      public: virtual bool HasUserData(const std::string &_key) const = 0;
     };
     }
   }

include/ignition/rendering/base/BaseNode.hh

@@ -180,6 +180,9 @@ namespace ignition
       // Documentation inherited
       public: virtual Variant UserData(const std::string &_key) const override;
 
+      // Documentation inherited
+      public: virtual bool HasUserData(const std::string &_key) const override;
+
       protected: virtual void PreRenderChildren();
 
       protected: virtual math::Pose3d RawLocalPose() const = 0;
@@ -666,12 +669,14 @@ namespace ignition
     }
     }
 
+    //////////////////////////////////////////////////
     template <class T>
     void BaseNode<T>::SetUserData(const std::string &_key, Variant _value)
     {
      this->userData[_key] = _value;
     }
 
+    //////////////////////////////////////////////////
     template <class T>
     Variant BaseNode<T>::UserData(const std::string &_key) const
     {
@@ -681,6 +686,13 @@ namespace ignition
        value = it->second;
      return value;
     }
+
+    //////////////////////////////////////////////////
+    template <class T>
+    bool BaseNode<T>::HasUserData(const std::string &_key) const
+    {
+      return this->userData.find(_key) != this->userData.end();
+    }
   }
 }
 #endif

ogre2/src/Ogre2GpuRays.cc

@@ -240,58 +240,58 @@ void Ogre2LaserRetroMaterialSwitcher::cameraPreRenderScene(
       Ogre2VisualPtr ogreVisual =
           std::dynamic_pointer_cast<Ogre2Visual>(result);
 
-      // get laser_retro
-      Variant tempLaserRetro = ogreVisual->UserData(laserRetroKey);
-
-      float retroValue = -1.0;
-      try
-      {
-        retroValue = std::get<float>(tempLaserRetro);
-      }
-      catch(...)
+      if (ogreVisual->HasUserData(laserRetroKey))
       {
+        // get laser_retro
+        Variant tempLaserRetro = ogreVisual->UserData(laserRetroKey);
+
+        float retroValue = -1.0;
         try
         {
-          retroValue = std::get<double>(tempLaserRetro);
+          retroValue = std::get<float>(tempLaserRetro);
         }
         catch(...)
         {
           try
           {
-            retroValue = std::get<int>(tempLaserRetro);
+            retroValue = std::get<double>(tempLaserRetro);
           }
-          catch(std::bad_variant_access &e)
+          catch(...)
           {
-            ignerr << "Error casting user data: " << e.what() << "\n";
-            retroValue = -1.0;
+            try
+            {
+              retroValue = std::get<int>(tempLaserRetro);
+            }
+            catch(std::bad_variant_access &e)
+            {
+              ignerr << "Error casting user data: " << e.what() << "\n";
+              retroValue = -1.0;
+            }
           }
         }
-      }
 
-      // only accept positive laser retro value
-      if (retroValue >= 0)
-      {
-        // set visibility flag so the camera can see it
-        item->addVisibilityFlags(0x01000000);
-        for (unsigned int i = 0; i < item->getNumSubItems(); ++i)
+        // only accept positive laser retro value
+        if (retroValue >= 0)
         {
-          Ogre::SubItem *subItem = item->getSubItem(i);
-          if (!subItem->hasCustomParameter(this->customParamIdx))
+          for (unsigned int i = 0; i < item->getNumSubItems(); ++i)
           {
-            // limit laser retro value to 2000 (as in gazebo)
-            if (retroValue > 2000.0)
+            Ogre::SubItem *subItem = item->getSubItem(i);
+            if (!subItem->hasCustomParameter(this->customParamIdx))
             {
-              retroValue = 2000.0;
+              // limit laser retro value to 2000 (as in gazebo)
+              if (retroValue > 2000.0)
+              {
+                retroValue = 2000.0;
+              }
+              float color = retroValue / 2000.0;
+              subItem->setCustomParameter(this->customParamIdx,
+                  Ogre::Vector4(color, color, color, 1.0));
             }
-            float color = retroValue / 2000.0;
+            Ogre::HlmsDatablock *datablock = subItem->getDatablock();
+            this->datablockMap[subItem] = datablock;
 
-            subItem->setCustomParameter(this->customParamIdx,
-                Ogre::Vector4(color, color, color, 1.0));
+            subItem->setMaterial(this->laserRetroSourceMaterial);
           }
-          Ogre::HlmsDatablock *datablock = subItem->getDatablock();
-          this->datablockMap[subItem] = datablock;
-
-          subItem->setMaterial(this->laserRetroSourceMaterial);
         }
       }
     }
@@ -879,7 +879,17 @@ 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 = 0x01000000 &
+      // todo(anyone) currently in this color + depth pass, lidar sees all
+      // objects, including ones without laser_retro set. So the lidar outputs
+      // data containing depth data + some non-zero retro value for all
+      // objects. If we want to output a retro value of zero for objects
+      // without laser_retro, one possible approach could be to separate out
+      // the color and depth pass:
+      // 1: color pass that see only objects with laser_retro by using custom
+      //    visibility mask
+      // 2: depth pass that see all objects
+      // Then assemble these data in the final quad pass.
+      passScene->mVisibilityMask = IGN_VISIBILITY_ALL &
           ~Ogre2ParticleEmitter::kParticleVisibilityFlags;
     }
 
@@ -929,7 +939,7 @@ void Ogre2GpuRays::Setup1stPass()
           particleTargetDef->addPass(Ogre::PASS_SCENE));
       passScene->setAllLoadActions(Ogre::LoadAction::Clear);
       passScene->setAllClearColours(Ogre::ColourValue::Black);
-      // set camera custom visibility mask when rendering laser retro
+      // set camera custom visibility mask when rendering particles
       passScene->mVisibilityMask =
           Ogre2ParticleEmitter::kParticleVisibilityFlags;
     }
@@ -1197,7 +1207,7 @@ void Ogre2GpuRays::CreateGpuRaysTextures()
 /////////////////////////////////////////////////
 void Ogre2GpuRays::UpdateRenderTarget1stPass()
 {
-  Ogre::vector<Ogre::TextureGpu*>::type swappedTargets;
+  Ogre::vector<Ogre::TextureGpu *>::type swappedTargets;
   swappedTargets.reserve(2u);
 
   // update the compositors
@@ -1226,7 +1236,7 @@ void Ogre2GpuRays::UpdateRenderTarget2ndPass()
   this->dataPtr->ogreCompositorWorkspace2nd->_update();
   this->dataPtr->ogreCompositorWorkspace2nd->_endUpdate(false);
 
-  Ogre::vector<Ogre::TextureGpu*>::type swappedTargets;
+  Ogre::vector<Ogre::TextureGpu *>::type swappedTargets;
   swappedTargets.reserve(2u);
   this->dataPtr->ogreCompositorWorkspace2nd->_swapFinalTarget(swappedTargets);
 }

src/Visual_TEST.cc

@@ -325,42 +325,54 @@ void VisualTest::UserData(const std::string &_renderEngine)
   // int
   std::string intKey = "int";
   int intValue = 1998;
+  EXPECT_FALSE(visual->HasUserData(intKey));
   visual->SetUserData(intKey, intValue);
+  EXPECT_TRUE(visual->HasUserData(intKey));
   Variant value = visual->UserData(intKey);
   EXPECT_EQ(intValue, std::get<int>(value));
 
   // float
   std::string floatKey = "float";
   float floatValue = 1.345f;
+  EXPECT_FALSE(visual->HasUserData(floatKey));
   visual->SetUserData(floatKey, floatValue);
+  EXPECT_TRUE(visual->HasUserData(floatKey));
   value = visual->UserData(floatKey);
   EXPECT_FLOAT_EQ(floatValue, std::get<float>(value));
 
   // double
   std::string doubleKey = "double";
   double doubleValue = -0.123;
+  EXPECT_FALSE(visual->HasUserData(doubleKey));
   visual->SetUserData(doubleKey, doubleValue);
+  EXPECT_TRUE(visual->HasUserData(doubleKey));
   value = visual->UserData(doubleKey);
   EXPECT_DOUBLE_EQ(doubleValue, std::get<double>(value));
 
   // string
   std::string stringKey = "string";
   std::string stringValue = "test_string";
+  EXPECT_FALSE(visual->HasUserData(stringKey));
   visual->SetUserData(stringKey, stringValue);
+  EXPECT_TRUE(visual->HasUserData(stringKey));
   value = visual->UserData(stringKey);
   EXPECT_EQ(stringValue, std::get<std::string>(value));
 
   // bool
   std::string boolKey = "bool";
   bool boolValue = true;
+  EXPECT_FALSE(visual->HasUserData(boolKey));
   visual->SetUserData(boolKey, boolValue);
+  EXPECT_TRUE(visual->HasUserData(boolKey));
   value = visual->UserData(boolKey);
   EXPECT_EQ(boolValue, std::get<bool>(value));
 
   // unsigned int
   std::string unsignedIntKey = "unsignedInt";
   unsigned int unsignedIntValue = 5u;
+  EXPECT_FALSE(visual->HasUserData(unsignedIntKey));
   visual->SetUserData(unsignedIntKey, unsignedIntValue);
+  EXPECT_TRUE(visual->HasUserData(unsignedIntKey));
   value = visual->UserData(unsignedIntKey);
   EXPECT_EQ(unsignedIntValue, std::get<unsigned int>(value));
 
@@ -372,6 +384,7 @@ void VisualTest::UserData(const std::string &_renderEngine)
   EXPECT_FALSE(std::holds_alternative<std::string>(value));
   EXPECT_FALSE(std::holds_alternative<bool>(value));
   EXPECT_FALSE(std::holds_alternative<unsigned int>(value));
+  EXPECT_EQ(0u, value.index());
 
   // test invalid access
   EXPECT_THROW(

test/integration/gpu_rays.cc

@@ -236,7 +236,7 @@ void GpuRaysTest::RaysUnitBox(const std::string &_renderEngine)
   root->AddChild(gpuRays2);
 
   // Laser retro test values
-  double laserRetro1 = 2000;
+  double laserRetro1 = 1500;
   double laserRetro2 = 1000;
   std::string userDataKey = "laser_retro";
 
@@ -251,8 +251,6 @@ void GpuRaysTest::RaysUnitBox(const std::string &_renderEngine)
   visualBox1->SetUserData(userDataKey, laserRetro1);
   root->AddChild(visualBox1);
 
-
-
   // box on the right of the first gpu rays caster
   ignition::math::Pose3d box02Pose(ignition::math::Vector3d(0, -5, 0.5),
                                    ignition::math::Quaterniond::Identity);