-
Notifications
You must be signed in to change notification settings - Fork 0
/
idm_controller.cc
132 lines (108 loc) · 4.67 KB
/
idm_controller.cc
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
#include "drake/automotive/idm_controller.h"
#include <limits>
#include <utility>
#include <vector>
#include "drake/common/cond.h"
#include "drake/common/drake_assert.h"
#include "drake/common/symbolic_formula.h"
#include "drake/math/saturate.h"
namespace drake {
namespace automotive {
using maliput::api::RoadGeometry;
using maliput::api::RoadPosition;
using maliput::api::Rotation;
using math::saturate;
using pose_selector::RoadOdometry;
using systems::rendering::FrameVelocity;
using systems::rendering::PoseBundle;
using systems::rendering::PoseVector;
static constexpr int kIdmParamsIndex{0};
template <typename T>
IdmController<T>::IdmController(const RoadGeometry& road)
: road_(road),
ego_pose_index_{
this->DeclareVectorInputPort(PoseVector<T>()).get_index()},
ego_velocity_index_{
this->DeclareVectorInputPort(FrameVelocity<T>()).get_index()},
traffic_index_{this->DeclareAbstractInputPort().get_index()},
acceleration_index_{
this->DeclareVectorOutputPort(systems::BasicVector<T>(1))
.get_index()} {
this->DeclareNumericParameter(IdmPlannerParameters<T>());
}
template <typename T>
IdmController<T>::~IdmController() {}
template <typename T>
const systems::InputPortDescriptor<T>& IdmController<T>::ego_pose_input()
const {
return systems::System<T>::get_input_port(ego_pose_index_);
}
template <typename T>
const systems::InputPortDescriptor<T>& IdmController<T>::ego_velocity_input()
const {
return systems::System<T>::get_input_port(ego_velocity_index_);
}
template <typename T>
const systems::InputPortDescriptor<T>& IdmController<T>::traffic_input() const {
return systems::System<T>::get_input_port(traffic_index_);
}
template <typename T>
const systems::OutputPortDescriptor<T>& IdmController<T>::acceleration_output()
const {
return systems::System<T>::get_output_port(acceleration_index_);
}
template <typename T>
void IdmController<T>::DoCalcOutput(const systems::Context<T>& context,
systems::SystemOutput<T>* output) const {
// Obtain the parameters.
const IdmPlannerParameters<T>& idm_params =
this->template GetNumericParameter<IdmPlannerParameters>(context,
kIdmParamsIndex);
// Obtain the input/output data structures.
const PoseVector<T>* const ego_pose =
this->template EvalVectorInput<PoseVector>(context, ego_pose_index_);
DRAKE_ASSERT(ego_pose != nullptr);
const FrameVelocity<T>* const ego_velocity =
this->template EvalVectorInput<FrameVelocity>(context,
ego_velocity_index_);
DRAKE_ASSERT(ego_velocity != nullptr);
const PoseBundle<T>* const traffic_poses =
this->template EvalInputValue<PoseBundle<T>>(context, traffic_index_);
DRAKE_ASSERT(traffic_poses != nullptr);
systems::BasicVector<T>* const accel_output =
output->GetMutableVectorData(acceleration_index_);
DRAKE_ASSERT(accel_output != nullptr);
ImplDoCalcOutput(*ego_pose, *ego_velocity, *traffic_poses, idm_params,
accel_output);
}
template <typename T>
void IdmController<T>::ImplDoCalcOutput(
const PoseVector<T>& ego_pose, const FrameVelocity<T>& ego_velocity,
const PoseBundle<T>& traffic_poses,
const IdmPlannerParameters<T>& idm_params,
systems::BasicVector<T>* command) const {
DRAKE_DEMAND(idm_params.IsValid());
// Find the single closest car ahead.
const RoadOdometry<T>& lead_car_odom =
pose_selector::FindClosestLeading(road_, ego_pose, traffic_poses);
const RoadPosition ego_position =
pose_selector::CalcRoadPosition(road_, ego_pose.get_isometry());
const T& s_ego = ego_position.pos.s();
const T& s_dot_ego = pose_selector::GetSVelocity(
RoadOdometry<double>(ego_position, ego_velocity));
const T& s_lead = lead_car_odom.pos.s();
const T& s_dot_lead = pose_selector::GetSVelocity(lead_car_odom);
// Saturate the net_distance at distance_lower_bound away from the ego car to
// avoid near-singular solutions inherent to the IDM equation.
const T net_distance = saturate(s_lead - s_ego - idm_params.bloat_diameter(),
idm_params.distance_lower_limit(),
std::numeric_limits<T>::infinity());
const T closing_velocity = s_dot_ego - s_dot_lead;
// Compute the acceleration command from the IDM equation.
(*command)[0] = IdmPlanner<T>::Evaluate(idm_params, s_dot_ego, net_distance,
closing_velocity);
}
// These instantiations must match the API documentation in idm_controller.h.
template class IdmController<double>;
} // namespace automotive
} // namespace drake