Add laser_retro in Visual (#454)

Signed-off-by: Guillaume Doisy <[email protected]>
gazebosim · Jan 25, 2021 · b22a595 · b22a595
1 parent 0989bff
commit b22a595
Show file tree

Hide file tree

Showing 5 changed files with 117 additions and 14 deletions.
diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh
@@ -193,6 +193,24 @@ namespace sdf
     /// \param[in] _flags visibility flags
     public: void SetVisibilityFlags(uint32_t _flags);
 
+    /// \brief Set whether the lidar reflective intensity
+    /// has been specified.
+    /// \param[in] _laserRetro True if the lidar reflective intensity
+    /// has been set in the sdf.
+    public: void SetHasLaserRetro(bool _laserRetro);
+
+    /// \brief Get whether the lidar reflective intensity was set was set.
+    /// \return True if the lidar reflective intensity was set was set.
+    public: bool HasLaserRetro() const;
+
+    /// \brief Get the flidar reflective intensity.
+    /// \return The lidar reflective intensity.
+    public: double LaserRetro() const;
+
+    /// \brief Set the lidar reflective intensity.
+    /// \param[in] _laserRetro The lidar reflective intensity.
+    public: void SetLaserRetro(double _laserRetro);
+
     /// \brief Give the name of the xml parent of this object, to be used
     /// for resolving poses. This is private and is intended to be called by
     /// Link::SetPoseRelativeToGraph.

diff --git a/src/Visual.cc b/src/Visual.cc
@@ -69,6 +69,12 @@ class sdf::VisualPrivate
 
   /// \brief Visibility flags of a visual. Defaults to 0xFFFFFFFF
   public: uint32_t visibilityFlags = 4294967295u;
+
+  /// \brief True indicates the lidar reflective intensity was set.
+  public: bool hasLaserRetro{false};
+
+  /// \brief Lidar reflective intensity
+  public: double laserRetro = 0;
 };
 
 /////////////////////////////////////////////////
@@ -80,7 +86,9 @@ VisualPrivate::VisualPrivate(const VisualPrivate &_visualPrivate)
       poseRelativeTo(_visualPrivate.poseRelativeTo),
       geom(_visualPrivate.geom),
       sdf(_visualPrivate.sdf),
-      visibilityFlags(_visualPrivate.visibilityFlags)
+      visibilityFlags(_visualPrivate.visibilityFlags),
+      hasLaserRetro(_visualPrivate.hasLaserRetro),
+      laserRetro(_visualPrivate.laserRetro)
 {
   if (_visualPrivate.material)
   {
@@ -193,6 +201,12 @@ Errors Visual::Load(ElementPtr _sdf)
   Errors geomErr = this->dataPtr->geom.Load(_sdf->GetElement("geometry"));
   errors.insert(errors.end(), geomErr.begin(), geomErr.end());
 
+  // Load the lidar reflective intensity if it is given
+  if (_sdf->HasElement("laser_retro"))
+  {
+    this->SetLaserRetro(_sdf->Get<double>("laser_retro"));
+  }
+
   return errors;
 }
 
@@ -292,19 +306,6 @@ void Visual::SetGeom(const Geometry &_geom)
   this->dataPtr->geom = _geom;
 }
 
-/////////////////////////////////////////////////
-void Visual::SetXmlParentName(const std::string &_xmlParentName)
-{
-  this->dataPtr->xmlParentName = _xmlParentName;
-}
-
-/////////////////////////////////////////////////
-void Visual::SetPoseRelativeToGraph(
-    std::weak_ptr<const PoseRelativeToGraph> _graph)
-{
-  this->dataPtr->poseRelativeToGraph = _graph;
-}
-
 /////////////////////////////////////////////////
 sdf::SemanticPose Visual::SemanticPose() const
 {
@@ -344,3 +345,41 @@ void Visual::SetVisibilityFlags(uint32_t _flags)
 {
   this->dataPtr->visibilityFlags = _flags;
 }
+
+//////////////////////////////////////////////////
+void Visual::SetHasLaserRetro(bool _laserRetro)
+{
+  this->dataPtr->hasLaserRetro = _laserRetro;
+}
+
+//////////////////////////////////////////////////
+bool Visual::HasLaserRetro() const
+{
+  return this->dataPtr->hasLaserRetro;
+}
+
+//////////////////////////////////////////////////
+double Visual::LaserRetro() const
+{
+  return this->dataPtr->laserRetro;
+}
+
+//////////////////////////////////////////////////
+void Visual::SetLaserRetro(double _laserRetro)
+{
+  this->dataPtr->hasLaserRetro = true;
+  this->dataPtr->laserRetro = _laserRetro;
+}
+
+/////////////////////////////////////////////////
+void Visual::SetXmlParentName(const std::string &_xmlParentName)
+{
+  this->dataPtr->xmlParentName = _xmlParentName;
+}
+
+/////////////////////////////////////////////////
+void Visual::SetPoseRelativeToGraph(
+    std::weak_ptr<const PoseRelativeToGraph> _graph)
+{
+  this->dataPtr->poseRelativeToGraph = _graph;
+}
diff --git a/src/Visual_TEST.cc b/src/Visual_TEST.cc
@@ -272,3 +272,16 @@ TEST(DOMVisual, SetMaterial)
   EXPECT_EQ(ignition::math::Color(0.f, 0.1f, 0.9f),
             visual.Material()->Specular());
 }
+
+/////////////////////////////////////////////////
+TEST(DOMVisual, SetLaserRetro)
+{
+  sdf::Visual visual;
+  EXPECT_EQ(nullptr, visual.Element());
+  EXPECT_TRUE(visual.Name().empty());
+
+  visual.SetLaserRetro(150);
+
+  EXPECT_TRUE(visual.HasLaserRetro());
+  EXPECT_DOUBLE_EQ(150, visual.LaserRetro());
+}
diff --git a/test/integration/visual_dom.cc b/test/integration/visual_dom.cc
@@ -203,6 +203,30 @@ TEST(DOMVisual, Transparency)
   EXPECT_FLOAT_EQ(0.22f, vis1->Transparency());
 }
 
+//////////////////////////////////////////////////
+TEST(DOMVisual, LaserRetro)
+{
+  const std::string testFile =
+    sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf",
+        "shapes.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  EXPECT_TRUE(root.Load(testFile).empty());
+
+  const sdf::Model *model = root.ModelByIndex(0);
+  ASSERT_NE(nullptr, model);
+
+  const sdf::Link *link = model->LinkByIndex(0);
+  ASSERT_NE(nullptr, link);
+
+  const sdf::Visual *vis1 = link->VisualByName("sphere_vis_laser_retro");
+  ASSERT_NE(nullptr, vis1);
+
+  ASSERT_EQ(true, vis1->HasLaserRetro());
+  EXPECT_DOUBLE_EQ(1150, vis1->LaserRetro());
+}
+
 /////////////////////////////////////////////////
 TEST(DOMVisual, LoadModelFramesRelativeToJoint)
 {

diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf
@@ -153,6 +153,15 @@
           </sphere>
         </geometry>
       </visual>
+
+      <visual name="sphere_vis_laser_retro">
+        <laser_retro>1150</laser_retro>
+        <geometry>
+          <sphere>
+            <radius>0.5</radius>
+          </sphere>
+        </geometry>
+      </visual>
     </link>
   </model>
 </sdf>