Add flag to disable ignoring entities in lidar link (#39)

Author: msz-rai

README.md

@@ -86,8 +86,15 @@ RGLServerPlugin contains two plugins: `RGLServerPluginManager` and `RGLServerPlu
 
 ## How to include RGLServerPluginManager in your sdf:
 ```xml
-<plugin filename="RGLServerPluginManager" name="rgl::RGLServerPluginManager"></plugin>
+<plugin name='rgl::RGLServerPluginManager' filename='RGLServerPluginManager'>
+    <do_ignore_entities_in_lidar_link>true</do_ignore_entities_in_lidar_link>
+</plugin>
 ```
+
+### Parameters description:
+- **do_ignore_entities_in_lidar_link** - if enabled, all entities attached to the same \<link\> as lidar will be ignored from raycasting. It could be useful when a visual representation of the sensor is added. (optional, default: true) \
+*Note: It has been noticed that when the lidar link is chained to another link with a joint component, the entity tree is simplified and the parent link becomes the lidar link. In such case, the whole robot could be ignored from raycasting. Yet to be resolved.*
+
 This is a global plugin and should be included only once per sdf, preferably inside the world entity. RGLServerPluginManager is responsible for synchronizing the scene between Gazebo and GPU (CUDA). At the moment manager handles all primitive geometry types (Box, Capsule, Cylinder, Ellipsoid, Sphere), planes, meshes and submeshes.
 
 ![](docs/images/RGLServerPluginManager.png)
@@ -109,7 +116,6 @@ Inside the link entity in your model, add a custom sensor:
   </plugin>
 </sensor>
 ```
-*Note: All entities attached to the same \<link\> as lidar will be ignored from raycasting. This enables an adding visual representation of the sensor.*
 
 ### Parameters description:
 - **range** - the minimum and maximum range that the hits will be registered (in meters).

RGLServerPlugin/include/RGLServerPluginManager.hh

@@ -60,6 +60,9 @@ private:
     // contains pointers to all entities that were loaded to rgl (as well as to their meshes)
     std::unordered_map<gz::sim::Entity, std::pair<rgl_entity_t, rgl_mesh_t>> entitiesInRgl;
 
+    // whether to ignore entities attached to the same link as the lidar
+    bool doIgnoreEntitiesInLidarLink{true};
+
     // the entity ids, that the lidars are attached to
     std::unordered_set<gz::sim::Entity> lidarEntities;
 

RGLServerPlugin/src/RGLServerPluginManager.cc

@@ -17,6 +17,7 @@
 
 #include "RGLServerPluginManager.hh"
 
+#define PARAM_DO_IGNORE_ENTITIES_IN_LIDAR_LINK_ID "do_ignore_entities_in_lidar_link"
 
 GZ_ADD_PLUGIN(
     rgl::RGLServerPluginManager,
@@ -32,14 +33,18 @@ namespace rgl
 
 void RGLServerPluginManager::Configure(
         const gz::sim::Entity& entity,
-        const std::shared_ptr<const sdf::Element>&,
+        const std::shared_ptr<const sdf::Element>& sdf,
         gz::sim::EntityComponentManager& ecm,
         gz::sim::EventManager& evm)
 {
     ValidateRGLVersion();
     if (!CheckRGL(rgl_configure_logging(RGL_LOG_LEVEL_ERROR, nullptr, true))) {
         gzerr << "Failed to configure RGL logging.\n";
     }
+
+    if (sdf->HasElement(PARAM_DO_IGNORE_ENTITIES_IN_LIDAR_LINK_ID)) {
+        doIgnoreEntitiesInLidarLink = sdf->Get<bool>(PARAM_DO_IGNORE_ENTITIES_IN_LIDAR_LINK_ID);
+    }
 }
 
 void RGLServerPluginManager::PostUpdate(

RGLServerPlugin/src/Scene.cc

@@ -46,8 +46,10 @@ bool RGLServerPluginManager::RegisterNewLidarCb(
     for (const auto& plugin : plugins) {
         if (plugin.name() == RGL_INSTANCE) {
             lidarEntities.insert(entity);
-            for (auto descendant: ecm.Descendants(entity)) {
-                entitiesToIgnore.insert(descendant);
+            if (doIgnoreEntitiesInLidarLink) {
+                for (auto descendant: ecm.Descendants(entity)) {
+                    entitiesToIgnore.insert(descendant);
+                }
             }
         }
     }
@@ -57,10 +59,12 @@ bool RGLServerPluginManager::RegisterNewLidarCb(
         return true;
     }
 
-    // Ignore all entities in link associated with RGL lidar
-    // Link could contain visual representation of the lidar
-    for (auto entityInParentLink : GetEntitiesInParentLink(entity, ecm)) {
-        entitiesToIgnore.insert(entityInParentLink);
+    if (doIgnoreEntitiesInLidarLink) {
+        // Ignore all entities in link associated with RGL lidar
+        // Link could contain visual representation of the lidar
+        for (auto entityInParentLink : GetEntitiesInParentLink(entity, ecm)) {
+            entitiesToIgnore.insert(entityInParentLink);
+        }
     }
 
     return true;

test_world/rgl_playground.sdf

@@ -2,7 +2,9 @@
   <world name='rgl_playground'>
 
     <!-- Global RGL scene manager required to upload meshes to GPU -->
-    <plugin name='rgl::RGLServerPluginManager' filename='RGLServerPluginManager'/>
+    <plugin name='rgl::RGLServerPluginManager' filename='RGLServerPluginManager'>
+      <do_ignore_entities_in_lidar_link>true</do_ignore_entities_in_lidar_link>
+    </plugin>
 
     <physics name='1ms' type='ignored'>
       <max_step_size>0.001</max_step_size>