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 
21 
22 namespace drake {
23 namespace automotive {
24 
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 
90  explicit MobilPlanner(const maliput::api::RoadGeometry& road,
94  bool initial_with_s);
95 
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;
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
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
Specializes BasicVector with specific getters and setters.
Definition: idm_planner_parameters.h:44
Specializes BasicVector with specific getters and setters.
Definition: mobil_planner_parameters.h:38
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:25
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:35
Provides careful macros to selectively enable or disable the special member functions for copy-constr...