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 #include <vector>
7 
8 #include <Eigen/Geometry>
9 
10 #include "drake/automotive/calc_ongoing_road_position.h"
11 #include "drake/automotive/gen/idm_planner_parameters.h"
12 #include "drake/automotive/gen/mobil_planner_parameters.h"
13 #include "drake/automotive/idm_planner.h"
14 #include "drake/automotive/lane_direction.h"
15 #include "drake/automotive/maliput/api/lane.h"
16 #include "drake/automotive/maliput/api/road_geometry.h"
17 #include "drake/automotive/pose_selector.h"
18 #include "drake/automotive/road_odometry.h"
19 #include "drake/common/drake_copyable.h"
20 #include "drake/systems/framework/leaf_system.h"
21 #include "drake/systems/rendering/pose_bundle.h"
22 #include "drake/systems/rendering/pose_vector.h"
23 
24 namespace drake {
25 namespace automotive {
26 
27 /// MOBIL (Minimizing Overall Braking Induced by Lane Changes) [1] is a planner
28 /// that minimizes braking requirement for the ego car while also minimizing
29 /// (per a weighting factor) the braking requirements of any trailing cars
30 /// within the ego car's immediate neighborhood. The weighting factor
31 /// encapsulates the politeness of the ego car to the surrounding traffic.
32 /// Neighboring cars are defined as those cars immediately ahead and behind the
33 /// ego, in the current lane and any adjacent lanes; these are determined from
34 /// the PoseSelector logic applied to a multi-lane Maliput road.
35 ///
36 /// The induced braking by the ego car and the car following immediately behind
37 /// it is compared with the induced braking by the ego and its new follower if
38 /// the ego were to move to any of the neighboring lanes. The choice that
39 /// minimizes the induced braking - alternatively maximizes the ego car's
40 /// "incentive" (the weighted sum of accelerations that the ego car and its
41 /// neighbors gain by changing lanes) - is chosen as the new lane request. The
42 /// request is expressed as a LaneDirection, that references a valid lane in the
43 /// provided RoadGeometry and the direction of travel.
44 ///
45 /// Assumptions:
46 /// 1) The planner supports only symmetric lane change rules, without giving
47 /// preference to lanes to the left or right.
48 /// 2) The planner assumes all traffic behaves according to the Intelligent
49 /// Driver Model (IDM).
50 /// 3) All neighboring lanes are confluent (i.e. with_s points in the same
51 /// direction).
52 ///
53 /// Instantiated templates for the following kinds of T's are provided:
54 /// - double
55 ///
56 /// They are already available to link against in the containing library.
57 ///
58 /// Input Port 0: A PoseVector for the ego car.
59 /// (InputPortDescriptor getter: ego_pose_input())
60 ///
61 /// Input Port 1: A FrameVelocity for the ego car.
62 /// (InputPortDescriptor getter: ego_velocity_input())
63 ///
64 /// Input Port 2: A BasicVector containing the ego car's commanded acceleration
65 /// value intercepted from the vehicle's controller (e.g. IdmController).
66 /// (InputPortDescriptor getter: ego_acceleration_input())
67 ///
68 /// Input Port 3: A PoseBundle for the traffic cars, possibly including the ego
69 /// car's pose.
70 /// (InputPortDescriptor getter: traffic_input())
71 ///
72 /// Output Port 0: A LaneDirection containing a lane that the ego vehicle must
73 /// move into and the direction of travel with respect to the lane's canonical
74 /// direction of travel. LaneDirection must be consistent with the provided
75 /// road.
76 /// (OutputPort getter: lane_output())
77 ///
78 /// @ingroup automotive_controllers
79 ///
80 /// [1] Arne Kesting, Martin Treiber and Dirk Helbing, MOBIL: General
81 /// Lane-Changing Model for Car-Following Models, Journal of the
82 /// Transportation Research Board, v1999, 2007, pp 86-94.
83 /// http://trrjournalonline.trb.org/doi/abs/10.3141/1999-10.
84 template <typename T>
85 class MobilPlanner : public systems::LeafSystem<T> {
86  public:
87  typedef typename std::map<AheadOrBehind, const ClosestPose<T>> ClosestPoses;
88 
90 
91  /// A constructor that initializes the MOBIL planner.
92  /// @param road The pre-defined RoadGeometry.
93  /// @param initial_with_s The initial direction of travel in the lane
94  /// corresponding to the ego vehicle's initial state.
95  /// @param road_position_strategy Determines whether or not to memorize
96  /// RoadPosition. See `calc_ongoing_road_position.h`.
97  /// @param period_sec The update period to use if road_position_strategy ==
98  /// RoadPositionStrategy::kCache.
99  MobilPlanner(const maliput::api::RoadGeometry& road, bool initial_with_s,
100  RoadPositionStrategy road_position_strategy,
101  double period_sec);
102 
103  /// See the class description for details on the following input ports.
104  /// @{
105  const systems::InputPortDescriptor<T>& ego_pose_input() const;
106  const systems::InputPortDescriptor<T>& ego_velocity_input() const;
107  const systems::InputPortDescriptor<T>& ego_acceleration_input() const;
108  const systems::InputPortDescriptor<T>& traffic_input() const;
109  const systems::OutputPort<T>& lane_output() const;
110  /// @}
111 
112  /// Getters to mutable named-vector references associated with MobilPlanner's
113  /// Parameters groups.
114  /// @{
115  inline IdmPlannerParameters<T>& get_mutable_idm_params(
116  systems::Context<T>* context) const {
117  return this->template GetMutableNumericParameter<IdmPlannerParameters>(
118  context, kIdmParamsIndex);
119  }
120  inline MobilPlannerParameters<T>& get_mutable_mobil_params(
121  systems::Context<T>* context) const {
122  return this->template GetMutableNumericParameter<MobilPlannerParameters>(
123  context, kMobilParamsIndex);
124  }
125  /// @}
126 
127  protected:
129  const systems::Context<T>& context,
131  systems::State<T>* state) const override;
132 
133  private:
134  void CalcLaneDirection(const systems::Context<T>& context,
135  LaneDirection* lane_direction) const;
136 
137  // Performs the calculations for the lane_output() port.
138  void ImplCalcLaneDirection(
139  const systems::rendering::PoseVector<T>& ego_pose,
140  const systems::rendering::FrameVelocity<T>& ego_velocity,
141  const systems::rendering::PoseBundle<T>& traffic_poses,
142  const systems::BasicVector<T>& ego_accel_command,
143  const IdmPlannerParameters<T>& idm_params,
144  const MobilPlannerParameters<T>& mobil_params,
145  const maliput::api::RoadPosition& ego_rp,
146  LaneDirection* lane_direction) const;
147 
148  // Computes a pair of incentive measures for the provided neighboring lanes.
149  // The first and second elements in `lanes` correspond to, respectively, a
150  // pair of lanes included in the incentive query. The respective incentives
151  // for these lanes are returned as the first and second elements in the return
152  // value.
153  const std::pair<T, T> ComputeIncentives(
154  const std::pair<const maliput::api::Lane*, const maliput::api::Lane*>
155  lanes,
156  const IdmPlannerParameters<T>& idm_params,
157  const MobilPlannerParameters<T>& mobil_params,
158  const ClosestPose<T>& ego_closest_pose,
159  const systems::rendering::PoseVector<T>& ego_pose,
160  const systems::rendering::PoseBundle<T>& traffic_poses,
161  const T& ego_acceleration) const;
162 
163  // Computes a pair of incentive measures that consider the leading and
164  // trailing vehicles that are closest to the pre-computed result in the
165  // current lane. `closest_poses` contains the odometries and relative
166  // distances to the leading and trailing cars.
167  void ComputeIncentiveOutOfLane(const IdmPlannerParameters<T>& idm_params,
168  const MobilPlannerParameters<T>& mobil_params,
169  const ClosestPoses& closest_poses,
170  const ClosestPose<T>& ego_closest_pose,
171  const T& ego_old_accel,
172  const T& trailing_delta_accel_this,
173  T* incentive) const;
174 
175  // Computes an acceleration based on the IDM equation (via a call to
176  // IdmPlanner::Eval()).
177  const T EvaluateIdm(const IdmPlannerParameters<T>& idm_params,
178  const ClosestPose<T>& trailing_closest_pose,
179  const ClosestPose<T>& leading_closest_pose) const;
180 
181  static constexpr int kIdmParamsIndex{0};
182  static constexpr int kMobilParamsIndex{1};
183  static constexpr double kDefaultLargeAccel{1e6}; // m/s^2
184 
185  const maliput::api::RoadGeometry& road_;
186  const bool with_s_{true};
187  const RoadPositionStrategy road_position_strategy_{};
188 
189  // Indices for the input / output ports.
190  const int ego_pose_index_{};
191  const int ego_velocity_index_{};
192  const int ego_acceleration_index_{};
193  const int traffic_index_{};
194  const int lane_index_{};
195 };
196 
197 } // namespace automotive
198 } // namespace drake
MobilPlannerParameters< T > & get_mutable_mobil_params(systems::Context< T > *context) const
Definition: mobil_planner.h:120
Definition: automotive_demo.cc:89
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: context.h:38
PoseBundle is a container for a set of poses, represented by an Isometry3, and corresponding velociti...
Definition: pose_bundle.h:40
This class represents an unrestricted update event.
Definition: event.h:349
Abstract API for the geometry of a road network, including both the network topology and the geometry...
Definition: road_geometry.h:26
const systems::InputPortDescriptor< T > & ego_acceleration_input() const
Definition: mobil_planner.cc:75
void DoCalcUnrestrictedUpdate(const systems::Context< T > &context, const std::vector< const systems::UnrestrictedUpdateEvent< T > * > &, systems::State< T > *state) const override
Definition: mobil_planner.cc:303
std::vector< double > vector
Definition: translator_test.cc:20
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
RoadPositionStrategy
If kCache, configures the calling system (e.g.
Definition: calc_ongoing_road_position.h:17
State is a container for all the data comprising the complete state of a particular System at a parti...
Definition: state.h:27
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:87
A 7-vector representing the transform of frame A in the world frame, X_WA, in the form {p_WA...
Definition: pose_vector.h:19
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
const systems::InputPortDescriptor< T > & ego_pose_input() const
See the class description for details on the following input ports.
Definition: mobil_planner.cc:64
const systems::InputPortDescriptor< T > & traffic_input() const
Definition: mobil_planner.cc:81
const systems::InputPortDescriptor< T > & ego_velocity_input() const
Definition: mobil_planner.cc:69
MOBIL (Minimizing Overall Braking Induced by Lane Changes) [1] is a planner that minimizes braking re...
Definition: mobil_planner.h:85
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:86
A 6-vector representing the derivatives of the position transform of frame A in the world frame...
Definition: frame_velocity.h:22
#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
A position in the road network, consisting of a pointer to a specific Lane and a Lane-frame position ...
Definition: lane_data.h:302
IdmPlannerParameters< T > & get_mutable_idm_params(systems::Context< T > *context) const
Getters to mutable named-vector references associated with MobilPlanner&#39;s Parameters groups...
Definition: mobil_planner.h:115