Skip to content

Commit

Permalink
systems: Use const-ref arguments, not unique_ptr (RobotLocomotion#14453)
Browse files Browse the repository at this point in the history
This commit changes FixInputPort and DeclareAbstractState to require
pass-by-value (the unique_ptr overloads are deprecated).

These two now-deprecated functions ...

- ContextBase::FixInputPort(int, std::unique_ptr<AbstractValue>)
- LeafSystem::DeclareAbstractState(std::unique_ptr<AbstractValue>)

... now clone the argument, instead of moving it.  Calling code that
relied on retaining an alias into the transferred storage will now end
up with a dangling pointer.  This is a breaking change for any code
that relied on such aliasing.
  • Loading branch information
jwnimmer-tri authored Dec 16, 2020
1 parent 71be659 commit 0dba0a7
Show file tree
Hide file tree
Showing 24 changed files with 81 additions and 72 deletions.
7 changes: 4 additions & 3 deletions bindings/pydrake/systems/framework_py_systems.cc
Original file line number Diff line number Diff line change
Expand Up @@ -652,9 +652,10 @@ Note: The above is for the C++ documentation. For Python, use
&LeafSystemPublic::DoCalcDiscreteVariableUpdates,
doc.LeafSystem.DoCalcDiscreteVariableUpdates.doc)
// Abstract state.
.def("DeclareAbstractState", &LeafSystemPublic::DeclareAbstractState,
// Keep alive, ownership: `AbstractValue` keeps `self` alive.
py::keep_alive<2, 1>(), doc.LeafSystem.DeclareAbstractState.doc);
.def("DeclareAbstractState",
py::overload_cast<const AbstractValue&>(
&LeafSystemPublic::DeclareAbstractState),
doc.LeafSystem.DeclareAbstractState.doc);

DefineTemplateClassWithDefault<Diagram<T>, PyDiagram, System<T>>(
m, "Diagram", GetPyParam<T>(), doc.Diagram.doc)
Expand Down
4 changes: 2 additions & 2 deletions examples/compass_gait/compass_gait.cc
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,8 @@ CompassGait<T>::CompassGait()

// Abstract state for indicating that the left foot is the stance foot. This
// is only used for the visualization output.
bool left_stance = true;
this->DeclareAbstractState(AbstractValue::Make(left_stance));
const bool left_stance = true;
this->DeclareAbstractState(Value<bool>(left_stance));

// Hip torque input.
this->DeclareVectorInputPort("hip_torque", systems::BasicVector<T>(1));
Expand Down
4 changes: 2 additions & 2 deletions examples/rimless_wheel/rimless_wheel.cc
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@ RimlessWheel<T>::RimlessWheel()
// Abstract state for indicating that we are in "double support" mode (e.g.
// two feet/spokes are touching the ground, and angular velocity is
// approximately zero).
bool double_support = false;
this->DeclareAbstractState(AbstractValue::Make(double_support));
const bool double_support = false;
this->DeclareAbstractState(Value<bool>(double_support));

// The minimal state of the system.
this->DeclareVectorOutputPort(RimlessWheelContinuousState<T>(),
Expand Down
4 changes: 2 additions & 2 deletions geometry/scene_graph.cc
Original file line number Diff line number Diff line change
Expand Up @@ -93,8 +93,8 @@ SceneGraph<T>::SceneGraph(bool data_as_state, int)
data_as_state_(data_as_state) {
model_inspector_.set(&model_);
if (data_as_state_) {
geometry_state_index_ = this->DeclareAbstractState(
make_unique<GeometryStateValue<T>>());
geometry_state_index_ =
this->DeclareAbstractState(GeometryStateValue<T>());
} else {
geometry_state_index_ =
this->DeclareAbstractParameter(GeometryStateValue<T>());
Expand Down
2 changes: 1 addition & 1 deletion manipulation/perception/optitrack_pose_extractor.cc
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ OptitrackPoseExtractor::OptitrackPoseExtractor(
.get_index()},
X_WO_(X_WO) {
DeclareAbstractState(
AbstractValue::Make<Isometry3<double>>(
Value<Isometry3<double>>(
Isometry3<double>::Identity()));
this->DeclareAbstractInputPort(
systems::kUseDefaultName,
Expand Down
2 changes: 1 addition & 1 deletion manipulation/perception/pose_smoother.cc
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ PoseSmoother::PoseSmoother(double desired_max_linear_velocity,
max_angular_velocity_(desired_max_angular_velocity),
is_filter_enabled_(filter_window_size > 1) {
this->DeclareAbstractState(
AbstractValue::Make<InternalState>(
Value<InternalState>(
InternalState(filter_window_size)));
this->DeclareAbstractInputPort(
systems::kUseDefaultName,
Expand Down
7 changes: 3 additions & 4 deletions manipulation/planner/robot_plan_interpolator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -90,11 +90,10 @@ RobotPlanInterpolator::RobotPlanInterpolator(
.get_index();

// This corresponds to the actual plan.
plan_index_ = this->DeclareAbstractState(
std::make_unique<Value<PlanData>>());
plan_index_ = this->DeclareAbstractState(Value<PlanData>());

// Flag indicating whether RobotPlanInterpolator::Initialize has been called.
init_flag_index_ = this->DeclareAbstractState(
std::make_unique<Value<bool>>(false));
init_flag_index_ = this->DeclareAbstractState(Value<bool>(false));

this->DeclarePeriodicUnrestrictedUpdate(update_interval, 0);
}
Expand Down
6 changes: 3 additions & 3 deletions systems/framework/context.cc
Original file line number Diff line number Diff line change
Expand Up @@ -16,22 +16,22 @@ template <typename T>
FixedInputPortValue& Context<T>::FixInputPort(
int index, const BasicVector<T>& vec) {
return ContextBase::FixInputPort(
index, std::make_unique<Value<BasicVector<T>>>(vec.Clone()));
index, Value<BasicVector<T>>(vec));
}

template <typename T>
FixedInputPortValue& Context<T>::FixInputPort(
int index, const Eigen::Ref<const VectorX<T>>& data) {
return ContextBase::FixInputPort(
index, std::make_unique<Value<BasicVector<T>>>(data));
index, Value<BasicVector<T>>(data));
}

template <typename T>
FixedInputPortValue& Context<T>::FixInputPort(
int index, std::unique_ptr<BasicVector<T>> vec) {
DRAKE_THROW_UNLESS(vec.get() != nullptr);
return ContextBase::FixInputPort(
index, std::make_unique<Value<BasicVector<T>>>(vec->Clone()));
index, Value<BasicVector<T>>(*vec));
}

template <typename T>
Expand Down
4 changes: 2 additions & 2 deletions systems/framework/context_base.cc
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,10 @@ std::string ContextBase::GetSystemPathname() const {
}

FixedInputPortValue& ContextBase::FixInputPort(
int index, std::unique_ptr<AbstractValue> value) {
int index, const AbstractValue& value) {
std::unique_ptr<FixedInputPortValue> fixed =
internal::ContextBaseFixedInputAttorney::CreateFixedInputPortValue(
std::move(value));
value.Clone());
FixedInputPortValue& fixed_ref = *fixed;
SetFixedInputPortValue(InputPortIndex(index), std::move(fixed));
return fixed_ref;
Expand Down
28 changes: 11 additions & 17 deletions systems/framework/context_base.h
Original file line number Diff line number Diff line change
Expand Up @@ -215,24 +215,18 @@ class ContextBase : public internal::ContextMessageInterface {
value present on that port.
@pre `index` selects an existing input port of this Context. */
FixedInputPortValue& FixInputPort(int index, const AbstractValue& value);

/** (Advanced) Same as above method but the value is passed by unique_ptr
instead of by const reference. The port will contain a copy of the `value`
(not retain a pointer to the `value`). */
DRAKE_DEPRECATED("2021-04-01",
"Use input_port.FixValue() instead of context.FixInputPort(), or in "
"rare cases if an InputPort is not available, pass the value argument "
"here by-value, instead of by-unique-ptr.")
FixedInputPortValue& FixInputPort(
int index, std::unique_ptr<AbstractValue> value);

/** (Advanced)
Same as above method but the value is passed by const reference instead
of by unique_ptr. The port will contain a copy of the `value` (not retain a
pointer to the `value`).
@note Calling this method on an already connected input port, i.e., an
input port that has previously been passed into a call to
DiagramBuilder::Connect(), causes FixedInputPortValue to override any other
value present on that port.
@exclude_from_pydrake_mkdoc{The prior overload's docstring is better, and we
only need one of the two -- overloading on ownership doesn't make sense for
pydrake.} */
FixedInputPortValue& FixInputPort(int index, const AbstractValue& value) {
return FixInputPort(index, value.Clone());
int index, std::unique_ptr<AbstractValue> value) {
return FixInputPort(index, *value);
}

/** For input port `index`, returns a const FixedInputPortValue if the port is
Expand Down
2 changes: 1 addition & 1 deletion systems/framework/input_port.h
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ class InputPort final : public InputPortBase {
is_vector_port
? internal::ValueToVectorValue<T>::ToAbstract(__func__, value)
: internal::ValueToAbstractValue::ToAbstract(__func__, value);
return context->FixInputPort(get_index(), std::move(abstract_value));
return context->FixInputPort(get_index(), *abstract_value);
}

/** Returns true iff this port is connected or has had a fixed value provided
Expand Down
10 changes: 8 additions & 2 deletions systems/framework/leaf_system.cc
Original file line number Diff line number Diff line change
Expand Up @@ -581,13 +581,19 @@ DiscreteStateIndex LeafSystem<T>::DeclareDiscreteState(

template <typename T>
AbstractStateIndex LeafSystem<T>::DeclareAbstractState(
std::unique_ptr<AbstractValue> abstract_state) {
const AbstractValue& abstract_state) {
const AbstractStateIndex index(model_abstract_states_.size());
model_abstract_states_.AddModel(index, std::move(abstract_state));
model_abstract_states_.AddModel(index, abstract_state.Clone());
this->AddAbstractState(index);
return index;
}

template <typename T>
AbstractStateIndex LeafSystem<T>::DeclareAbstractState(
std::unique_ptr<AbstractValue> abstract_state) {
return this->DeclareAbstractState(*abstract_state);
}

template <typename T>
InputPort<T>& LeafSystem<T>::DeclareVectorInputPort(
std::variant<std::string, UseDefaultName> name,
Expand Down
12 changes: 11 additions & 1 deletion systems/framework/leaf_system.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
#include "drake/common/default_scalars.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_deprecated.h"
#include "drake/common/eigen_types.h"
#include "drake/common/unused.h"
#include "drake/common/value.h"
Expand Down Expand Up @@ -1123,8 +1124,17 @@ class LeafSystem : public System<T> {
//@{

/** Declares an abstract state.
@param abstract_state The abstract state, its ownership is transferred.
@param abstract_state The abstract state model value.
@return index of the declared abstract state. */
AbstractStateIndex DeclareAbstractState(
const AbstractValue& abstract_state);

/** Declares an abstract state.
@param abstract_state The abstract state model value. The internal model
value will contain a copy of `value` (not retain a pointer to `value`).
@return index of the declared abstract state. */
DRAKE_DEPRECATED("2021-04-01",
"Pass the abstract_state by value, not by-unique-ptr")
AbstractStateIndex DeclareAbstractState(
std::unique_ptr<AbstractValue> abstract_state);
//@}
Expand Down
6 changes: 3 additions & 3 deletions systems/framework/test/diagram_context_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ class SystemWithDiscreteState : public LeafSystem<double> {
class SystemWithAbstractState : public LeafSystem<double> {
public:
SystemWithAbstractState() {
DeclareAbstractState(AbstractValue::Make<int>(42));
DeclareAbstractState(Value<int>(42));
}
~SystemWithAbstractState() override {}
};
Expand Down Expand Up @@ -150,9 +150,9 @@ class DiagramContextTest : public ::testing::Test {
}

void AttachInputPorts() {
context_->FixInputPort(0, std::make_unique<Value<BasicVector<double>>>(
context_->FixInputPort(0, Value<BasicVector<double>>(
Vector1<double>(128.0)));
context_->FixInputPort(1, std::make_unique<Value<BasicVector<double>>>(
context_->FixInputPort(1, Value<BasicVector<double>>(
Vector1<double>(256.0)));
}

Expand Down
8 changes: 4 additions & 4 deletions systems/framework/test/diagram_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -369,11 +369,11 @@ class KitchenSinkStateAndParameters final : public LeafSystem<T> {
for (int i = 0; i < 2; ++i) // Make two "groups" of discrete variables.
this->DeclareDiscreteState(4 + i);
for (int i = 0; i < 10; ++i) // Ten abstract state variables.
this->DeclareAbstractState(AbstractValue::Make<int>(3 + i));
this->DeclareAbstractState(Value<int>(3 + i));
for (int i = 0; i < 3; ++i) // Three "groups" of numeric parameters.
this->DeclareNumericParameter(BasicVector<T>(1 + i));
for (int i = 0; i < 7; ++i) // Seven abstract parameters.
this->DeclareAbstractParameter(*AbstractValue::Make<int>(29 + i));
this->DeclareAbstractParameter(Value<int>(29 + i));
}

// Scalar-converting copy constructor. See @ref system_scalar_conversion.
Expand Down Expand Up @@ -2293,7 +2293,7 @@ class SystemWithAbstractState : public LeafSystem<double> {
public:
SystemWithAbstractState(int id, double update_period) : id_(id) {
DeclarePeriodicUnrestrictedUpdate(update_period, 0);
DeclareAbstractState(AbstractValue::Make<double>(id_));
DeclareAbstractState(Value<double>(id_));

// Verify that no periodic discrete updates are registered.
EXPECT_FALSE(this->GetUniquePeriodicDiscreteUpdateAttribute());
Expand Down Expand Up @@ -3009,7 +3009,7 @@ class PerStepActionTestSystem : public LeafSystem<double> {
public:
PerStepActionTestSystem() {
DeclareDiscreteState(1);
DeclareAbstractState(AbstractValue::Make<std::string>(""));
DeclareAbstractState(Value<std::string>(""));
}

using LeafSystem<double>::DeclarePerStepEvent;
Expand Down
6 changes: 3 additions & 3 deletions systems/framework/test/fixed_input_port_value_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -39,11 +39,11 @@ class FixedInputPortTest : public ::testing::Test {
protected:
void SetUp() override {
port0_value_ = &context_.FixInputPort(
InputPortIndex(0), std::make_unique<Value<BasicVector<double>>>(
InputPortIndex(0), Value<BasicVector<double>>(
Eigen::Vector2d(5.0, 6.0)));

port1_value_ = &context_.FixInputPort(
InputPortIndex(1), std::make_unique<Value<std::string>>("foo"));
InputPortIndex(1), Value<std::string>("foo"));

// The input ports and free values should have distinct tickets.
DependencyTicket ticket0 = context_.input_port_ticket(InputPortIndex(0));
Expand Down Expand Up @@ -124,7 +124,7 @@ TEST_F(FixedInputPortTest, RepeatedFixInputPortUsesSameTracker) {

// Replace the value object for output port 1.
const FixedInputPortValue& new_port1_value = context_.FixInputPort(
InputPortIndex(1), std::make_unique<Value<std::string>>("bar"));
InputPortIndex(1), Value<std::string>("bar"));

// The new value object should have the same ticket & tracker as the old one.
EXPECT_EQ(new_port1_value.ticket(), old_port1_value_ticket);
Expand Down
6 changes: 3 additions & 3 deletions systems/framework/test/leaf_context_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ class LeafContextTest : public ::testing::Test {
// (That's not allowed in user code.)
for (int i = 0; i < kNumInputPorts; ++i) {
context_.FixInputPort(
i, std::make_unique<Value<BasicVector<double>>>(kInputSize[i]));
i, Value<BasicVector<double>>(kInputSize[i]));
++next_ticket_;
}

Expand Down Expand Up @@ -355,7 +355,7 @@ TEST_F(LeafContextTest, GetVectorInput) {
AddInputPorts(2, &context);

// Add input port 0 to the context, but leave input port 1 uninitialized.
context.FixInputPort(0, std::make_unique<Value<BasicVector<double>>>(
context.FixInputPort(0, Value<BasicVector<double>>(
Eigen::Vector2d(5.0, 6.0)));

// Test that port 0 is retrievable.
Expand All @@ -373,7 +373,7 @@ TEST_F(LeafContextTest, GetAbstractInput) {
AddInputPorts(2, &context);

// Add input port 0 to the context, but leave input port 1 uninitialized.
context.FixInputPort(0, std::make_unique<Value<std::string>>("foo"));
context.FixInputPort(0, Value<std::string>("foo"));

// Test that port 0 is retrievable.
EXPECT_EQ("foo", *ReadStringInputPort(context, 0));
Expand Down
19 changes: 9 additions & 10 deletions systems/framework/test/leaf_system_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -126,9 +126,8 @@ class TestSystem : public LeafSystem<T> {
this->DeclareAbstractParameter(Value<std::string>("parameter value"));

this->DeclareDiscreteState(3);
this->DeclareAbstractState(AbstractValue::Make<int>(5));
this->DeclareAbstractState(
AbstractValue::Make<std::string>("second abstract state"));
this->DeclareAbstractState(Value<int>(5));
this->DeclareAbstractState(Value<std::string>("second abstract state"));
}
~TestSystem() override {}

Expand Down Expand Up @@ -1099,10 +1098,10 @@ GTEST_TEST(ModelLeafSystemTest, ModelInputGovernsFixedInput) {
dut.reset();

// The first port should only accept a 1d vector.
context->FixInputPort(0, std::make_unique<Value<BasicVector<double>>>(
context->FixInputPort(0, Value<BasicVector<double>>(
VectorXd::Constant(1, 0.0)));
DRAKE_EXPECT_THROWS_MESSAGE(
context->FixInputPort(0, std::make_unique<Value<BasicVector<double>>>(
context->FixInputPort(0, Value<BasicVector<double>>(
VectorXd::Constant(2, 0.0))),
std::exception,
"System::FixInputPortTypeCheck\\(\\): expected value of type "
Expand All @@ -1111,7 +1110,7 @@ GTEST_TEST(ModelLeafSystemTest, ModelInputGovernsFixedInput) {
"drake::systems::BasicVector<double> with size=2. "
"\\(System ::dut\\)");
DRAKE_EXPECT_THROWS_MESSAGE(
context->FixInputPort(0, std::make_unique<Value<std::string>>()),
context->FixInputPort(0, Value<std::string>()),
std::exception,
"System::FixInputPortTypeCheck\\(\\): expected value of type "
"drake::Value<drake::systems::BasicVector<double>> "
Expand All @@ -1120,9 +1119,9 @@ GTEST_TEST(ModelLeafSystemTest, ModelInputGovernsFixedInput) {
"\\(System ::dut\\)");

// The second port should only accept ints.
context->FixInputPort(2, std::make_unique<Value<int>>(11));
context->FixInputPort(2, Value<int>(11));
DRAKE_EXPECT_THROWS_MESSAGE(
context->FixInputPort(2, std::make_unique<Value<std::string>>()),
context->FixInputPort(2, Value<std::string>()),
std::exception,
"System::FixInputPortTypeCheck\\(\\): expected value of type "
"int "
Expand Down Expand Up @@ -1351,8 +1350,8 @@ GTEST_TEST(ModelLeafSystemTest, ModelAbstractState) {
class DeclaredModelAbstractStateSystem : public LeafSystem<double> {
public:
DeclaredModelAbstractStateSystem() {
DeclareAbstractState(AbstractValue::Make<int>(1));
DeclareAbstractState(AbstractValue::Make<std::string>("wow"));
DeclareAbstractState(Value<int>(1));
DeclareAbstractState(Value<std::string>("wow"));
}
};

Expand Down
2 changes: 1 addition & 1 deletion systems/framework/test/vector_system_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -163,7 +163,7 @@ TEST_F(VectorSystemTest, TopologyFailFast) {
// code and tests to support it.
TestVectorSystem dut;
DRAKE_EXPECT_NO_THROW(dut.CreateDefaultContext());
dut.DeclareAbstractState(std::make_unique<Value<double>>(1.0));
dut.DeclareAbstractState(Value<double>(1.0));
EXPECT_THROW(dut.CreateDefaultContext(), std::exception);
}

Expand Down
4 changes: 2 additions & 2 deletions systems/lcm/lcm_subscriber_system.cc
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,9 @@ LcmSubscriberSystem::LcmSubscriberSystem(

// Declare our two states (message_value, message_count).
static_assert(kStateIndexMessage == 0, "");
this->DeclareAbstractState(AllocateSerializerOutputValue());
this->DeclareAbstractState(*AllocateSerializerOutputValue());
static_assert(kStateIndexMessageCount == 1, "");
this->DeclareAbstractState(AbstractValue::Make<int>(0));
this->DeclareAbstractState(Value<int>(0));

// Declare an unrestricted forced update handler that is invoked when a
// "forced" trigger occurs. This gives the user flexibility to force update
Expand Down
Loading

0 comments on commit 0dba0a7

Please sign in to comment.