set rendering sensor pose once

Signed-off-by: Ian Chen <[email protected]>
gazebosim · May 31, 2024 · 2c7edc6 · 2c7edc6
1 parent ae68e81
commit 2c7edc6
Showing 1 changed file with 17 additions and 80 deletions.
diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
@@ -24,6 +24,7 @@
 #include <vector>
 
 #include <sdf/Actor.hh>
+#include <sdf/Camera.hh>
 #include <sdf/Collision.hh>
 #include <sdf/Element.hh>
 #include <sdf/Joint.hh>
@@ -1323,6 +1324,22 @@ void RenderUtil::Update()
           gzerr << "Failed to create sensor [" << sensorName << "]"
                  << std::endl;
         }
+        else
+        {
+          auto sensorNode = this->dataPtr->sceneManager.NodeById(entity);
+          auto semPose = dataSdf.SemanticPose();
+          math::Pose3d sensorPose;
+          sdf::Errors errors = semPose.Resolve(sensorPose);
+          if (!errors.empty())
+          {
+            sensorPose = dataSdf.RawPose();
+          }
+          if (dataSdf.CameraSensor())
+          {
+            sensorPose = sensorPose * dataSdf.CameraSensor()->RawPose();
+          }
+          sensorNode->SetLocalPose(sensorPose);
+        }
       }
     }
   }
@@ -2388,86 +2405,6 @@ void RenderUtilPrivate::UpdateRenderingEntities(
         this->entityPoses[_entity] = _pose->Data();
         return true;
       });
-
-  // Update cameras
-  _ecm.Each<components::Camera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::Camera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update depth cameras
-  _ecm.Each<components::DepthCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::DepthCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update RGBD cameras
-  _ecm.Each<components::RgbdCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::RgbdCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update gpu_lidar
-  _ecm.Each<components::GpuLidar, components::Pose>(
-      [&](const Entity &_entity,
-        const components::GpuLidar *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update thermal cameras
-  _ecm.Each<components::ThermalCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::ThermalCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update segmentation cameras
-  _ecm.Each<components::SegmentationCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::SegmentationCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update bounding box cameras
-  _ecm.Each<components::BoundingBoxCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::BoundingBoxCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
-
-  // Update wide angle cameras
-  _ecm.Each<components::WideAngleCamera, components::Pose>(
-      [&](const Entity &_entity,
-        const components::WideAngleCamera *,
-        const components::Pose *_pose)->bool
-      {
-        this->entityPoses[_entity] = _pose->Data();
-        return true;
-      });
 }
 
 //////////////////////////////////////////////////