Skip to content

Commit

Permalink
Merge pull request #11413 from andrewbest-tri/maliput/api/Intersectio…
Browse files Browse the repository at this point in the history 
…nBook_RoadNetwork

Adds IntersectionBook to RoadNetwork.
  • Loading branch information
@andrewbest-tri
andrewbest-tri authored May 8, 2019
2 parents 7c074fd + 3cd0769 commit b6190fb
Show file tree
Hide file tree
Showing 5 changed files with 81 additions and 136 deletions.
34 changes: 9 additions & 25 deletions automotive/maliput/api/road_network.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <utility>

#include "drake/automotive/maliput/api/lane.h"
#include "drake/automotive/maliput/api/rules/regions.h"
#include "drake/common/drake_throw.h"

namespace drake {
Expand All @@ -13,30 +14,24 @@ RoadNetwork::RoadNetwork(
std::unique_ptr<const RoadGeometry> road_geometry,
std::unique_ptr<const rules::RoadRulebook> rulebook,
std::unique_ptr<const rules::TrafficLightBook> traffic_light_book,
std::vector<std::unique_ptr<Intersection>> intersections,
std::unique_ptr<const IntersectionBook> intersection_book,
std::unique_ptr<rules::PhaseRingBook> phase_ring_book,
std::unique_ptr<rules::RuleStateProvider> rule_state_provider,
std::unique_ptr<rules::PhaseProvider> phase_provider,
std::vector<rules::SpeedLimitRule> speed_limits,
std::vector<rules::DirectionUsageRule> direction_usage_rules)
std::unique_ptr<rules::PhaseProvider> phase_provider)
: road_geometry_(std::move(road_geometry)),
rulebook_(std::move(rulebook)),
traffic_light_book_(std::move(traffic_light_book)),
intersections_(std::move(intersections)),
intersection_book_(std::move(intersection_book)),
phase_ring_book_(std::move(phase_ring_book)),
rule_state_provider_(std::move(rule_state_provider)),
phase_provider_(std::move(phase_provider)),
speed_limits_(std::move(speed_limits)),
direction_usage_rules_(std::move(direction_usage_rules)) {
phase_provider_(std::move(phase_provider)) {
DRAKE_THROW_UNLESS(road_geometry_.get() != nullptr);
DRAKE_THROW_UNLESS(rulebook_.get() != nullptr);
DRAKE_THROW_UNLESS(traffic_light_book_.get() != nullptr);
DRAKE_THROW_UNLESS(intersection_book_.get() != nullptr);
DRAKE_THROW_UNLESS(phase_ring_book_.get() != nullptr);
DRAKE_THROW_UNLESS(rule_state_provider_.get() != nullptr);
DRAKE_THROW_UNLESS(phase_provider_.get() != nullptr);
for (int i = 0; i < static_cast<int>(intersections_.size()); ++i) {
intersections_map_[intersections_.at(i)->id()] = intersections_.at(i).get();
}

// Confirms full DirectionUsageRule coverage. Currently, this is determined by
// verifying that each Lane within the RoadGeometry has an associated
Expand All @@ -46,21 +41,10 @@ RoadNetwork::RoadNetwork(
const auto& lanes_map = road_geometry_->ById().GetLanes();
for (const auto& lane_map : lanes_map) {
const LaneId lane_id = lane_map.first;
const auto RuleIdMatchesLaneId = [&](rules::DirectionUsageRule& rule) {
return rule.zone().lane_id() == lane_id;
};
const auto lane_rule = std::find_if(direction_usage_rules_.begin(),
direction_usage_rules_.end(), RuleIdMatchesLaneId);
DRAKE_THROW_UNLESS(lane_rule != direction_usage_rules_.end());
}
}

optional<const Intersection*> RoadNetwork::intersection(
const Intersection::Id& id) const {
if (intersections_map_.find(id) == intersections_map_.end()) {
return nullopt;
const auto results =
rulebook_->FindRules({{lane_id, {0.0, lane_map.second->length()}}}, 0);
DRAKE_THROW_UNLESS(results.direction_usage.size() > 0);
}
return intersections_map_.at(id);
}

} // namespace api
Expand Down
27 changes: 7 additions & 20 deletions automotive/maliput/api/road_network.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
#include <unordered_map>
#include <vector>

#include "drake/automotive/maliput/api/intersection.h"
#include "drake/automotive/maliput/api/intersection_book.h"
#include "drake/automotive/maliput/api/road_geometry.h"
#include "drake/automotive/maliput/api/rules/direction_usage_rule.h"
#include "drake/automotive/maliput/api/rules/phase_provider.h"
Expand All @@ -30,12 +30,10 @@ class RoadNetwork {
RoadNetwork(std::unique_ptr<const RoadGeometry> road_geometry,
std::unique_ptr<const rules::RoadRulebook> rulebook,
std::unique_ptr<const rules::TrafficLightBook> traffic_light_book,
std::vector<std::unique_ptr<Intersection>> intersections,
std::unique_ptr<const IntersectionBook> intersection_book,
std::unique_ptr<rules::PhaseRingBook> phase_ring_book,
std::unique_ptr<rules::RuleStateProvider> rule_state_provider,
std::unique_ptr<rules::PhaseProvider> phase_provider,
std::vector<rules::SpeedLimitRule> speed_limits,
std::vector<rules::DirectionUsageRule> direction_usage_rules);
std::unique_ptr<rules::PhaseProvider> phase_provider);

virtual ~RoadNetwork() = default;

Expand All @@ -47,9 +45,9 @@ class RoadNetwork {
return traffic_light_book_.get();
}

/// Returns a pointer to the Intersection with the specified ID, or nullopt if
/// no such Intersection exists.
optional<const Intersection*> intersection(const Intersection::Id& id) const;
const IntersectionBook* intersection_book() const {
return intersection_book_.get();
}

const rules::PhaseRingBook* phase_ring_book() const {
return phase_ring_book_.get();
Expand All @@ -63,25 +61,14 @@ class RoadNetwork {
return phase_provider_.get();
}

const std::vector<rules::SpeedLimitRule>* speed_limits() const {
return &speed_limits_;
}

const std::vector<rules::DirectionUsageRule>* direction_usage_rules() const {
return &direction_usage_rules_;
}

private:
std::unique_ptr<const RoadGeometry> road_geometry_;
std::unique_ptr<const rules::RoadRulebook> rulebook_;
std::unique_ptr<const rules::TrafficLightBook> traffic_light_book_;
std::vector<std::unique_ptr<Intersection>> intersections_;
std::unordered_map<Intersection::Id, Intersection*> intersections_map_;
std::unique_ptr<const IntersectionBook> intersection_book_;
std::unique_ptr<rules::PhaseRingBook> phase_ring_book_;
std::unique_ptr<rules::RuleStateProvider> rule_state_provider_;
std::unique_ptr<rules::PhaseProvider> phase_provider_;
std::vector<rules::SpeedLimitRule> speed_limits_;
std::vector<rules::DirectionUsageRule> direction_usage_rules_;
};

} // namespace api
Expand Down
52 changes: 22 additions & 30 deletions automotive/maliput/api/test/mock.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,15 +80,13 @@ class MockRoadRulebook final : public rules::RoadRulebook {
double) const override {
return {{}, {}};
}
RightOfWayRule DoGetRule(
const RightOfWayRule::Id&) const override {
RightOfWayRule DoGetRule(const RightOfWayRule::Id&) const override {
return Rule();
}
SpeedLimitRule DoGetRule(const SpeedLimitRule::Id&) const override {
return SpeedLimitRule(rules::SpeedLimitRule::Id("some_id"),
CreateLaneSRange(),
rules::SpeedLimitRule::Severity::kStrict, 33.,
77.);
rules::SpeedLimitRule::Severity::kStrict, 33., 77.);
}

DirectionUsageRule DoGetRule(const DirectionUsageRule::Id&) const override {
Expand All @@ -102,8 +100,8 @@ class MockTrafficLightBook final : public rules::TrafficLightBook {
MockTrafficLightBook() {}

private:
optional<TrafficLight> DoGetTrafficLight(const TrafficLight::Id&) const
override {
optional<TrafficLight> DoGetTrafficLight(
const TrafficLight::Id&) const override {
return nullopt;
}
};
Expand All @@ -114,13 +112,13 @@ class MockPhaseRingBook final : public rules::PhaseRingBook {
MockPhaseRingBook() {}

private:
optional<rules::PhaseRing> DoGetPhaseRing(const rules::PhaseRing::Id&) const
override {
optional<rules::PhaseRing> DoGetPhaseRing(
const rules::PhaseRing::Id&) const override {
return nullopt;
}

optional<rules::PhaseRing> DoFindPhaseRing(const rules::RightOfWayRule::Id&)
const override {
optional<rules::PhaseRing> DoFindPhaseRing(
const rules::RightOfWayRule::Id&) const override {
return nullopt;
}
};
Expand All @@ -131,8 +129,8 @@ class MockRuleStateProvider final : public rules::RuleStateProvider {
MockRuleStateProvider() {}

private:
drake::optional<RightOfWayResult> DoGetState(const RightOfWayRule::Id&) const
override {
drake::optional<RightOfWayResult> DoGetState(
const RightOfWayRule::Id&) const override {
return nullopt;
}
};
Expand All @@ -148,27 +146,22 @@ class MockPhaseProvider final : public rules::PhaseProvider {
}
};

class MockIntersection final : public Intersection {
class MockIntersectionBook final : public IntersectionBook {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MockIntersection)
explicit MockIntersection(const Intersection::Id& id,
const rules::PhaseRing::Id& ring_id)
: Intersection(id, {}, ring_id) {}
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MockIntersectionBook);
MockIntersectionBook() {}

private:
optional<rules::PhaseProvider::Result> Phase() const override {
return nullopt;
api::Intersection* DoGetIntersection(const api::Intersection::Id&) {
return nullptr;
}

void SetPhase(const api::rules::Phase::Id&) override {}
};

} // namespace

rules::LaneSRoute CreateLaneSRoute() {
return rules::LaneSRoute(
{rules::LaneSRange(LaneId("a"), {0., 9.}),
rules::LaneSRange(LaneId("b"), {17., 12.})});
return rules::LaneSRoute({rules::LaneSRange(LaneId("a"), {0., 9.}),
rules::LaneSRange(LaneId("b"), {17., 12.})});
}

rules::LaneSRange CreateLaneSRange() {
Expand Down Expand Up @@ -198,9 +191,9 @@ RightOfWayRule Rule() {

DirectionUsageRule::State CreateDirectionUsageRuleState() {
return DirectionUsageRule::State(
DirectionUsageRule::State::Id("dur_state"),
DirectionUsageRule::State::Type::kWithS,
DirectionUsageRule::State::Severity::kStrict);
DirectionUsageRule::State::Id("dur_state"),
DirectionUsageRule::State::Type::kWithS,
DirectionUsageRule::State::Severity::kStrict);
}

DirectionUsageRule CreateDirectionUsageRule() {
Expand Down Expand Up @@ -233,9 +226,8 @@ std::unique_ptr<rules::PhaseProvider> CreatePhaseProvider() {
return std::make_unique<MockPhaseProvider>();
}

std::unique_ptr<Intersection> CreateIntersection(
const Intersection::Id& id, const rules::PhaseRing::Id& ring_id) {
return std::make_unique<MockIntersection>(id, ring_id);
std::unique_ptr<IntersectionBook> CreateIntersectionBook() {
return std::make_unique<MockIntersectionBook>();
}

} // namespace test
Expand Down
7 changes: 3 additions & 4 deletions automotive/maliput/api/test/mock.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

#include <memory>

#include "drake/automotive/maliput/api/intersection.h"
#include "drake/automotive/maliput/api/intersection_book.h"
#include "drake/automotive/maliput/api/road_geometry.h"
#include "drake/automotive/maliput/api/rules/direction_usage_rule.h"
#include "drake/automotive/maliput/api/rules/phase_provider.h"
Expand Down Expand Up @@ -61,9 +61,8 @@ std::unique_ptr<rules::RuleStateProvider> CreateRuleStateProvider();
/// Returns an arbitrary rules::PhaseProvider.
std::unique_ptr<rules::PhaseProvider> CreatePhaseProvider();

/// Returns an arbitrary Intersection.
std::unique_ptr<Intersection> CreateIntersection(
const Intersection::Id& id, const rules::PhaseRing::Id& ring_id);
/// Returns an arbitrary IntersectionBook.
std::unique_ptr<IntersectionBook> CreateIntersectionBook();

} // namespace test
} // namespace api
Expand Down
Loading

0 comments on commit b6190fb

Please sign in to comment.