Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Deprecate camera pose #1431

Closed
wants to merge 3 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions Migration.md
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,17 @@ but with improved human-readability..
matches `"0"`, `"1"`, `"true"`, or `"false"` and returns `false`
otherwise.

### Deprecations

- **sdf/Camera.hh**:
+ The `//sensor/camera/pose` SDF element and corresponding pose functions
in the Camera DOM class are deprecated. Please specify camera pose using
the `//sensor/pose` SDF element instead.
+ ***Deprecation:*** const gz::math::Pose3d &RawPose() const
+ ***Deprecation:*** void SetRawPose(const gz::math::Pose3d &_pose)
+ ***Deprecation:*** const std::string &PoseRelativeTo() const
+ ***Deprecation:*** void SetPoseRelativeTo(const std::string &_frame)

## libsdformat 13.x to 14.x

### Additions
Expand Down
10 changes: 5 additions & 5 deletions include/sdf/Camera.hh
Original file line number Diff line number Diff line change
Expand Up @@ -334,27 +334,27 @@ namespace sdf
/// \param[in] _center Distortion center or principal point.
public: void SetDistortionCenter(const gz::math::Vector2d &_center);

/// \brief Get the pose of the camer. This is the pose of the camera
/// \brief Get the pose of the camera. This is the pose of the camera
/// as specified in SDF (<camera> <pose> ... </pose></camera>).
/// \return The pose of the link.
public: const gz::math::Pose3d &RawPose() const;
public: const gz::math::Pose3d GZ_DEPRECATED(15) &RawPose() const;

/// \brief Set the pose of the camera.
/// \sa const gz::math::Pose3d &RawPose() const
/// \param[in] _pose The new camera pose.
public: void SetRawPose(const gz::math::Pose3d &_pose);
public: void GZ_DEPRECATED(15) SetRawPose(const gz::math::Pose3d &_pose);

/// \brief Get the name of the coordinate frame relative to which this
/// object's pose is expressed. An empty value indicates that the frame is
/// relative to the parent link.
/// \return The name of the pose relative-to frame.
public: const std::string &PoseRelativeTo() const;
public: const std::string GZ_DEPRECATED(15) &PoseRelativeTo() const;

/// \brief Set the name of the coordinate frame relative to which this
/// object's pose is expressed. An empty value indicates that the frame is
/// relative to the parent link.
/// \param[in] _frame The name of the pose relative-to frame.
public: void SetPoseRelativeTo(const std::string &_frame);
public: void GZ_DEPRECATED(15) SetPoseRelativeTo(const std::string &_frame);

/// \brief Get the name of the coordinate frame relative to which this
/// object's camera_info message header is expressed.
Expand Down
3 changes: 3 additions & 0 deletions python/src/sdf/pyCamera.cc
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ namespace python
/////////////////////////////////////////////////
void defineCamera(pybind11::object module)
{
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
pybind11::class_<sdf::Camera> cameraModule(module, "Camera");
cameraModule
.def(pybind11::init<>())
Expand Down Expand Up @@ -296,6 +297,8 @@ void defineCamera(pybind11::object module)
.value("BAYER_BGGR8", sdf::PixelFormatType::BAYER_BGGR8)
.value("BAYER_GBRG8", sdf::PixelFormatType::BAYER_GBRG8)
.value("BAYER_GRBG8", sdf::PixelFormatType::BAYER_GRBG8);

GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
}
} // namespace python
} // namespace SDF_VERSION_NAMESPACE
Expand Down
2 changes: 1 addition & 1 deletion sdf/1.12/camera.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -226,5 +226,5 @@
<description>An optional frame id name to be used in the camera_info message header.</description>
</element>

<include filename="pose.sdf" required="0"/>
<include filename="pose.sdf" required="-1"/>
</element> <!-- End Camera -->
5 changes: 5 additions & 0 deletions src/Camera.cc
Original file line number Diff line number Diff line change
Expand Up @@ -379,6 +379,8 @@ Errors Camera::Load(ElementPtr _sdf)
errors.insert(errors.end(), noiseErr.begin(), noiseErr.end());
}

// \todo(iche033) //sensor/camera/pose is being deprecated.
// Remove in sdformat16.
// Load the pose. Ignore the return value since the pose is optional.
loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo);

Expand Down Expand Up @@ -1222,7 +1224,10 @@ sdf::ElementPtr Camera::ToElement() const
poseElem->GetAttribute("relative_to")->Set<std::string>(
this->dataPtr->poseRelativeTo);
}

GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
poseElem->Set<gz::math::Pose3d>(this->RawPose());
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
elem->GetElement("horizontal_fov")->Set<double>(
this->HorizontalFov().Radian());
sdf::ElementPtr imageElem = elem->GetElement("image");
Expand Down
6 changes: 6 additions & 0 deletions src/Camera_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -126,13 +126,15 @@ TEST(DOMCamera, Construction)
cam.SetDistortionCenter(gz::math::Vector2d(0.1, 0.2));
EXPECT_EQ(gz::math::Vector2d(0.1, 0.2), cam.DistortionCenter());

GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
EXPECT_EQ(gz::math::Pose3d::Zero, cam.RawPose());
cam.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0));
EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), cam.RawPose());

EXPECT_TRUE(cam.PoseRelativeTo().empty());
cam.SetPoseRelativeTo("/frame");
EXPECT_EQ("/frame", cam.PoseRelativeTo());
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION

EXPECT_TRUE(cam.OpticalFrameId().empty());
cam.SetOpticalFrameId("/optical_frame");
Expand Down Expand Up @@ -270,7 +272,9 @@ TEST(DOMCamera, ToElement)
cam.SetNearClip(0.2);
cam.SetFarClip(200.2);
cam.SetVisibilityMask(123u);
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
cam.SetPoseRelativeTo("/frame");
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
cam.SetSaveFrames(true);
cam.SetSaveFramesPath("/tmp");
cam.SetOpticalFrameId("/optical_frame");
Expand All @@ -293,7 +297,9 @@ TEST(DOMCamera, ToElement)
EXPECT_DOUBLE_EQ(0.2, cam2.NearClip());
EXPECT_DOUBLE_EQ(200.2, cam2.FarClip());
EXPECT_EQ(123u, cam2.VisibilityMask());
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
EXPECT_EQ("/frame", cam2.PoseRelativeTo());
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
EXPECT_TRUE(cam2.SaveFrames());
EXPECT_EQ("/tmp", cam2.SaveFramesPath());
EXPECT_EQ("/optical_frame", cam2.OpticalFrameId());
Expand Down
4 changes: 4 additions & 0 deletions test/integration/link_dom.cc
Original file line number Diff line number Diff line change
Expand Up @@ -314,8 +314,10 @@ TEST(DOMLink, Sensors)
const sdf::Camera *camSensor = cameraSensor->CameraSensor();
ASSERT_NE(nullptr, camSensor);
EXPECT_EQ("my_camera", camSensor->Name());
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
EXPECT_EQ(gz::math::Pose3d(0.1, 0.2, 0.3, 0, 0, 0),
camSensor->RawPose());
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
EXPECT_DOUBLE_EQ(0.75, camSensor->HorizontalFov().Radian());
EXPECT_EQ("", cameraSensor->Topic());
EXPECT_EQ("my_camera/trigger", camSensor->TriggerTopic());
Expand Down Expand Up @@ -731,8 +733,10 @@ TEST(DOMLink, Sensors)
const sdf::Camera *wideAngleCam = wideAngleCameraSensor->CameraSensor();
ASSERT_NE(nullptr, wideAngleCam);
EXPECT_EQ("wide_angle_cam", wideAngleCam->Name());
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
EXPECT_EQ(gz::math::Pose3d(0.2, 0.3, 0.4, 0, 0, 0),
wideAngleCam->RawPose());
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
EXPECT_DOUBLE_EQ(3.14, wideAngleCam->HorizontalFov().Radian());
EXPECT_EQ("wideanglecamera", wideAngleCameraSensor->Topic());
EXPECT_EQ("", wideAngleCam->TriggerTopic());
Expand Down
2 changes: 2 additions & 0 deletions test/integration/sdf_dom_conversion.cc
Original file line number Diff line number Diff line change
Expand Up @@ -120,8 +120,10 @@ TEST(SDFDomConversion, Sensors)
const sdf::Camera *camSensor = cameraSensor->CameraSensor();
ASSERT_NE(nullptr, camSensor);
EXPECT_EQ("my_camera", camSensor->Name());
GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
EXPECT_EQ(gz::math::Pose3d(0.1, 0.2, 0.3, 0, 0, 0),
camSensor->RawPose());
GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION
EXPECT_DOUBLE_EQ(0.75, camSensor->HorizontalFov().Radian());
EXPECT_EQ(640u, camSensor->ImageWidth());
EXPECT_EQ(480u, camSensor->ImageHeight());
Expand Down
Loading