Skip to content

Commit

Permalink
Removes monolane dependency from road_path_test and `idm_controller…
Browse files Browse the repository at this point in the history
  • Loading branch information
agalbachicar authored Sep 14, 2018
1 parent 9201bb6 commit 86cb328
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 29 deletions.
4 changes: 1 addition & 3 deletions automotive/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -372,7 +372,6 @@ drake_cc_library(
hdrs = ["road_path.h"],
deps = [
":lane_direction",
":monolane_onramp_merge",
"//automotive/maliput/api",
"//common:essential",
"//common:unused",
Expand Down Expand Up @@ -717,7 +716,6 @@ drake_cc_googletest(
name = "idm_controller_test",
deps = [
"//automotive:idm_controller",
"//automotive:monolane_onramp_merge",
"//automotive/maliput/dragway",
"//common/test_utilities:eigen_matrix_compare",
"//systems/framework/test_utilities:scalar_conversion",
Expand Down Expand Up @@ -768,7 +766,7 @@ drake_cc_googletest(
deps = [
":road_path",
"//automotive/maliput/dragway",
"//automotive/maliput/monolane",
"//automotive/maliput/multilane",
"//common/test_utilities:eigen_matrix_compare",
],
)
Expand Down
1 change: 0 additions & 1 deletion automotive/test/idm_controller_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
#include <gtest/gtest.h>

#include "drake/automotive/maliput/dragway/road_geometry.h"
#include "drake/automotive/monolane_onramp_merge.h"
#include "drake/common/eigen_types.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/multibody/multibody_tree/math/spatial_velocity.h"
Expand Down
65 changes: 40 additions & 25 deletions automotive/test/road_path_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -9,24 +9,31 @@
#include "drake/automotive/maliput/api/lane_data.h"
#include "drake/automotive/maliput/api/road_geometry.h"
#include "drake/automotive/maliput/dragway/road_geometry.h"
#include "drake/automotive/maliput/monolane/builder.h"
#include "drake/automotive/monolane_onramp_merge.h"
#include "drake/automotive/maliput/multilane/builder.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"

namespace drake {
namespace automotive {
namespace {

using maliput::api::GeoPosition;
using maliput::api::HBounds;
using maliput::api::JunctionId;
using maliput::api::Lane;
using maliput::api::LaneEnd;
using maliput::api::LanePosition;
using maliput::api::RoadGeometry;
using maliput::monolane::Builder;
using maliput::monolane::Connection;
using maliput::monolane::Endpoint;
using maliput::monolane::EndpointZ;

using maliput::multilane::ArcOffset;
using maliput::multilane::Builder;
using maliput::multilane::ComputationPolicy;
using maliput::multilane::Direction;
using maliput::multilane::Endpoint;
using maliput::multilane::EndpointZ;
using maliput::multilane::LaneLayout;
using maliput::multilane::LineOffset;
using maliput::multilane::EndReference;
using maliput::multilane::StartReference;

// The length of the straight lane segment.
const double kStraightRoadLength{10};
Expand All @@ -41,32 +48,40 @@ const EndpointZ kEndZ{0, 0, 0, 0}; // Specifies zero elevation/super-elevation.

// Build a road with two lanes in series.
std::unique_ptr<const RoadGeometry> MakeTwoLaneRoad(bool is_opposing) {
Builder builder(maliput::api::RBounds(-2, 2), /* lane_bounds */
maliput::api::RBounds(-4, 4), /* driveable_bounds */
maliput::api::HBounds(0, 5), /* elevation bounds */
0.01, /* linear tolerance */
M_PI_2 / 180.0); /* angular_tolerance */
builder.Connect("0_fwd", /* id */
Endpoint({0, 0, 0}, kEndZ), /* start */
kStraightRoadLength, /* length */
kEndZ); /* z_end */
Builder builder(
4. /* lane width */, HBounds(0., 5.), 0.01 /* linear tolerance */,
M_PI_2 / 180.0 /* angular tolerance */, 1. /* scale length*/,
ComputationPolicy::kPreferAccuracy /* accuracy */);
const LaneLayout lane_layout(
2. /* left shoulder */, 2. /* right shoulder */, 1 /* number of lanes */,
0 /* reference lane*/, 0. /* reference r0 */);

builder.Connect(
"0_fwd", lane_layout,
StartReference().at(Endpoint({0, 0, 0}, kEndZ), Direction::kForward),
LineOffset(kStraightRoadLength),
EndReference().z_at(kEndZ, Direction::kForward));

if (is_opposing) {
// Construct a curved segment that is directionally opposite the straight
// lane.
builder.Connect("1_rev", /* id */
Endpoint({kStraightRoadLength + kCurvedRoadRadius,
kCurvedRoadRadius, 1.5 * M_PI},
kEndZ), /* start */
{kCurvedRoadRadius, -kCurvedRoadTheta}, /* arc */
kEndZ); /* z_end */
builder.Connect(
"1_rev", lane_layout,
StartReference().at(Endpoint({kStraightRoadLength + kCurvedRoadRadius,
kCurvedRoadRadius, 1.5 * M_PI},
kEndZ),
Direction::kForward),
ArcOffset(kCurvedRoadRadius, -kCurvedRoadTheta),
EndReference().z_at(kEndZ, Direction::kForward));
} else {
// Construct a curved segment that is directionally confluent with the
// straight lane.
builder.Connect("1_fwd", /* id */
Endpoint({kStraightRoadLength, 0, 0}, kEndZ), /* start */
{kCurvedRoadRadius, kCurvedRoadTheta}, /* arc */
kEndZ); /* z_end */
builder.Connect(
"1_fwd", lane_layout,
StartReference().at(Endpoint({kStraightRoadLength, 0, 0}, kEndZ),
Direction::kForward),
ArcOffset(kCurvedRoadRadius, kCurvedRoadTheta),
EndReference().z_at(kEndZ, Direction::kForward));
}

return builder.Build(maliput::api::RoadGeometryId("TwoLaneStretchOfRoad"));
Expand Down

0 comments on commit 86cb328

Please sign in to comment.