Add LidarVisual point colors for Ogre1 (#124)

* Added Point colors for Ogre1 Signed-off-by: Mihir Kulkarni <[email protected]> * check if vector size equal Signed-off-by: Mihir Kulkarni <[email protected]> * default color of points set to Blue Signed-off-by: Mihir Kulkarni <[email protected]> * optimise to avoid color search Signed-off-by: Mihir Kulkarni <[email protected]> * windows error fix Signed-off-by: Mihir Kulkarni <[email protected]> * minor edit Signed-off-by: Mihir Kulkarni <[email protected]>
gazebosim · Aug 19, 2020 · 489bfc3 · 489bfc3
1 parent 5929c3f
commit 489bfc3
Show file tree

Hide file tree

Showing 7 changed files with 125 additions and 19 deletions.
diff --git a/include/ignition/rendering/LidarVisual.hh b/include/ignition/rendering/LidarVisual.hh
@@ -70,6 +70,12 @@ namespace ignition
       /// \param[in] _points Vector of points representing distance of the ray
       public: virtual void SetPoints(const std::vector<double> &_points) = 0;
 
+      /// \brief Set lidar points to be visualised
+      /// \param[in] _points Vector of points representing distance of the ray
+      /// \param[in] _colors Vector of colors for the rendered points
+      public: virtual void SetPoints(const std::vector<double> &_points,
+                        const std::vector<ignition::math::Color> &_colors) = 0;
+
       /// \brief Set minimum vertical angle
       /// \param[in] _minVerticalAngle Minimum vertical angle
       public: virtual void SetMinVerticalAngle(

diff --git a/include/ignition/rendering/base/BaseLidarVisual.hh b/include/ignition/rendering/base/BaseLidarVisual.hh
@@ -54,6 +54,11 @@ namespace ignition
       public: virtual void SetPoints(
               const std::vector<double> &_points) override;
 
+      // Documentation inherited
+      public: virtual void SetPoints(const std::vector<double> &_points,
+                            const std::vector<ignition::math::Color> &_colors)
+                            override;
+
       // Documentation inherited
       public: virtual void Update() override;
 
@@ -247,6 +252,14 @@ namespace ignition
       // no op
     }
 
+    /////////////////////////////////////////////////
+    template <class T>
+    void BaseLidarVisual<T>::SetPoints(const std::vector<double> &,
+                                const std::vector<ignition::math::Color> &)
+    {
+      // no op
+    }
+
     /////////////////////////////////////////////////
     template <class T>
     void BaseLidarVisual<T>::Init()

diff --git a/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh b/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh
@@ -60,6 +60,12 @@ namespace ignition
       public: virtual void SetPoints(
               const std::vector<double> &_points) override;
 
+      // Documentation inherited
+      // This only affects lidar visuals with type LVT_POINTS
+      public: virtual void SetPoints(const std::vector<double> &_points,
+                        const std::vector<ignition::math::Color> &_colors)
+                                    override;
+
       // Documentation inherited
       public: virtual void ClearPoints() override;
 

diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc
@@ -50,6 +50,9 @@ class ignition::rendering::OgreLidarVisualPrivate
   /// \brief The current lidar points data
   public: std::vector<double> lidarPoints;
 
+  /// \brief The colour of rendered points
+  public: std::vector<ignition::math::Color> pointColors;
+
   /// \brief True if new points data is received
   public: bool receivedData = false;
 };
@@ -149,6 +152,26 @@ void OgreLidarVisual::ClearVisualData()
 void OgreLidarVisual::SetPoints(const std::vector<double> &_points)
 {
   this->dataPtr->lidarPoints = _points;
+  this->dataPtr->pointColors.clear();
+  for (unsigned int i = 0u; i < this->dataPtr->lidarPoints.size(); ++i)
+  {
+    this->dataPtr->pointColors.push_back(ignition::math::Color::Blue);
+  }
+  this->dataPtr->receivedData = true;
+}
+
+//////////////////////////////////////////////////
+void OgreLidarVisual::SetPoints(const std::vector<double> &_points,
+                        const std::vector<ignition::math::Color> &_colors)
+{
+  if (_points.size() != _colors.size())
+  {
+    ignerr << "Unequal size of point and color vector."
+           << "Setting all point colors blue." << std::endl;
+    this->SetPoints(_points);
+  }
+  this->dataPtr->lidarPoints = _points;
+  this->dataPtr->pointColors = _colors;
   this->dataPtr->receivedData = true;
 }
 
@@ -216,6 +239,25 @@ void OgreLidarVisual::Update()
     return;
   }
 
+  #if (!(OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)))
+  // the Materials are assigned here to avoid repetitive search for materials
+  Ogre::MaterialPtr noHitRayStripsMat =
+                  Ogre::MaterialManager::getSingleton().getByName(
+                                                    "Lidar/LightBlueStrips");
+  Ogre::MaterialPtr rayLineMat =
+                  Ogre::MaterialManager::getSingleton().getByName(
+                                                    "Lidar/BlueRay");
+  Ogre::MaterialPtr hitRayStripsMat =
+                  Ogre::MaterialManager::getSingleton().getByName(
+                                                    "Lidar/BlueStrips");
+  Ogre::MaterialPtr deadZoneMat =
+                  Ogre::MaterialManager::getSingleton().getByName(
+                                                    "Lidar/TransBlack");
+  Ogre::MaterialPtr pointsMat =
+                  Ogre::MaterialManager::getSingleton().getByName(
+                                                    "PointCloudPoint");
+  #endif
+
   // Process each point from received data
   // Every line segment, and every triangle is saved separately,
   // as a pointer to a DynamicLine
@@ -236,10 +278,7 @@ void OgreLidarVisual::Update()
         #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7))
               line->setMaterial("Lidar/BlueRay");
         #else
-            Ogre::MaterialPtr mat =
-                Ogre::MaterialManager::getSingleton().getByName(
-                                                "Lidar/BlueRay");
-            line->setMaterial(mat);
+            line->setMaterial(rayLineMat);
         #endif
         std::shared_ptr<Ogre::MovableObject> mv =
                 std::dynamic_pointer_cast<Ogre::MovableObject>(line);
@@ -254,9 +293,7 @@ void OgreLidarVisual::Update()
           #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7))
             line->setMaterial("Lidar/LightBlueStrips");
           #else
-            mat = Ogre::MaterialManager::getSingleton().getByName(
-                                                    "Lidar/LightBlueStrips");
-            line->setMaterial(mat);
+            line->setMaterial(noHitRayStripsMat);
           #endif
           mv = std::dynamic_pointer_cast<Ogre::MovableObject>(line);
           this->Node()->attachObject(mv.get());
@@ -268,9 +305,7 @@ void OgreLidarVisual::Update()
           #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7))
             line->setMaterial("Lidar/TransBlack");
           #else
-            mat = Ogre::MaterialManager::getSingleton().getByName(
-                                                    "Lidar/TransBlack");
-            line->setMaterial(mat);
+            line->setMaterial(deadZoneMat);
           #endif
           mv = std::dynamic_pointer_cast<Ogre::MovableObject>(line);
           this->Node()->attachObject(mv.get());
@@ -284,9 +319,7 @@ void OgreLidarVisual::Update()
           #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7))
             line->setMaterial("Lidar/BlueStrips");
           #else
-            mat = Ogre::MaterialManager::getSingleton().getByName(
-                                                    "Lidar/BlueStrips");
-            line->setMaterial(mat);
+            line->setMaterial(hitRayStripsMat);
           #endif
           mv = std::dynamic_pointer_cast<Ogre::MovableObject>(line);
           this->Node()->attachObject(mv.get());
@@ -305,12 +338,9 @@ void OgreLidarVisual::Update()
                               new OgreDynamicLines(MT_POINTS));
 
         #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7))
-              line->setMaterial("Lidar/BlueRay");
+            line->setMaterial("PointCloudPoint");
         #else
-            Ogre::MaterialPtr mat =
-                  Ogre::MaterialManager::getSingleton().getByName(
-                                                    "Lidar/BlueRay");
-            line->setMaterial(mat);
+            line->setMaterial(pointsMat);
         #endif
         std::shared_ptr<Ogre::MovableObject> mv =
                 std::dynamic_pointer_cast<Ogre::MovableObject>(line);
@@ -418,7 +448,8 @@ void OgreLidarVisual::Update()
         {
           if (this->displayNonHitting || !inf)
           {
-            this->dataPtr->points[j]->AddPoint(inf ? noHitPt: pt);
+            this->dataPtr->points[j]->AddPoint(inf ? noHitPt: pt,
+                  this->dataPtr->pointColors[j * this->horizontalCount + i]);
           }
         }
         else

diff --git a/ogre/src/media/materials/programs/point_fs.glsl b/ogre/src/media/materials/programs/point_fs.glsl
@@ -0,0 +1,6 @@
+#version 120
+
+void main()
+{
+  gl_FragColor = gl_Color;
+}
diff --git a/ogre/src/media/materials/programs/point_vs.glsl b/ogre/src/media/materials/programs/point_vs.glsl
@@ -0,0 +1,14 @@
+#version 120
+
+// Generic vertex shader for point sprites
+// sets position and point size.
+// Works for perspective and orthographic projection.
+
+uniform mat4 worldviewproj_matrix;
+
+void main()
+{
+  gl_Position = worldviewproj_matrix * gl_Vertex;
+  gl_FrontColor = gl_Color;
+  gl_PointSize = 10 / gl_Position.w;
+}
diff --git a/ogre/src/media/materials/scripts/point_cloud_point.material b/ogre/src/media/materials/scripts/point_cloud_point.material
@@ -0,0 +1,30 @@
+
+vertex_program PointCloudVS glsl
+{
+  source point_vs.glsl
+
+  default_params
+  {
+    param_named_auto worldviewproj_matrix worldviewproj_matrix
+  }
+}
+
+fragment_program PointCloudFS glsl
+{
+  source point_fs.glsl
+
+}
+
+material PointCloudPoint
+{
+  technique
+  {
+    pass
+    {
+      point_size_attenuation on
+      point_sprites on
+      vertex_program_ref   PointCloudVS {}
+      fragment_program_ref PointCloudFS {}
+    }
+  }
+}