Refactor PoseSelector to accept poses up to an arbitrary lookahead di…

…stance, including in default ongoing lanes

drake/automotive/BUILD

@@ -145,8 +145,8 @@ drake_cc_library(
         ":generated_vectors",
         ":idm_planner",
         ":pose_selector",
+        ":road_odometry",
         "//drake/automotive/maliput/api",
-        "//drake/math:saturate",
         "//drake/systems/rendering:pose_bundle",
         "//drake/systems/rendering:pose_vector",
     ],
@@ -195,6 +195,7 @@ drake_cc_library(
         ":idm_planner",
         ":lane_direction",
         ":pose_selector",
+        ":road_odometry",
         "//drake/automotive/maliput/api",
         "//drake/common:cond",
         "//drake/common:symbolic",
@@ -220,6 +221,8 @@ drake_cc_library(
     hdrs = ["pose_selector.h"],
     visibility = [],
     deps = [
+        ":lane_direction",
+        ":road_odometry",
         "//drake/automotive/maliput/api",
         "//drake/systems/rendering:pose_bundle",
         "//drake/systems/rendering:pose_vector",
@@ -250,7 +253,6 @@ drake_cc_library(
     deps = [
         ":generated_vectors",
         ":lane_direction",
-        ":pose_selector",
         "//drake/automotive/maliput/api",
         "//drake/common:cond",
         "//drake/common:symbolic",
@@ -266,12 +268,21 @@ drake_cc_library(
     deps = [
         ":generated_vectors",
         ":lane_direction",
-        ":pose_selector",
         ":pure_pursuit",
         "//drake/systems/rendering:pose_vector",
     ],
 )
 
+drake_cc_library(
+    name = "road_odometry",
+    srcs = ["road_odometry.cc"],
+    hdrs = ["road_odometry.h"],
+    deps = [
+        "//drake/automotive/maliput/api",
+        "//drake/systems/rendering:frame_velocity",
+    ],
+)
+
 drake_cc_library(
     name = "road_path",
     srcs = ["road_path.cc"],
@@ -567,6 +578,7 @@ drake_cc_googletest(
     deps = [
         "//drake/automotive:pose_selector",
         "//drake/automotive/maliput/dragway",
+        "//drake/automotive/maliput/monolane:builder",
     ],
 )
 

drake/automotive/gen/idm_planner_parameters.cc

@@ -15,13 +15,14 @@ const int IdmPlannerParametersIndices::kTimeHeadway;
 const int IdmPlannerParametersIndices::kDelta;
 const int IdmPlannerParametersIndices::kBloatDiameter;
 const int IdmPlannerParametersIndices::kDistanceLowerLimit;
+const int IdmPlannerParametersIndices::kScanAheadDistance;
 
 const std::vector<std::string>&
 IdmPlannerParametersIndices::GetCoordinateNames() {
   static const never_destroyed<std::vector<std::string>> coordinates(
       std::vector<std::string>{
           "v_ref", "a", "b", "s_0", "time_headway", "delta", "bloat_diameter",
-          "distance_lower_limit",
+          "distance_lower_limit", "scan_ahead_distance",
       });
   return coordinates.access();
 }

drake/automotive/gen/idm_planner_parameters.h

@@ -19,7 +19,7 @@ namespace automotive {
 /// Describes the row indices of a IdmPlannerParameters.
 struct IdmPlannerParametersIndices {
   /// The total number of rows (coordinates).
-  static const int kNumCoordinates = 8;
+  static const int kNumCoordinates = 9;
 
   // The index of each individual coordinate.
   static const int kVRef = 0;
@@ -30,6 +30,7 @@ struct IdmPlannerParametersIndices {
   static const int kDelta = 5;
   static const int kBloatDiameter = 6;
   static const int kDistanceLowerLimit = 7;
+  static const int kScanAheadDistance = 8;
 
   /// Returns a vector containing the names of each coordinate within this
   /// class. The indices within the returned vector matches that of this class.
@@ -54,6 +55,7 @@ class IdmPlannerParameters : public systems::BasicVector<T> {
   /// @arg @c delta defaults to 4.0 in units of dimensionless.
   /// @arg @c bloat_diameter defaults to 4.5 in units of m.
   /// @arg @c distance_lower_limit defaults to 1e-2 in units of m.
+  /// @arg @c scan_ahead_distance defaults to 100.0 in units of m.
   IdmPlannerParameters() : systems::BasicVector<T>(K::kNumCoordinates) {
     this->set_v_ref(10.0);
     this->set_a(1.0);
@@ -63,6 +65,7 @@ class IdmPlannerParameters : public systems::BasicVector<T> {
     this->set_delta(4.0);
     this->set_bloat_diameter(4.5);
     this->set_distance_lower_limit(1e-2);
+    this->set_scan_ahead_distance(100.0);
   }
 
   IdmPlannerParameters<T>* DoClone() const override {
@@ -123,6 +126,15 @@ class IdmPlannerParameters : public systems::BasicVector<T> {
   void set_distance_lower_limit(const T& distance_lower_limit) {
     this->SetAtIndex(K::kDistanceLowerLimit, distance_lower_limit);
   }
+  /// distance to scan ahead on road for a leading vehicle
+  /// @note @c scan_ahead_distance is expressed in units of m.
+  /// @note @c scan_ahead_distance has a limited domain of [0.0, +Inf].
+  const T& scan_ahead_distance() const {
+    return this->GetAtIndex(K::kScanAheadDistance);
+  }
+  void set_scan_ahead_distance(const T& scan_ahead_distance) {
+    this->SetAtIndex(K::kScanAheadDistance, scan_ahead_distance);
+  }
   //@}
 
   /// See IdmPlannerParametersIndices::GetCoordinateNames().
@@ -150,6 +162,8 @@ class IdmPlannerParameters : public systems::BasicVector<T> {
     result = result && (bloat_diameter() >= T(0.0));
     result = result && !isnan(distance_lower_limit());
     result = result && (distance_lower_limit() >= T(0.0));
+    result = result && !isnan(scan_ahead_distance());
+    result = result && (scan_ahead_distance() >= T(0.0));
     return result;
   }
 };

drake/automotive/idm_controller.cc

@@ -1,22 +1,20 @@
 #include "drake/automotive/idm_controller.h"
 
+#include <algorithm>
 #include <limits>
 #include <utility>
 #include <vector>
 
 #include "drake/common/cond.h"
 #include "drake/common/drake_assert.h"
 #include "drake/common/symbolic_formula.h"
-#include "drake/math/saturate.h"
 
 namespace drake {
 namespace automotive {
 
 using maliput::api::RoadGeometry;
 using maliput::api::RoadPosition;
 using maliput::api::Rotation;
-using math::saturate;
-using pose_selector::RoadOdometry;
 using systems::rendering::FrameVelocity;
 using systems::rendering::PoseBundle;
 using systems::rendering::PoseVector;
@@ -59,8 +57,7 @@ const systems::InputPortDescriptor<T>& IdmController<T>::traffic_input() const {
 }
 
 template <typename T>
-const systems::OutputPort<T>& IdmController<T>::acceleration_output()
-    const {
+const systems::OutputPort<T>& IdmController<T>::acceleration_output() const {
   return systems::System<T>::get_output_port(acceleration_index_);
 }
 
@@ -96,25 +93,34 @@ void IdmController<T>::ImplCalcAcceleration(
     const PoseBundle<T>& traffic_poses,
     const IdmPlannerParameters<T>& idm_params,
     systems::BasicVector<T>* command) const {
+  using std::abs;
+  using std::max;
+
   DRAKE_DEMAND(idm_params.IsValid());
 
-  // Find the single closest car ahead.
-  const RoadOdometry<T>& lead_car_odom =
-      pose_selector::FindClosestLeading(road_, ego_pose, traffic_poses);
+  const auto translation = ego_pose.get_isometry().translation();
+  const maliput::api::GeoPosition geo_position(translation.x(), translation.y(),
+                                               translation.z());
   const RoadPosition ego_position =
-      pose_selector::CalcRoadPosition(road_, ego_pose.get_isometry());
-
-  const T& s_ego = ego_position.pos.s();
-  const T& s_dot_ego = pose_selector::GetSVelocity(
-      RoadOdometry<double>(ego_position, ego_velocity));
-  const T& s_lead = lead_car_odom.pos.s();
-  const T& s_dot_lead = pose_selector::GetSVelocity(lead_car_odom);
-
-  // Saturate the net_distance at distance_lower_bound away from the ego car to
-  // avoid near-singular solutions inherent to the IDM equation.
-  const T net_distance = saturate(s_lead - s_ego - idm_params.bloat_diameter(),
-                                  idm_params.distance_lower_limit(),
-                                  std::numeric_limits<T>::infinity());
+      road_.ToRoadPosition(geo_position, nullptr, nullptr, nullptr);
+
+  // Find the single closest car ahead.
+  const ClosestPose<T> lead_car_pose = PoseSelector<T>::FindSingleClosestPose(
+      ego_position.lane, ego_pose, traffic_poses,
+      idm_params.scan_ahead_distance(), AheadOrBehind::kAhead);
+  const double headway_distance = lead_car_pose.distance;
+
+  T s_dot_ego = PoseSelector<T>::GetSigmaVelocity({ego_position, ego_velocity});
+  T s_dot_lead =
+      (abs(lead_car_pose.odometry.pos.s()) ==
+       std::numeric_limits<T>::infinity())
+          ? 0.
+          : PoseSelector<T>::GetSigmaVelocity(lead_car_pose.odometry);
+
+  // Saturate the net_distance at `idm_params.distance_lower_limit()` away from
+  // the ego car to avoid near-singular solutions inherent to the IDM equation.
+  const T actual_headway = headway_distance - idm_params.bloat_diameter();
+  const T net_distance = max(actual_headway, idm_params.distance_lower_limit());
   const T closing_velocity = s_dot_ego - s_dot_lead;
 
   // Compute the acceleration command from the IDM equation.

drake/automotive/idm_controller.h

@@ -6,6 +6,7 @@
 
 #include "drake/automotive/gen/idm_planner_parameters.h"
 #include "drake/automotive/idm_planner.h"
+#include "drake/automotive/maliput/api/lane_data.h"
 #include "drake/automotive/maliput/api/road_geometry.h"
 #include "drake/automotive/pose_selector.h"
 #include "drake/common/drake_copyable.h"
@@ -39,6 +40,12 @@ namespace automotive {
 /// Output Port 0: A BasicVector containing the acceleration request.
 ///   (OutputPort getter: acceleration_output())
 ///
+///
+/// Instantiated templates for the following kinds of T's are provided:
+/// - double
+///
+/// They are already available to link against in the containing library.
+///
 /// @ingroup automotive_controllers
 template <typename T>
 class IdmController : public systems::LeafSystem<T> {
@@ -80,6 +87,8 @@ class IdmController : public systems::LeafSystem<T> {
   void CalcAcceleration(const systems::Context<T>& context,
                         systems::BasicVector<T>* accel_output) const;
 
+  // TODO(jadecastro): Introduce DoToAutoDiffXd overload and unit tests.
+
   const maliput::api::RoadGeometry& road_;
 
   // Indices for the input / output ports.

drake/automotive/idm_planner_parameters.named_vector

@@ -57,3 +57,10 @@ element {
      default_value: "1e-2"
      min_value: "0.0"
 }
+element {
+     name: "scan_ahead_distance"
+     doc: "distance to scan ahead on road for a leading vehicle"
+     doc_units: "m"
+     default_value: "100.0"
+     min_value: "0.0"
+}

drake/automotive/maliput/api/lane_data.cc

@@ -21,6 +21,14 @@ std::ostream& operator<<(std::ostream& out, const GeoPosition& geo_position) {
       << ", z = " << geo_position.z() << ")";
 }
 
+bool operator==(const GeoPosition& lhs, const GeoPosition& rhs) {
+  return (lhs.xyz() == rhs.xyz());
+}
+
+bool operator!=(const GeoPosition& lhs, const GeoPosition& rhs) {
+  return (lhs.xyz() != rhs.xyz());
+}
+
 std::ostream& operator<<(std::ostream& out, const LanePosition& lane_position) {
   return out << "(s = " << lane_position.s() << ", r = " << lane_position.r()
       << ", h = " << lane_position.h() << ")";

drake/automotive/maliput/api/lane_data.h

@@ -135,6 +135,10 @@ class GeoPosition {
   /// Fully parameterized constructor.
   GeoPosition(double x, double y, double z) : xyz_(x, y, z) {}
 
+  /// Fully parameterized constructor from a 3-vector @p xyz of the form
+  /// `[x, y, z]`.
+  explicit GeoPosition(const Vector3<double>& xyz) : xyz_(xyz) {}
+
   /// Constructs a GeoPosition from a 3-vector @p xyz of the form `[x, y, z]`.
   static GeoPosition FromXyz(const Vector3<double>& xyz) {
     return GeoPosition(xyz);
@@ -163,15 +167,19 @@ class GeoPosition {
 
  private:
   Vector3<double> xyz_;
-
-  explicit GeoPosition(const Vector3<double>& xyz) : xyz_(xyz) {}
 };
 
 /// Streams a string representation of @p geo_position into @p out. Returns
 /// @p out. This method is provided for the purposes of debugging or
 /// text-logging. It is not intended for serialization.
 std::ostream& operator<<(std::ostream& out, const GeoPosition& geo_position);
 
+/// GeoPosition overload for the equality operator.
+bool operator==(const GeoPosition& lhs, const GeoPosition& rhs);
+
+/// GeoPosition overload for the inequality operator.
+bool operator!=(const GeoPosition& lhs, const GeoPosition& rhs);
+
 /// A 3-dimensional position in a `Lane`-frame, consisting of three components:
 ///  * s is longitudinal position, as arc-length along a Lane's reference line.
 ///  * r is lateral position, perpendicular to the reference line at s.

drake/automotive/maliput/api/test/lane_data_test.cc

@@ -117,6 +117,26 @@ GTEST_TEST(GeoPositionTest, ComponentSetters) {
   CHECK_ALL_GEO_POSITION_ACCESSORS(dut, 99., 2.3, 42.);
 }
 
+GTEST_TEST(GeoPositionTest, EqualityInequalityOperators) {
+  // Checks that equality is true iff the constituent components are all equal,
+  // and that inequality is true otherwise.
+  GeoPosition gp1(0.1, 0.2, 0.3);
+  GeoPosition gp2(0.1, 0.2, 0.3);
+
+  EXPECT_TRUE(gp1 == gp2);
+  EXPECT_FALSE(gp1 != gp2);
+
+  GeoPosition gp_xerror(gp2.x() + 1e-6, gp2.y(), gp2.z());
+  EXPECT_FALSE(gp1 == gp_xerror);
+  EXPECT_TRUE(gp1 != gp_xerror);
+  GeoPosition gp_yerror(gp2.x(), gp2.y() + 1e-6, gp2.z());
+  EXPECT_FALSE(gp1 == gp_yerror);
+  EXPECT_TRUE(gp1 != gp_xerror);
+  GeoPosition gp_zerror(gp2.x(), gp2.y(), gp2.z() + 1e-6);
+  EXPECT_FALSE(gp1 == gp_zerror);
+  EXPECT_TRUE(gp1 != gp_xerror);
+}
+
 #undef CHECK_ALL_GEO_POSITION_ACCESSORS
 
 // An arbitrary very small number (that passes the tests).