diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 0c833a60ca..73bec2c531 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -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( - [&](const Entity &_entity, - const components::Camera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update depth cameras - _ecm.Each( - [&](const Entity &_entity, - const components::DepthCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update RGBD cameras - _ecm.Each( - [&](const Entity &_entity, - const components::RgbdCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update gpu_lidar - _ecm.Each( - [&](const Entity &_entity, - const components::GpuLidar *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update thermal cameras - _ecm.Each( - [&](const Entity &_entity, - const components::ThermalCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update segmentation cameras - _ecm.Each( - [&](const Entity &_entity, - const components::SegmentationCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update bounding box cameras - _ecm.Each( - [&](const Entity &_entity, - const components::BoundingBoxCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update wide angle cameras - _ecm.Each( - [&](const Entity &_entity, - const components::WideAngleCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); } //////////////////////////////////////////////////