Drake
idm_controller.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 
5 #include <Eigen/Geometry>
6 
16 
17 namespace drake {
18 namespace automotive {
19 
50 template <typename T>
52  public:
54 
55 
58  ~IdmController() override;
59 
67 
68  protected:
69  const maliput::api::RoadGeometry& road() const { return road_; }
70  int ego_pose_index() const { return ego_pose_index_; }
71  int ego_velocity_index() const { return ego_velocity_index_; }
72  int traffic_index() const { return traffic_index_; }
73  int acceleration_index() const { return acceleration_index_; }
74 
76  const systems::rendering::PoseVector<T>& ego_pose,
77  const systems::rendering::FrameVelocity<T>& ego_velocity,
78  const systems::rendering::PoseBundle<T>& traffic_poses,
79  const IdmPlannerParameters<T>& idm_params,
80  systems::BasicVector<T>* command) const;
81 
82  private:
83  // Converts @p pose into RoadPosition.
84  const maliput::api::RoadPosition GetRoadPosition(
85  const Isometry3<T>& pose) const;
86 
87  void CalcAcceleration(const systems::Context<T>& context,
88  systems::BasicVector<T>* accel_output) const;
89 
90  // TODO(jadecastro): Introduce AutoDiff support and unit tests.
91 
92  const maliput::api::RoadGeometry& road_;
93 
94  // Indices for the input / output ports.
95  const int ego_pose_index_{};
96  const int ego_velocity_index_{};
97  const int traffic_index_{};
98  const int acceleration_index_{};
99 };
100 
101 } // namespace automotive
102 } // namespace drake
const systems::InputPortDescriptor< T > & ego_velocity_input() const
Definition: idm_controller.cc:49
Eigen::Transform< Scalar, 3, Eigen::Isometry > Isometry3
An Isometry templated on scalar type.
Definition: eigen_types.h:106
IdmController implements the IDM (Intelligent Driver Model) planner, computed based only on the neare...
Definition: idm_controller.h:51
Definition: automotive_demo.cc:88
int acceleration_index() const
Definition: idm_controller.h:73
const maliput::api::RoadGeometry & road() const
Definition: idm_controller.h:69
const systems::OutputPort< T > & acceleration_output() const
Definition: idm_controller.cc:60
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
IdmController(const IdmController &)=delete
int traffic_index() const
Definition: idm_controller.h:72
int ego_velocity_index() const
Definition: idm_controller.h:71
InputPortDescriptor is a notation for specifying the kind of input a System accepts, on a given port.
Definition: input_port_descriptor.h:19
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:70
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:91
~IdmController() override
Definition: idm_controller.cc:40
const systems::InputPortDescriptor< T > & traffic_input() const
Definition: idm_controller.cc:55
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:43
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
Specializes BasicVector with specific getters and setters.
Definition: idm_planner_parameters.h:44
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:35
A position in the road network, consisting of a pointer to a specific Lane and a Lane-frame position ...
Definition: lane_data.h:258
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:43