MonolaneOnrampMerge example and merge example generator (RobotLocomot…

…ion#5169)

* Initial commit of monolane onramp merge example

* Port a working figure-8 example for testing

* Derive from RoadGeometry

* Add unbranching road builder, unit test

* Spin off utility class, allow for road reversal

* Add unit tests

* Formatting, documentation, update CMakelists.txt

* Remove all vestiges of Connection-reversal functionality

* cpplint

* Address Liang's comments

* Refactor using the original builder api

* Add ascii art

* Address comments

* Address second-round

* Address Jeremy's comments

* Make rb local

drake/automotive/BUILD

@@ -125,6 +125,16 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "monolane_onramp_merge",
+    srcs = ["monolane_onramp_merge.cc"],
+    hdrs = ["monolane_onramp_merge.h"],
+    deps = [
+        "//drake/automotive/maliput/api",
+        "//drake/automotive/maliput/monolane:monolane",
+    ],
+)
+
 drake_cc_library(
     name = "simple_car",
     srcs = ["simple_car.cc"],
@@ -431,6 +441,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "monolane_onramp_merge_test",
+    deps = [
+        ":monolane_onramp_merge",
+        "//drake/automotive/maliput/utility",
+    ],
+)
+
 drake_cc_googletest(
     name = "simple_car_test",
     deps = [

drake/automotive/CMakeLists.txt

@@ -34,6 +34,7 @@ add_library_with_exports(LIB_NAME drakeAutomotive SOURCE_FILES
   maliput/utility/generate_obj.cc
   maliput/utility/generate_urdf.cc
   maliput/utility/infinite_circuit_road.cc
+  monolane_onramp_merge.cc
   simple_car.cc
   single_lane_ego_and_agent.cc
   trajectory_car.cc
@@ -52,6 +53,7 @@ drake_install_headers(
   curve2.h
   idm_planner.h
   linear_car.h
+  monolane_onramp_merge.h
   simple_car.h
   simple_car_to_euler_floating_joint.h
   single_lane_ego_and_agent.h

drake/automotive/monolane_onramp_merge.cc

@@ -0,0 +1,65 @@
+#include "drake/automotive/monolane_onramp_merge.h"
+
+#include "drake/automotive/maliput/api/road_geometry.h"
+
+namespace drake {
+namespace automotive {
+
+namespace mono = maliput::monolane;
+
+std::unique_ptr<const maliput::api::RoadGeometry>
+MonolaneOnrampMerge::BuildOnramp() {
+  std::unique_ptr<maliput::monolane::Builder> rb(
+      new maliput::monolane::Builder(rc_.lane_bounds, rc_.driveable_bounds,
+                                     linear_tolerance_, angular_tolerance_));
+
+  // Initialize the road from the origin.
+  const mono::EndpointXy kOriginXy{0., 0., 0.};
+  const mono::EndpointZ kFlatZ{0., 0., 0., 0.};
+  const mono::Endpoint kRoadOrigin{kOriginXy, kFlatZ};
+
+  // Construct the pre-merge road.
+  const double kPreArcLength = 25.;
+  const double kPreArcRadius = 40.;
+  const auto& pre0 = rb->Connect(
+      "pre0", kRoadOrigin,
+      mono::ArcOffset(kPreArcLength, -kPreArcRadius / kPreArcLength), kFlatZ);
+  const auto& pre1 = rb->Connect(
+      "pre1", pre0->end(),
+      mono::ArcOffset(kPreArcLength, kPreArcRadius / kPreArcLength), kFlatZ);
+  const auto& pre2 = rb->Connect(
+      "pre2", pre1->end(),
+      mono::ArcOffset(kPreArcLength, -kPreArcRadius / kPreArcLength), kFlatZ);
+  const auto& pre3 = rb->Connect(
+      "pre3", pre2->end(),
+      mono::ArcOffset(kPreArcLength, kPreArcRadius / kPreArcLength), kFlatZ);
+  const auto& pre4 = rb->Connect(
+      "pre4", pre3->end(),
+      mono::ArcOffset(kPreArcLength, -kPreArcRadius / kPreArcLength), kFlatZ);
+  const auto& pre5 = rb->Connect(
+      "pre5", pre4->end(),
+      mono::ArcOffset(kPreArcLength, kPreArcRadius / kPreArcLength), kFlatZ);
+
+  // Construct the post-merge road.
+  const double& kPostLinearLength = 100.;
+  rb->Connect("post0", pre5->end(), kPostLinearLength, kFlatZ);
+
+  // Construct the on-ramp (starting at merge junction and working backwards).
+  const double& kOnrampArcLength = 35.;
+  const double& kOnrampArcRadius = 50.;
+  const double& kOnrampLinearLength = 100.;
+  const auto& onramp1 = rb->Connect(
+      "onramp1", pre5->end(),
+      mono::ArcOffset(kOnrampArcLength, kOnrampArcRadius / kOnrampArcLength),
+      kFlatZ);
+
+  // Group the overlapping connections.
+  rb->MakeGroup("merge-point", {pre5, onramp1});
+
+  rb->Connect("onramp0", onramp1->end(), kOnrampLinearLength, kFlatZ);
+
+  return rb->Build({"monolane-merge-example"});
+}
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/monolane_onramp_merge.h

@@ -0,0 +1,74 @@
+#pragma once
+
+#include <cmath>
+#include <memory>
+
+#include "drake/automotive/maliput/api/road_geometry.h"
+#include "drake/automotive/maliput/monolane/builder.h"
+
+namespace drake {
+namespace automotive {
+
+/// RoadCharacteristics computes and stores characteristics of a road network;
+/// i.e. bounds on the lane width and driveable width. Default settings are
+/// taken if no others are specified.
+struct RoadCharacteristics {
+  /// Constructor for using default road geometries.
+  RoadCharacteristics() = default;
+
+  /// Constructor for custom road geometries.
+  RoadCharacteristics(const double lw, const double dw)
+      : lane_width(lw), driveable_width(dw) {}
+
+  const double lane_width{4.};
+  const double driveable_width{8.};
+
+  const maliput::api::RBounds lane_bounds{-lane_width / 2., lane_width / 2.};
+  const maliput::api::RBounds driveable_bounds{-driveable_width / 2.,
+                                               driveable_width / 2.};
+};
+
+/// MonolaneOnrampMerge contains an example lane-merge scenario expressed as a
+/// maliput monolane road geometry.  The intent of this class is to enable easy
+/// creation and modification of road geometries for simulating/analyzing
+/// such scenarios.
+///
+/// Implements the following onramp example, where each road section is composed
+/// of sequences of linear and arc primitives:
+///
+/// <pre>
+///           pre-merge      post-merge
+///             road           road
+///        |------>-------+------>-------|
+///                      /
+///                     /
+///            onramp  /
+///                   ^
+///                   |
+///                   |
+///                   _
+/// </pre>
+class MonolaneOnrampMerge {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MonolaneOnrampMerge)
+
+  /// Constructor for the example.  The user supplies @p rc, a
+  /// RoadCharacteristics structure that aggregates the road boundary data.
+  explicit MonolaneOnrampMerge(const RoadCharacteristics& rc) : rc_(rc) {}
+
+  /// Constructor for the example, using default RoadCharacteristics settings.
+  MonolaneOnrampMerge() : MonolaneOnrampMerge(RoadCharacteristics{}) {}
+
+  /// Implements the onramp example.
+  std::unique_ptr<const maliput::api::RoadGeometry> BuildOnramp();
+
+ private:
+  /// Tolerances for monolane's Builder.
+  const double linear_tolerance_  = 0.01;
+  const double angular_tolerance_ = 0.01 * M_PI;
+
+  const RoadCharacteristics rc_;
+};
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/test/CMakeLists.txt

@@ -23,6 +23,9 @@ target_link_libraries(idm_planner_test drakeAutomotive)
 drake_add_cc_test(single_lane_ego_and_agent_test)
 target_link_libraries(single_lane_ego_and_agent_test drakeAutomotive)
 
+drake_add_cc_test(monolane_onramp_merge_test)
+target_link_libraries(monolane_onramp_merge_test drakeAutomotive)
+
 if(lcm_FOUND)
   drake_add_cc_test(simple_car_state_translator_test)
   target_link_libraries(simple_car_state_translator_test drakeAutomotiveLcm)

drake/automotive/test/monolane_onramp_merge_test.cc

@@ -0,0 +1,61 @@
+#include "drake/automotive/monolane_onramp_merge.h"
+
+#include <memory>
+
+#include "gtest/gtest.h"
+
+namespace drake {
+namespace automotive {
+namespace {
+
+namespace mono = maliput::monolane;
+
+// Tests the soundness of the MonolaneOnrampMerge example generator.
+GTEST_TEST(MonolaneOnrampMergeTest, TestDefaultAndNonDefaultAttributes) {
+  const double kSPosition = 0.;
+
+  // Create the road with default road characteristics.
+  std::unique_ptr<MonolaneOnrampMerge> merge_example(new MonolaneOnrampMerge);
+  std::unique_ptr<const maliput::api::RoadGeometry> rg =
+      merge_example->BuildOnramp();
+  EXPECT_NE(nullptr, rg);
+
+  // Check the correctness of the default parameters.
+  EXPECT_EQ(
+      RoadCharacteristics{}.lane_bounds.r_max,
+      rg->junction(0)->segment(0)->lane(0)->lane_bounds(kSPosition).r_max);
+  EXPECT_EQ(
+      RoadCharacteristics{}.driveable_bounds.r_max,
+      rg->junction(0)->segment(0)->lane(0)->driveable_bounds(kSPosition).r_max);
+
+  EXPECT_EQ(rg->id().id, "monolane-merge-example");
+  EXPECT_EQ(rg->num_junctions(), 8);
+
+  // Initialize non-default road characteristics.
+  const double kLaneWidth = 6.3;
+  const double kDriveableWidth = 9.3;
+  const RoadCharacteristics new_road{kLaneWidth, kDriveableWidth};
+
+  std::unique_ptr<MonolaneOnrampMerge> new_merge_example(
+      new MonolaneOnrampMerge(new_road));
+  std::unique_ptr<const maliput::api::RoadGeometry> new_rg =
+      new_merge_example->BuildOnramp();
+  EXPECT_NE(nullptr, new_rg);
+
+  // Check the correctness of the non-default parameters.
+  EXPECT_EQ(
+      kLaneWidth / 2.,
+      new_rg->junction(0)->segment(0)->lane(0)->lane_bounds(kSPosition).r_max);
+  EXPECT_EQ(kDriveableWidth / 2., new_rg->junction(0)
+                                      ->segment(0)
+                                      ->lane(0)
+                                      ->driveable_bounds(kSPosition)
+                                      .r_max);
+
+  EXPECT_EQ(new_rg->id().id, "monolane-merge-example");
+  EXPECT_EQ(new_rg->num_junctions(), 8);
+}
+
+}  // namespace
+}  // namespace automotive
+}  // namespace drake