Skip to content

Commit

Permalink
Deprecating PoseBundle
Browse files Browse the repository at this point in the history
This deprecates PoseBundle as well as closely affiliated classes:
PoseVector, PoseAggregator, PoseBundoeToDrawMessage.

There are two systems that have PoseBUndle-valued output ports and two
diagrams that export those ports.

The systems had explicit getters for the ports; those getters have been
deprecated.

The diagrams were mixed. One had a getter, one relied on
get_output_port(name). To generally aid the case where a leaf output
port is exported, the calc methods of the two systems have been given
a "log once" warning.

Bindings and unit tests have likewise been updated.
  • Loading branch information
SeanCurtis-TRI committed Aug 2, 2021
1 parent 89958f6 commit b38beda
Show file tree
Hide file tree
Showing 32 changed files with 375 additions and 101 deletions.
20 changes: 13 additions & 7 deletions bindings/pydrake/geometry_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -513,13 +513,19 @@ void DoScalarDependentDefinitions(py::module m, T) {
cls // BR
.def(py::init<>(), cls_doc.ctor.doc)
.def("get_source_pose_port", &Class::get_source_pose_port,
py_rvp::reference_internal, cls_doc.get_source_pose_port.doc)
.def(
"get_pose_bundle_output_port",
[](Class* self) -> const systems::OutputPort<T>& {
return self->get_pose_bundle_output_port();
},
py_rvp::reference_internal, cls_doc.get_pose_bundle_output_port.doc)
py_rvp::reference_internal, cls_doc.get_source_pose_port.doc);

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
cls // BR
.def("get_pose_bundle_output_port",
WrapDeprecated(cls_doc.get_pose_bundle_output_port.doc_deprecated,
&Class::get_pose_bundle_output_port),
py_rvp::reference_internal,
cls_doc.get_pose_bundle_output_port.doc_deprecated);
#pragma GCC diagnostic pop

cls // BR
.def("get_query_output_port", &Class::get_query_output_port,
py_rvp::reference_internal, cls_doc.get_query_output_port.doc)
.def("model_inspector", &Class::model_inspector,
Expand Down
2 changes: 2 additions & 0 deletions bindings/pydrake/systems/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,7 @@ drake_pybind_library(
name = "rendering_py",
cc_deps = [
"//bindings/pydrake:documentation_pybind",
"//bindings/pydrake/common:deprecation_pybind",
"//bindings/pydrake/common:eigen_geometry_pybind",
"//bindings/pydrake/common:value_pybind",
],
Expand Down Expand Up @@ -565,6 +566,7 @@ drake_py_unittest(
data = ["//multibody/benchmarks/acrobot:models"],
deps = [
":rendering_py",
"//bindings/pydrake/common/test_utilities:deprecation_py",
"//bindings/pydrake/multibody:math_py",
"//bindings/pydrake/multibody:parsing_py",
"//bindings/pydrake/multibody:plant_py",
Expand Down
1 change: 0 additions & 1 deletion bindings/pydrake/systems/meshcat_visualizer.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
from pydrake.lcm import DrakeLcm, Subscriber
from pydrake.math import RigidTransform, RotationMatrix
from pydrake.systems.framework import LeafSystem, PublishEvent, TriggerType
from pydrake.systems.rendering import PoseBundle
from pydrake.multibody.plant import ContactResults
import pydrake.perception as mut

Expand Down
29 changes: 22 additions & 7 deletions bindings/pydrake/systems/planar_scenegraph_visualizer.py
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,9 @@ def __init__(self,
use_random_colors=False,
substitute_collocated_mesh_files=True,
ax=None,
show=None):
show=None,
need_pose_bundle=True,
):
"""
Args:
scene_graph: A SceneGraph object.
Expand Down Expand Up @@ -125,10 +127,14 @@ def __init__(self,
self._T_VW = T_VW

# (2021-11-01) Remove at end of deprecation period.
# Pose bundle (from SceneGraph) input port.
self._pose_bundle_port = self.DeclareAbstractInputPort(
"lcm_visualization", AbstractValue.Make(PoseBundle(0)))
self._warned_pose_bundle_input_port_connected = False
# Pose bundle (from SceneGraph) input port (and the secret
# need_pose_bundle flag).
if need_pose_bundle:
self._pose_bundle_port = self.DeclareAbstractInputPort(
"lcm_visualization", AbstractValue.Make(PoseBundle(0)))
self._warned_pose_bundle_input_port_connected = False
else:
self._pose_bundle_port = None
# End of deprecation block.

self._geometry_query_input_port = self.DeclareAbstractInputPort(
Expand Down Expand Up @@ -391,7 +397,7 @@ def _update_body_fill_verts(self, body_fill, patch_V):

def draw(self, context):
"""Overrides base with the implementation."""
if self._pose_bundle_port.HasValue(context):
if self._pose_bundle_port and self._pose_bundle_port.HasValue(context):
self._draw_deprecated(context)
else:
self._draw(context)
Expand Down Expand Up @@ -476,8 +482,17 @@ def ConnectPlanarSceneGraphVisualizer(builder,
Returns:
The newly created PlanarSceneGraphVisualizer object.
"""
# While PoseBundle is deprecated, we can use this method to create a
# visualizer that doesn't even have a pose bundle port. This should be safe
# as it also handles the connection.
# It's necessary to do this because calls to this function in jupyter
# notebooks can't be guarded with deprecation warnings.
use_pose_bundle = output_port and isinstance(
output_port.Allocate().get_value(), PoseBundle)

visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, **kwargs))
PlanarSceneGraphVisualizer(
scene_graph, need_pose_bundle=use_pose_bundle, **kwargs))

if output_port and isinstance(output_port.Allocate().get_value(),
PoseBundle):
Expand Down
55 changes: 41 additions & 14 deletions bindings/pydrake/systems/rendering_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#include "pybind11/pybind11.h"
#include <Eigen/Dense>

#include "drake/bindings/pydrake/common/deprecation_pybind.h"
#include "drake/bindings/pydrake/common/eigen_geometry_pybind.h"
#include "drake/bindings/pydrake/common/value_pybind.h"
#include "drake/bindings/pydrake/documentation_pybind.h"
Expand All @@ -28,14 +29,27 @@ PYBIND11_MODULE(rendering, m) {

using T = double;

// Basically *all* of these classes/structs are deprecated, so we'll put the
// whole block of them inside the pragma.
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"

// Because we've deprecated the *classes* and not the methods, applying
// deprecation to the constructors requires augmenting the doc string into
// something with an appropriately formatted deprecation warning.
const std::string deprecated_rendering(
" This class has been deprecated and will be removed by 2021-12-01.");

py::class_<PoseVector<T>, BasicVector<T>> pose_vector(
m, "PoseVector", doc.PoseVector.doc);
pose_vector // BR
.def(py::init(), doc.PoseVector.ctor.doc_0args)
.def(py::init<const Eigen::Quaternion<T>&,
const Eigen::Translation<T, 3>&>(),
.def(py_init_deprecated<PoseVector<T>>(
doc.PoseVector.ctor.doc_0args + deprecated_rendering))
.def(py_init_deprecated<PoseVector<T>, const Eigen::Quaternion<T>&,
const Eigen::Translation<T, 3>&>(
doc.PoseVector.ctor.doc_2args + deprecated_rendering),
py::arg("rotation"), py::arg("translation"),
doc.PoseVector.ctor.doc_2args)
(doc.PoseVector.ctor.doc_2args + deprecated_rendering).c_str())
.def("get_transform", &PoseVector<T>::get_transform,
doc.PoseVector.get_transform.doc)
.def("set_transform", &PoseVector<T>::set_transform,
Expand All @@ -52,21 +66,30 @@ PYBIND11_MODULE(rendering, m) {
pose_vector.attr("kSize") = int{PoseVector<T>::kSize};

py::class_<FrameVelocity<T>, BasicVector<T>> frame_velocity(
m, "FrameVelocity", doc.FrameVelocity.doc);
m, "FrameVelocity", doc.FrameVelocity.doc_deprecated);
frame_velocity // BR
.def(py::init(), doc.FrameVelocity.ctor.doc_0args)
.def(py::init<const multibody::SpatialVelocity<T>&>(),
py::arg("velocity"), doc.FrameVelocity.ctor.doc_1args)
.def(py_init_deprecated<FrameVelocity<T>>(
doc.FrameVelocity.ctor.doc_0args + deprecated_rendering),
(doc.FrameVelocity.ctor.doc_0args + deprecated_rendering).c_str())
.def(py_init_deprecated<FrameVelocity<T>,
const multibody::SpatialVelocity<T>&>(
doc.FrameVelocity.ctor.doc_1args + deprecated_rendering),
py::arg("velocity"),
(doc.FrameVelocity.ctor.doc_1args + deprecated_rendering).c_str())
.def("get_velocity", &FrameVelocity<T>::get_velocity,
doc.FrameVelocity.get_velocity.doc)
.def("set_velocity", &FrameVelocity<T>::set_velocity, py::arg("velocity"),
doc.FrameVelocity.set_velocity.doc);

frame_velocity.attr("kSize") = int{FrameVelocity<T>::kSize};

py::class_<PoseBundle<T>> pose_bundle(m, "PoseBundle", doc.PoseBundle.doc);
py::class_<PoseBundle<T>> pose_bundle(
m, "PoseBundle", doc.PoseBundle.doc_deprecated);
pose_bundle
.def(py::init<int>(), py::arg("num_poses"), doc.PoseBundle.ctor.doc)
.def(py_init_deprecated<PoseBundle<T>, int>(
doc.PoseBundle.ctor.doc + deprecated_rendering),
py::arg("num_poses"),
(doc.PoseBundle.ctor.doc + deprecated_rendering).c_str())
.def("get_num_poses", &PoseBundle<T>::get_num_poses,
doc.PoseBundle.get_num_poses.doc)
.def("get_transform", &PoseBundle<T>::get_transform,
Expand Down Expand Up @@ -103,8 +126,10 @@ PYBIND11_MODULE(rendering, m) {
doc.PoseVelocityInputPorts.velocity_input_port.doc);

py::class_<PoseAggregator<T>, LeafSystem<T>>(
m, "PoseAggregator", doc.PoseAggregator.doc)
.def(py::init<>(), doc.PoseAggregator.ctor.doc)
m, "PoseAggregator", doc.PoseAggregator.doc_deprecated)
.def(py_init_deprecated<PoseAggregator<T>>(
doc.PoseAggregator.ctor.doc + deprecated_rendering),
(doc.PoseAggregator.ctor.doc + deprecated_rendering).c_str())
.def("AddSingleInput", &PoseAggregator<T>::AddSingleInput,
py_rvp::reference_internal, doc.PoseAggregator.AddSingleInput.doc)
.def("AddSinglePoseAndVelocityInput",
Expand All @@ -113,6 +138,10 @@ PYBIND11_MODULE(rendering, m) {
.def("AddBundleInput", &PoseAggregator<T>::AddBundleInput,
py_rvp::reference_internal, doc.PoseAggregator.AddBundleInput.doc);

#pragma GCC diagnostic pop

// See the todo in multibody_position_to_geometry_pose.h. This should
// ultimately move into a different module.
py::class_<MultibodyPositionToGeometryPose<T>, LeafSystem<T>>(m,
"MultibodyPositionToGeometryPose",
doc.MultibodyPositionToGeometryPose.doc)
Expand All @@ -122,8 +151,6 @@ PYBIND11_MODULE(rendering, m) {
py::keep_alive<1, 2>(),
doc.MultibodyPositionToGeometryPose.ctor
.doc_2args_plant_input_multibody_state);

// TODO(eric.cousineau): Add more systems as needed.
}

} // namespace pydrake
Expand Down
13 changes: 11 additions & 2 deletions bindings/pydrake/systems/sensors_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -199,8 +199,17 @@ PYBIND11_MODULE(sensors, m) {
py_rvp::reference_internal, cls_doc.depth_image_16U_output_port.doc)
.def("label_image_output_port", &Class::label_image_output_port,
py_rvp::reference_internal, cls_doc.label_image_output_port.doc)
.def("X_WB_output_port", &Class::X_WB_output_port,
py_rvp::reference_internal, cls_doc.X_WB_output_port.doc);
.def("body_pose_in_world_output_port",
&Class::body_pose_in_world_output_port, py_rvp::reference_internal,
cls_doc.body_pose_in_world_output_port.doc);

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
py_class.def("X_WB_output_port", &Class::X_WB_output_port,
py_rvp::reference_internal, cls_doc.X_WB_output_port.doc_deprecated);
#pragma GCC diagnostic pop
DeprecateAttribute(
py_class, "X_WB_output_port", cls_doc.X_WB_output_port.doc_deprecated);
};

py::class_<RgbdSensor, LeafSystem<T>> rgbd_sensor(
Expand Down
Loading

0 comments on commit b38beda

Please sign in to comment.