Drake
maliput_railcar.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <vector>
5 
6 #include "drake/automotive/gen/maliput_railcar_params.h"
7 #include "drake/automotive/gen/maliput_railcar_state.h"
15 
16 namespace drake {
17 namespace automotive {
18 
19 /// MaliputRailcar models a vehicle that follows a maliput::api::Lane as if it
20 /// were on rails and neglecting all physics.
21 ///
22 /// Parameters:
23 /// * See MaliputRailcarParams.
24 ///
25 /// State vector:
26 /// * See MaliputRailcarState.
27 ///
28 /// Abstract state:
29 /// * See LaneDirection.
30 ///
31 /// <B>Input Port Accessors:</B>
32 ///
33 /// - command_input(): Contains the desired acceleration. This port
34 /// contains a systems::BasicVector of size 1. It is optional in that it
35 /// need not be connected. When it is unconnected, the railcar will travel
36 /// at its initial velocity, which is specified in MaliputRailcarParams.
37 ///
38 /// <B>Output Port Accessors:</B>
39 ///
40 /// - state_output(): Contains this system's state vector. See
41 /// MaliputRailcarState.
42 ///
43 /// - lane_state_output(): Contains this system's lane direction state. See
44 /// LaneDirection.
45 ///
46 /// - pose_output(): Contains PoseVector `X_WC`, where `C` is the car frame
47 /// and `W` is the world frame.
48 ///
49 /// - velocity_output(): Contains FrameVelocity `V_WC_W`, where `C` is the car
50 /// frame and `W` is the world frame. Currently the rotational component is
51 /// always zero, see #5751.
52 ///
53 /// @tparam T must support certain arithmetic operations;
54 /// for details, see drake::symbolic::Expression.
55 ///
56 /// Instantiated templates for the following ScalarTypes are provided:
57 /// - double
58 ///
59 /// They are already available to link against in the containing library.
60 ///
61 /// @ingroup automotive_plants
62 template <typename T>
63 class MaliputRailcar final : public systems::LeafSystem<T> {
64  public:
66  /// Defines a distance that is "close enough" to the end of a lane for the
67  /// vehicle to transition to an ongoing branch. The primary constraint on the
68  /// selection of this variable is the application's degree of sensitivity to
69  /// position state discontinuity when the MaliputRailcar "jumps" from its
70  /// current lane to a lane in an ongoing branch. A smaller value results in a
71  /// smaller spatial discontinuity. If this value is zero, the spatial
72  /// discontinuity will be zero. However, it will trigger the use of
73  /// kTimeEpsilon, which results in a temporal discontinuity.
74  static constexpr double kLaneEndEpsilon{1e-12};
75 
76  /// Defines a time interval that is used to ensure a desired update time is
77  /// always greater than (i.e., after) the current time. Despite the spatial
78  /// window provided by kLaneEndEpsilon, it is still possible for the vehicle
79  /// to end up precisely at the end of its current lane (e.g., it could be
80  /// initialized in this state). In this scenario, the next update time will be
81  /// equal to the current time. The integrator, however, requires that the next
82  /// update time be strictly after the current time, which is when this
83  /// constant is used. The primary constraint on the selection of this constant
84  /// is the application's sensitivity to a MaliputRailcar being "late" in its
85  /// transition to an ongoing branch once it is at the end of its current lane.
86  /// The smaller this value, the less "late" the transition will occur. This
87  /// value cannot be zero since that will violate the integrator's need for the
88  /// next update time to be strictly after the current time.
89  static constexpr double kTimeEpsilon{1e-12};
90 
91  /// The constructor.
92  ///
93  /// @param initial_lane_direction The initial lane and direction of travel.
94  ///
95  explicit MaliputRailcar(const LaneDirection& initial_lane_direction);
96 
97  /// Returns a mutable reference to the parameters in the given @p context.
98  MaliputRailcarParams<T>& get_mutable_parameters(
99  systems::Context<T>* context) const;
100 
101  /// Getter methods for input and output ports.
102  /// @{
103  const systems::InputPort<T>& command_input() const;
104  const systems::OutputPort<T>& state_output() const;
106  const systems::OutputPort<T>& pose_output() const;
108  /// @}
109 
110  static constexpr T kDefaultInitialS = T(0);
111  static constexpr T kDefaultInitialSpeed = T(1);
112 
113  private:
114  // System<T> overrides.
115  void DoCalcTimeDerivatives(
116  const systems::Context<T>& context,
117  systems::ContinuousState<T>* derivatives) const override;
118 
119  void SetDefaultState(const systems::Context<T>& context,
120  systems::State<T>* state) const override;
121 
122  /// Sets `railcar_state` to contain the default state for MaliputRailcar.
123  static void SetDefaultState(MaliputRailcarState<T>* railcar_state);
124 
125  // LeafSystem<T> overrides.
126  std::unique_ptr<systems::AbstractValues> AllocateAbstractState()
127  const override;
128  optional<bool> DoHasDirectFeedthrough(int, int) const override;
129  void DoCalcNextUpdateTime(const systems::Context<T>& context,
131  T* time) const override;
132  void DoCalcUnrestrictedUpdate(
133  const systems::Context<T>& context,
135  systems::State<T>* state) const override;
136 
137  void CalcStateOutput(
138  const systems::Context<T>& context,
139  MaliputRailcarState<T>* output) const;
140 
141  void CalcLaneOutput(
142  const systems::Context<T>& context,
143  LaneDirection* output) const;
144 
145  void CalcPose(
146  const systems::Context<T>& context,
148 
149  void CalcVelocity(
150  const systems::Context<T>& context,
152 
153  void ImplCalcTimeDerivatives(
154  const MaliputRailcarParams<T>& params,
155  const MaliputRailcarState<T>& state,
156  const LaneDirection& lane_direction,
157  const systems::BasicVector<T>& input,
158  MaliputRailcarState<T>* rates) const;
159 
160  void ImplCalcTimeDerivativesDouble(
161  const MaliputRailcarParams<double>& params,
162  const MaliputRailcarState<double>& state,
163  MaliputRailcarState<double>* rates) const;
164 
165  // Calculates the vehicle's `r` coordinate based on whether it's traveling
166  // with or against `s` in the current lane relative to the initial lane.
167  T CalcR(const MaliputRailcarParams<T>& params,
168  const LaneDirection& lane_direction) const;
169 
170  // Finds our parameters in a context.
171  const MaliputRailcarParams<T>& get_parameters(
172  const systems::Context<T>& context) const;
173 
174  const LaneDirection initial_lane_direction_{};
175  int command_input_port_index_{};
176  int state_output_port_index_{};
177  int lane_state_output_port_index_{};
178  int pose_output_port_index_{};
179  int velocity_output_port_index_{};
180 };
181 
182 } // namespace automotive
183 } // namespace drake
static constexpr T kDefaultInitialSpeed
Definition: maliput_railcar.h:111
static constexpr double kTimeEpsilon
Defines a time interval that is used to ensure a desired update time is always greater than (i...
Definition: maliput_railcar.h:89
const systems::OutputPort< T > & state_output() const
Definition: maliput_railcar.cc:107
static constexpr double kLaneEndEpsilon
Defines a distance that is "close enough" to the end of a lane for the vehicle to transition to an on...
Definition: maliput_railcar.h:74
const systems::InputPort< T > & command_input() const
Getter methods for input and output ports.
Definition: maliput_railcar.cc:102
Definition: automotive_demo.cc:90
Context is an abstract class template that represents all the typed values that are used in a System&#39;...
Definition: context.h:41
MaliputRailcar models a vehicle that follows a maliput::api::Lane as if it were on rails and neglecti...
Definition: maliput_railcar.h:63
This class represents an unrestricted update event.
Definition: event.h:482
MaliputRailcarParams< T > & get_mutable_parameters(systems::Context< T > *context) const
Returns a mutable reference to the parameters in the given context.
Definition: maliput_railcar.cc:127
std::vector< double > vector
Definition: translator_test.cc:20
stx::optional< T > optional
Definition: drake_optional.h:22
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:71
static constexpr T kDefaultInitialS
Definition: maliput_railcar.h:110
const systems::OutputPort< T > & pose_output() const
Definition: maliput_railcar.cc:117
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::OutputPort< T > & velocity_output() const
Definition: maliput_railcar.cc:122
const systems::OutputPort< T > & lane_state_output() const
Definition: maliput_railcar.cc:112
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:84
LaneDirection holds the lane that a MaliputRailcar is traversing and the direction in which it is mov...
Definition: lane_direction.h:13
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
This class bundles an instance of each EventCollection<EventType> into one object that stores the het...
Definition: event.h:27
const double time
Definition: robot_plan_interpolator_test.cc:64
MaliputRailcar(const MaliputRailcar &)=delete
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
ContinuousState is a view of, and optionally a container for, all the continuous state variables xc o...
Definition: continuous_state.h:76
#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
This extends InputPortBase with some scalar type-dependent methods.
Definition: input_port.h:20
Provides careful macros to selectively enable or disable the special member functions for copy-constr...