Drake
idm_controller.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 
5 #include <Eigen/Geometry>
6 
7 #include "drake/automotive/gen/idm_planner_parameters.h"
16 
17 namespace drake {
18 namespace automotive {
19 
20 /// IdmController implements the IDM (Intelligent Driver Model) planner,
21 /// computed based only on the nearest car ahead. See IdmPlanner and
22 /// PoseSelector for details. The output of this block is an acceleration value
23 /// passed as a command to the vehicle.
24 ///
25 /// Instantiated templates for the following kinds of T's are provided:
26 /// - double
27 ///
28 /// They are already available to link against in the containing library.
29 ///
30 /// Input Port 0: PoseVector for the ego car.
31 /// (InputPortDescriptor getter: ego_pose_input())
32 ///
33 /// Input Port 1: FrameVelocity of the ego car.
34 /// (InputPortDescriptor getter: ego_velocity_input())
35 ///
36 /// Input Port 2: PoseBundle for the traffic cars, possibly inclusive of the ego
37 /// car's pose.
38 /// (InputPortDescriptor getter: traffic_input())
39 ///
40 /// Output Port 0: A BasicVector containing the acceleration request.
41 /// (OutputPort getter: acceleration_output())
42 ///
43 ///
44 /// Instantiated templates for the following kinds of T's are provided:
45 /// - double
46 /// - AutoDiffXd
47 ///
48 /// They are already available to link against in the containing library.
49 ///
50 /// @ingroup automotive_controllers
51 template <typename T>
53  public:
55 
56  /// Constructor.
57  /// @param road is the pre-defined RoadGeometry.
59 
60  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
61  template <typename U>
62  explicit IdmController(const IdmController<U>& other)
63  : IdmController<T>(other.road_) {}
64 
65  ~IdmController() override;
66 
67  /// See the class description for details on the following input ports.
68  /// @{
73  /// @}
74 
75  protected:
76  const maliput::api::RoadGeometry& road() const { return road_; }
77  int ego_pose_index() const { return ego_pose_index_; }
78  int ego_velocity_index() const { return ego_velocity_index_; }
79  int traffic_index() const { return traffic_index_; }
80  int acceleration_index() const { return acceleration_index_; }
81 
83  const systems::rendering::PoseVector<T>& ego_pose,
84  const systems::rendering::FrameVelocity<T>& ego_velocity,
85  const systems::rendering::PoseBundle<T>& traffic_poses,
86  const IdmPlannerParameters<T>& idm_params,
87  systems::BasicVector<T>* command) const;
88 
89  private:
90  // Allow different specializations to access each other's private data.
91  template <typename> friend class IdmController;
92 
93  // Converts @p pose into RoadPosition.
94  const maliput::api::RoadPosition GetRoadPosition(
95  const Isometry3<T>& pose) const;
96 
97  void CalcAcceleration(const systems::Context<T>& context,
98  systems::BasicVector<T>* accel_output) const;
99 
101 
102  // Indices for the input / output ports.
103  const int ego_pose_index_{};
104  const int ego_velocity_index_{};
105  const int traffic_index_{};
106  const int acceleration_index_{};
107 };
108 
109 } // namespace automotive
110 
111 namespace systems {
112 namespace scalar_conversion {
113 // Disable symbolic support, because we use ExtractDoubleOrThrow.
114 template <>
115 struct Traits<automotive::IdmController> : public NonSymbolicTraits {};
116 } // namespace scalar_conversion
117 } // namespace systems
118 
119 } // namespace drake
const systems::InputPortDescriptor< T > & ego_velocity_input() const
Definition: idm_controller.cc:55
Eigen::Transform< Scalar, 3, Eigen::Isometry > Isometry3
An Isometry templated on scalar type.
Definition: eigen_types.h:106
IdmController(const IdmController< U > &other)
Scalar-converting copy constructor. See System Scalar Conversion.
Definition: idm_controller.h:62
IdmController implements the IDM (Intelligent Driver Model) planner, computed based only on the neare...
Definition: idm_controller.h:52
std::unique_ptr< maliput::dragway::RoadGeometry > road_
Definition: idm_controller_test.cc:95
Definition: automotive_demo.cc:88
int acceleration_index() const
Definition: idm_controller.h:80
const maliput::api::RoadGeometry & road() const
Definition: idm_controller.h:76
const systems::OutputPort< T > & acceleration_output() const
Definition: idm_controller.cc:66
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
PoseBundle is a container for a set of poses, represented by an Isometry3, and corresponding velociti...
Definition: pose_bundle.h:40
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:79
int ego_velocity_index() const
Definition: idm_controller.h:78
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
int ego_pose_index() const
Definition: idm_controller.h:77
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, systems::BasicVector< T > *command) const
Definition: idm_controller.cc:97
~IdmController() override
Definition: idm_controller.cc:46
const systems::InputPortDescriptor< T > & traffic_input() const
Definition: idm_controller.cc:61
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:91
A concrete traits class providing sugar to disable support for symbolic evaluation (i...
Definition: scalar_conversion_traits.h:52
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
A position in the road network, consisting of a pointer to a specific Lane and a Lane-frame position ...
Definition: lane_data.h:282
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
const systems::InputPortDescriptor< T > & ego_pose_input() const
See the class description for details on the following input ports.
Definition: idm_controller.cc:49