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