Implement BeamModel's scalar conversion constructor (RobotLocomotion#…

…13615) * Implement BeamModel's scalar conversion constructor
Slumon · Jul 2, 2020 · 115cfd9 · 115cfd9
1 parent 4ff5165
commit 115cfd9
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 1 deletion.
diff --git a/systems/sensors/BUILD.bazel b/systems/sensors/BUILD.bazel
@@ -262,6 +262,7 @@ drake_cc_googletest(
         ":beam_model",
         "//common/proto:call_python",
         "//systems/analysis:simulator",
+        "//systems/framework/test_utilities",
         "//systems/primitives:constant_vector_source",
         "//systems/primitives:random_source",
         "//systems/primitives:signal_logger",

diff --git a/systems/sensors/beam_model.cc b/systems/sensors/beam_model.cc
@@ -11,7 +11,7 @@ namespace sensors {
 
 template <typename T>
 BeamModel<T>::BeamModel(int num_depth_readings, double max_range)
-    : max_range_(max_range) {
+    : LeafSystem<T>(SystemTypeTag<BeamModel>{}), max_range_(max_range) {
   DRAKE_DEMAND(num_depth_readings > 0);
   DRAKE_DEMAND(max_range >= 0.0);
   // Declare depth input port.
@@ -54,6 +54,11 @@ BeamModel<T>::BeamModel(int num_depth_readings, double max_range)
       "event probabilities sum to one");
 }
 
+template <typename T>
+template <typename U>
+BeamModel<T>::BeamModel(const BeamModel<U>& other)
+    : BeamModel<T>(other.max_range(), other.get_depth_input_port().size()) {}
+
 template <typename T>
 BeamModelParams<T>& BeamModel<T>::get_mutable_parameters(
     Context<T>* context) const {

diff --git a/systems/sensors/beam_model.h b/systems/sensors/beam_model.h
@@ -82,5 +82,12 @@ class BeamModel final : public LeafSystem<T> {
 };
 
 }  // namespace sensors
+
+// Explicitly disable symbolic::Expression (for now).
+namespace scalar_conversion {
+template <>
+struct Traits<sensors::BeamModel> : public NonSymbolicTraits {};
+}  // namespace scalar_conversion
+
 }  // namespace systems
 }  // namespace drake
diff --git a/systems/sensors/test/beam_model_test.cc b/systems/sensors/test/beam_model_test.cc
@@ -6,6 +6,7 @@
 #include "drake/systems/analysis/simulator.h"
 #include "drake/systems/framework/diagram.h"
 #include "drake/systems/framework/diagram_builder.h"
+#include "drake/systems/framework/test_utilities/scalar_conversion.h"
 #include "drake/systems/primitives/constant_vector_source.h"
 #include "drake/systems/primitives/random_source.h"
 #include "drake/systems/primitives/signal_logger.h"
@@ -179,6 +180,16 @@ GTEST_TEST(BeamModelTest, TestProbabilityDensity) {
       p_max, 3e-3);
 }
 
+GTEST_TEST(BeamModelTest, ScalarConversion) {
+  const int kNumReadings = 10;
+  const double kMaxRange = 5.0;
+  BeamModel<double> model(kNumReadings, kMaxRange);
+  EXPECT_TRUE(is_autodiffxd_convertible(model));
+  // N.B. Thus far conversion to symbolic is not supported. Update this test to
+  // EXPECT_TRUE when supported.
+  EXPECT_FALSE(is_symbolic_convertible(model));
+}
+
 }  // namespace
 }  // namespace sensors
 }  // namespace systems