Drake
idm_controller.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <vector>
5 
6 #include <Eigen/Geometry>
7 
8 #include "drake/automotive/calc_ongoing_road_position.h"
9 #include "drake/automotive/gen/idm_planner_parameters.h"
10 #include "drake/automotive/idm_planner.h"
11 #include "drake/automotive/maliput/api/lane_data.h"
12 #include "drake/automotive/maliput/api/road_geometry.h"
13 #include "drake/automotive/pose_selector.h"
14 #include "drake/common/drake_copyable.h"
15 #include "drake/systems/framework/leaf_system.h"
16 #include "drake/systems/rendering/pose_bundle.h"
17 #include "drake/systems/rendering/pose_vector.h"
18 
19 namespace drake {
20 namespace automotive {
21 
22 /// IdmController implements the IDM (Intelligent Driver Model) planner,
23 /// computed based only on the nearest car ahead. See IdmPlanner and
24 /// PoseSelector for details. The output of this block is an acceleration value
25 /// passed as a command to the vehicle.
26 ///
27 /// Instantiated templates for the following kinds of T's are provided:
28 /// - double
29 ///
30 /// They are already available to link against in the containing library.
31 ///
32 /// Input Port 0: PoseVector for the ego car.
33 /// (InputPortDescriptor getter: ego_pose_input())
34 ///
35 /// Input Port 1: FrameVelocity of the ego car.
36 /// (InputPortDescriptor getter: ego_velocity_input())
37 ///
38 /// Input Port 2: PoseBundle for the traffic cars, possibly inclusive of the ego
39 /// car's pose.
40 /// (InputPortDescriptor getter: traffic_input())
41 ///
42 /// Output Port 0: A BasicVector containing the acceleration request.
43 /// (OutputPort getter: acceleration_output())
44 ///
45 ///
46 /// Instantiated templates for the following kinds of T's are provided:
47 /// - double
48 /// - AutoDiffXd
49 ///
50 /// They are already available to link against in the containing library.
51 ///
52 /// @ingroup automotive_controllers
53 template <typename T>
55  public:
57 
58  /// Constructor.
59  /// @param road The pre-defined RoadGeometry.
60  /// @param path_or_branches If ScanStrategy::kBranches, performs IDM
61  /// computations using vehicles detected in confluent branches; if
62  /// ScanStrategy::kPath, limits to vehicles on the default path. See
63  /// documentation for PoseSelector::FindSingleClosestPose().
64  /// @param road_position_strategy Determines whether or not to cache
65  /// RoadPosition. See `calc_ongoing_road_position.h`.
66  /// @param period_sec The update period to use if road_position_strategy ==
67  /// RoadPositionStrategy::kCache.
69  ScanStrategy path_or_branches,
70  RoadPositionStrategy road_position_strategy,
71  double period_sec);
72 
73  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
74  template <typename U>
75  explicit IdmController(const IdmController<U>& other)
76  : IdmController<T>(other.road_, other.path_or_branches_,
77  other.road_position_strategy_,
78  other.period_sec_) {}
79 
80  ~IdmController() override;
81 
82  /// See the class description for details on the following input ports.
83  /// @{
88  /// @}
89 
90  protected:
91  const maliput::api::RoadGeometry& road() const { return road_; }
92  int ego_pose_index() const { return ego_pose_index_; }
93  int ego_velocity_index() const { return ego_velocity_index_; }
94  int traffic_index() const { return traffic_index_; }
95  int acceleration_index() const { return acceleration_index_; }
96 
98  const systems::rendering::PoseVector<T>& ego_pose,
99  const systems::rendering::FrameVelocity<T>& ego_velocity,
100  const systems::rendering::PoseBundle<T>& traffic_poses,
101  const IdmPlannerParameters<T>& idm_params,
102  const maliput::api::RoadPosition& ego_rp,
103  systems::BasicVector<T>* command) const;
104 
106  const systems::Context<T>& context,
108  systems::State<T>* state) const override;
109 
110  private:
111  // Allow different specializations to access each other's private data.
112  template <typename> friend class IdmController;
113 
114  // Converts @p pose into RoadPosition.
115  const maliput::api::RoadPosition GetRoadPosition(
116  const Isometry3<T>& pose) const;
117 
118  void CalcAcceleration(const systems::Context<T>& context,
119  systems::BasicVector<T>* accel_output) const;
120 
122  const ScanStrategy path_or_branches_{};
123  const RoadPositionStrategy road_position_strategy_{};
124  const double period_sec_{};
125 
126  // Indices for the input / output ports.
127  const int ego_pose_index_{};
128  const int ego_velocity_index_{};
129  const int traffic_index_{};
130  const int acceleration_index_{};
131 };
132 
133 } // namespace automotive
134 
135 namespace systems {
136 namespace scalar_conversion {
137 // Disable symbolic support, because we use ExtractDoubleOrThrow.
138 template <>
139 struct Traits<automotive::IdmController> : public NonSymbolicTraits {};
140 } // namespace scalar_conversion
141 } // namespace systems
142 
143 } // namespace drake
const systems::InputPortDescriptor< T > & ego_velocity_input() const
Definition: idm_controller.cc:68
Eigen::Transform< Scalar, 3, Eigen::Isometry > Isometry3
An Isometry templated on scalar type.
Definition: eigen_types.h:127
IdmController(const IdmController< U > &other)
Scalar-converting copy constructor. See System Scalar Conversion.
Definition: idm_controller.h:75
void DoCalcUnrestrictedUpdate(const systems::Context< T > &context, const std::vector< const systems::UnrestrictedUpdateEvent< T > * > &, systems::State< T > *state) const override
Definition: idm_controller.cc:165
IdmController implements the IDM (Intelligent Driver Model) planner, computed based only on the neare...
Definition: idm_controller.h:54
std::unique_ptr< maliput::dragway::RoadGeometry > road_
Definition: idm_controller_test.cc:105
Definition: automotive_demo.cc:89
int acceleration_index() const
Definition: idm_controller.h:95
const maliput::api::RoadGeometry & road() const
Definition: idm_controller.h:91
const systems::OutputPort< T > & acceleration_output() const
Definition: idm_controller.cc:79
double period_sec_
Definition: idm_controller_test.cc:113
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
int traffic_index() const
Definition: idm_controller.h:94
int ego_velocity_index() const
Definition: idm_controller.h:93
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 a planning system (e.g.
Definition: pose_selector.h:53
int ego_pose_index() const
Definition: idm_controller.h:92
~IdmController() override
Definition: idm_controller.cc:59
State is a container for all the data comprising the complete state of a particular System at a parti...
Definition: state.h:27
const systems::InputPortDescriptor< T > & traffic_input() const
Definition: idm_controller.cc:74
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
A templated traits class for whether an S<T> can be converted into an S<U>; the default value is true...
Definition: scalar_conversion_traits.h:30
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
friend class IdmController
Definition: idm_controller.h:112
A concrete traits class providing sugar to disable support for symbolic evaluation (i...
Definition: scalar_conversion_traits.h:52
void ImplCalcAcceleration(const systems::rendering::PoseVector< T > &ego_pose, const systems::rendering::FrameVelocity< T > &ego_velocity, const systems::rendering::PoseBundle< T > &traffic_poses, const IdmPlannerParameters< T > &idm_params, const maliput::api::RoadPosition &ego_rp, systems::BasicVector< T > *command) const
Definition: idm_controller.cc:117
Isometry3< double > pose
Definition: pose_smoother.cc:28
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
ScanStrategy
Specifies whether to check ongoing lanes or both ongoing lanes and confluent branches for traffic...
Definition: pose_selector.h:45
A position in the road network, consisting of a pointer to a specific Lane and a Lane-frame position ...
Definition: lane_data.h:302
const systems::InputPortDescriptor< T > & ego_pose_input() const
See the class description for details on the following input ports.
Definition: idm_controller.cc:62