set sensor pose

Signed-off-by: Ian Chen <[email protected]>

src/BoundingBoxCameraSensor.cc

@@ -321,6 +321,8 @@ bool BoundingBoxCameraSensor::CreateCamera()
   this->dataPtr->boundingboxCamera->SetVisibilityMask(
     sdfCamera->VisibilityMask());
   this->dataPtr->boundingboxCamera->SetBoundingBoxType(this->dataPtr->type);
+  this->dataPtr->boundingboxCamera->SetLocalPose(
+      this->Pose() * sdfCamera->RawPose());
 
   // Add the camera to the scene
   this->Scene()->RootVisual()->AddChild(this->dataPtr->rgbCamera);

src/CameraSensor.cc

@@ -212,6 +212,7 @@ bool CameraSensor::CreateCamera()
   this->dataPtr->camera->SetNearClipPlane(cameraSdf->NearClip());
   this->dataPtr->camera->SetFarClipPlane(cameraSdf->FarClip());
   this->dataPtr->camera->SetVisibilityMask(cameraSdf->VisibilityMask());
+  this->dataPtr->camera->SetLocalPose(this->Pose() * cameraSdf->RawPose());
   this->AddSensor(this->dataPtr->camera);
 
   const std::map<SensorNoiseType, sdf::Noise> noises = {
@@ -453,9 +454,6 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
 
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
 
-  // move the camera to the current pose
-  this->dataPtr->camera->SetLocalPose(this->Pose());
-
   if (this->HasInfoConnections())
   {
     // publish the camera info message

src/DepthCameraSensor.cc

@@ -350,7 +350,7 @@ bool DepthCameraSensor::CreateCamera()
   this->dataPtr->depthCamera->SetFarClipPlane(far);
   this->dataPtr->depthCamera->SetVisibilityMask(
       cameraSdf->VisibilityMask());
-
+  this->dataPtr->depthCamera->SetLocalPose(this->Pose() * cameraSdf->RawPose());
   this->AddSensor(this->dataPtr->depthCamera);
 
   const std::map<SensorNoiseType, sdf::Noise> noises = {

src/GpuLidarSensor.cc

@@ -194,9 +194,6 @@ bool GpuLidarSensor::CreateLidar()
     return false;
   }
 
-  this->dataPtr->gpuRays->SetWorldPosition(this->Pose().Pos());
-  this->dataPtr->gpuRays->SetWorldRotation(this->Pose().Rot());
-
   this->dataPtr->gpuRays->SetNearClipPlane(this->RangeMin());
   this->dataPtr->gpuRays->SetFarClipPlane(this->RangeMax());
 
@@ -214,6 +211,7 @@ bool GpuLidarSensor::CreateLidar()
   this->dataPtr->gpuRays->SetRayCount(this->RayCount());
   this->dataPtr->gpuRays->SetVerticalRayCount(
       this->VerticalRayCount());
+  this->dataPtr->gpuRays->SetLocalPose(this->Pose());
 
   this->Scene()->RootVisual()->AddChild(
       this->dataPtr->gpuRays);

src/RgbdCameraSensor.cc

@@ -326,6 +326,8 @@ bool RgbdCameraSensor::CreateCameras()
   }
 
   this->dataPtr->depthCamera->SetVisibilityMask(cameraSdf->VisibilityMask());
+  this->dataPtr->depthCamera->SetLocalPose(
+      this->Pose() * cameraSdf->RawPose());
 
   this->AddSensor(this->dataPtr->depthCamera);
 

src/SegmentationCameraSensor.cc

@@ -369,6 +369,7 @@ bool SegmentationCameraSensor::CreateCamera()
   this->dataPtr->camera->SetFarClipPlane(sdfCamera->FarClip());
   this->dataPtr->camera->SetAspectRatio(aspectRatio);
   this->dataPtr->camera->SetHFOV(angle);
+  this->dataPtr->camera->SetLocalPose(this->Pose() * sdfCamera->RawPose());
 
   // Add the camera to the scene
   this->Scene()->RootVisual()->AddChild(this->dataPtr->camera);

src/ThermalCameraSensor.cc

@@ -300,7 +300,8 @@ bool ThermalCameraSensor::CreateCamera()
   this->dataPtr->thermalCamera->SetMinTemperature(this->dataPtr->minTemp);
   this->dataPtr->thermalCamera->SetMaxTemperature(this->dataPtr->maxTemp);
   this->dataPtr->thermalCamera->SetLinearResolution(this->dataPtr->resolution);
-
+  this->dataPtr->thermalCamera->SetLocalPose(
+      this->Pose() * cameraSdf->RawPose());
   this->AddSensor(this->dataPtr->thermalCamera);
 
   const std::map<SensorNoiseType, sdf::Noise> noises = {

src/WideAngleCameraSensor.cc

@@ -240,6 +240,7 @@ bool WideAngleCameraSensor::CreateCamera()
   this->dataPtr->camera->SetNearClipPlane(cameraSdf->NearClip());
   this->dataPtr->camera->SetFarClipPlane(cameraSdf->FarClip());
   this->dataPtr->camera->SetVisibilityMask(cameraSdf->VisibilityMask());
+  this->dataPtr->camera->SetLocalPose(this->Pose() * cameraSdf->RawPose());
 
   rendering::CameraLens lens;
   std::string lensType = cameraSdf->LensType();
@@ -413,9 +414,6 @@ bool WideAngleCameraSensor::Update(
     return false;
   }
 
-  // move the camera to the current pose
-  this->dataPtr->camera->SetLocalPose(this->Pose());
-
   // render only if necessary
   if (!this->dataPtr->pub.HasConnections() &&
       this->dataPtr->imageEvent.ConnectionCount() <= 0 &&