Drake
mobil_planner.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <map>
4 #include <memory>
5 #include <utility>
6 
7 #include <Eigen/Geometry>
8 
9 #include "drake/automotive/gen/idm_planner_parameters.h"
10 #include "drake/automotive/gen/mobil_planner_parameters.h"
21 
22 namespace drake {
23 namespace automotive {
24 
25 /// MOBIL (Minimizing Overall Braking Induced by Lane Changes) [1] is a planner
26 /// that minimizes braking requirement for the ego car while also minimizing
27 /// (per a weighting factor) the braking requirements of any trailing cars
28 /// within the ego car's immediate neighborhood. The weighting factor
29 /// encapsulates the politeness of the ego car to the surrounding traffic.
30 /// Neighboring cars are defined as those cars immediately ahead and behind the
31 /// ego, in the current lane and any adjacent lanes; these are determined from
32 /// the PoseSelector logic applied to a multi-lane Maliput road.
33 ///
34 /// The induced braking by the ego car and the car following immediately behind
35 /// it is compared with the induced braking by the ego and its new follower if
36 /// the ego were to move to any of the neighboring lanes. The choice that
37 /// minimizes the induced braking - alternatively maximizes the ego car's
38 /// "incentive" (the weighted sum of accelerations that the ego car and its
39 /// neighbors gain by changing lanes) - is chosen as the new lane request. The
40 /// request is expressed as a LaneDirection, that references a valid lane in the
41 /// provided RoadGeometry and the direction of travel.
42 ///
43 /// Assumptions:
44 /// 1) The planner supports only symmetric lane change rules, without giving
45 /// preference to lanes to the left or right.
46 /// 2) The planner assumes all traffic behaves according to the Intelligent
47 /// Driver Model (IDM).
48 /// 3) All neighboring lanes are confluent (i.e. with_s points in the same
49 /// direction).
50 ///
51 /// Instantiated templates for the following kinds of T's are provided:
52 /// - double
53 ///
54 /// They are already available to link against in the containing library.
55 ///
56 /// Input Port 0: A PoseVector for the ego car.
57 /// (InputPortDescriptor getter: ego_pose_input())
58 ///
59 /// Input Port 1: A FrameVelocity for the ego car.
60 /// (InputPortDescriptor getter: ego_velocity_input())
61 ///
62 /// Input Port 2: A BasicVector containing the ego car's commanded acceleration
63 /// value intercepted from the vehicle's controller (e.g. IdmController).
64 /// (InputPortDescriptor getter: ego_acceleration_input())
65 ///
66 /// Input Port 3: A PoseBundle for the traffic cars, possibly including the ego
67 /// car's pose.
68 /// (InputPortDescriptor getter: traffic_input())
69 ///
70 /// Output Port 0: A LaneDirection containing a lane that the ego vehicle must
71 /// move into and the direction of travel with respect to the lane's canonical
72 /// direction of travel. LaneDirection must be consistent with the provided
73 /// road.
74 /// (OutputPort getter: lane_output())
75 ///
76 /// @ingroup automotive_controllers
77 ///
78 /// [1] Arne Kesting, Martin Treiber and Dirk Helbing, MOBIL: General
79 /// Lane-Changing Model for Car-Following Models, Journal of the
80 /// Transportation Research Board, v1999, 2007, pp 86-94.
81 /// http://trrjournalonline.trb.org/doi/abs/10.3141/1999-10.
82 template <typename T>
83 class MobilPlanner : public systems::LeafSystem<T> {
84  public:
85  typedef typename std::map<AheadOrBehind, const ClosestPose<T>> ClosestPoses;
86 
88 
89  /// A constructor that initializes the MOBIL planner.
90  /// @param road is the pre-defined RoadGeometry.
91  /// @param initial_with_s is the initial direction of travel in the lane
92  /// corresponding to the ego vehicle's initial state.
93  explicit MobilPlanner(const maliput::api::RoadGeometry& road,
94  bool initial_with_s);
95 
96  /// See the class description for details on the following input ports.
97  /// @{
98  const systems::InputPortDescriptor<T>& ego_pose_input() const;
99  const systems::InputPortDescriptor<T>& ego_velocity_input() const;
100  const systems::InputPortDescriptor<T>& ego_acceleration_input() const;
101  const systems::InputPortDescriptor<T>& traffic_input() const;
102  const systems::OutputPort<T>& lane_output() const;
103  /// @}
104 
105  private:
106  void CalcLaneDirection(const systems::Context<T>& context,
107  LaneDirection* lane_direction) const;
108 
109  // Performs the calculations for the lane_output() port.
110  void ImplCalcLaneDirection(
111  const systems::rendering::PoseVector<T>& ego_pose,
112  const systems::rendering::FrameVelocity<T>& ego_velocity,
113  const systems::rendering::PoseBundle<T>& traffic_poses,
114  const systems::BasicVector<T>& ego_accel_command,
115  const IdmPlannerParameters<T>& idm_params,
116  const MobilPlannerParameters<T>& mobil_params,
117  LaneDirection* lane_direction) const;
118 
119  // Computes a pair of incentive measures for the provided neighboring lanes.
120  // The first and second elements in `lanes` correspond to, respectively, a
121  // pair of lanes included in the incentive query. The respective incentives
122  // for these lanes are returned as the first and second elements in the return
123  // value.
124  const std::pair<T, T> ComputeIncentives(
125  const std::pair<const maliput::api::Lane*, const maliput::api::Lane*>
126  lanes,
127  const IdmPlannerParameters<T>& idm_params,
128  const MobilPlannerParameters<T>& mobil_params,
129  const systems::rendering::PoseVector<T>& ego_pose,
130  const systems::rendering::FrameVelocity<T>& ego_velocity,
131  const systems::rendering::PoseBundle<T>& traffic_poses,
132  const T& ego_acceleration) const;
133 
134  // Computes a pair of incentive measures that consider the leading and
135  // trailing vehicles that are closest to the pre-computed result in the
136  // current lane. `closest_poses` contains the odometries and relative
137  // distances to the leading and trailing cars.
138  void ComputeIncentiveOutOfLane(const IdmPlannerParameters<T>& idm_params,
139  const MobilPlannerParameters<T>& mobil_params,
140  const ClosestPoses& closest_poses,
141  const ClosestPose<T>& ego_closest_pose,
142  const T& ego_old_accel,
143  const T& trailing_delta_accel_this,
144  T* incentive) const;
145 
146  // Computes an acceleration based on the IDM equation (via a call to
147  // IdmPlanner::Eval()).
148  const T EvaluateIdm(const IdmPlannerParameters<T>& idm_params,
149  const ClosestPose<T>& trailing_closest_pose,
150  const ClosestPose<T>& leading_closest_pose) const;
151 
152  const maliput::api::RoadGeometry& road_;
153  const bool with_s_{true};
154 
155  // Indices for the input / output ports.
156  const int ego_pose_index_{};
157  const int ego_velocity_index_{};
158  const int ego_acceleration_index_{};
159  const int traffic_index_{};
160  const int lane_index_{};
161 };
162 
163 } // namespace automotive
164 } // namespace drake
Definition: automotive_demo.cc:88
STL namespace.
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
const systems::InputPortDescriptor< T > & ego_acceleration_input() const
Definition: mobil_planner.cc:73
InputPortDescriptor is a notation for specifying the kind of input a System accepts, on a given port.
Definition: input_port_descriptor.h:21
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:67
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
LaneDirection holds the lane that a MaliputRailcar is traversing and the direction in which it is mov...
Definition: lane_direction.h:13
std::map< AheadOrBehind, const ClosestPose< T > > ClosestPoses
Definition: mobil_planner.h:85
const systems::InputPortDescriptor< T > & ego_pose_input() const
See the class description for details on the following input ports.
Definition: mobil_planner.cc:62
const systems::InputPortDescriptor< T > & traffic_input() const
Definition: mobil_planner.cc:79
const systems::InputPortDescriptor< T > & ego_velocity_input() const
Definition: mobil_planner.cc:67
MOBIL (Minimizing Overall Braking Induced by Lane Changes) [1] is a planner that minimizes braking re...
Definition: mobil_planner.h:83
ClosestPose bundles together the RoadOdometry of a particular target along with its distance measure ...
Definition: pose_selector.h:26
const systems::OutputPort< T > & lane_output() const
Definition: mobil_planner.cc:84
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
Provides careful macros to selectively enable or disable the special member functions for copy-constr...