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>
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  // System<T> overrides.
99  const systems::Context<T>& context,
100  systems::ContinuousState<T>* derivatives) const override;
101 
102  void SetDefaultState(const systems::Context<T>& context,
103  systems::State<T>* state) const override;
104 
105  /// Sets `railcar_state` to contain the default state for MaliputRailcar.
106  static void SetDefaultState(MaliputRailcarState<T>* railcar_state);
107 
108  /// Returns a mutable pointer to the parameters in the given @p context.
109  MaliputRailcarParams<T>& get_mutable_parameters(
110  systems::Context<T>* context) const;
111 
112  /// Getter methods for input and output ports.
113  /// @{
115  const systems::OutputPort<T>& state_output() const;
117  const systems::OutputPort<T>& pose_output() const;
119  /// @}
120 
121  static constexpr T kDefaultInitialS = T(0);
122  static constexpr T kDefaultInitialSpeed = T(1);
123 
124  protected:
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;
133  const systems::Context<T>& context,
135  systems::State<T>* state) const override;
136 
137  private:
138  void CalcStateOutput(
139  const systems::Context<T>& context,
140  MaliputRailcarState<T>* output) const;
141 
142  void CalcLaneOutput(
143  const systems::Context<T>& context,
144  LaneDirection* output) const;
145 
146  void CalcPose(
147  const systems::Context<T>& context,
149 
150  void CalcVelocity(
151  const systems::Context<T>& context,
153 
154  void ImplCalcTimeDerivatives(
155  const MaliputRailcarParams<T>& params,
156  const MaliputRailcarState<T>& state,
157  const LaneDirection& lane_direction,
158  const systems::BasicVector<T>& input,
159  MaliputRailcarState<T>* rates) const;
160 
161  void ImplCalcTimeDerivativesDouble(
162  const MaliputRailcarParams<double>& params,
163  const MaliputRailcarState<double>& state,
164  MaliputRailcarState<double>* rates) const;
165 
166  // Calculates the vehicle's `r` coordinate based on whether it's traveling
167  // with or against `s` in the current lane relative to the initial lane.
168  T CalcR(const MaliputRailcarParams<T>& params,
169  const LaneDirection& lane_direction) const;
170 
171  // Finds our parameters in a context.
172  const MaliputRailcarParams<T>& get_parameters(
173  const systems::Context<T>& context) const;
174 
175  const LaneDirection initial_lane_direction_{};
176  int command_input_port_index_{};
177  int state_output_port_index_{};
178  int lane_state_output_port_index_{};
179  int pose_output_port_index_{};
180  int velocity_output_port_index_{};
181 };
182 
183 } // namespace automotive
184 } // namespace drake
static constexpr T kDefaultInitialSpeed
Definition: maliput_railcar.h:122
void DoCalcTimeDerivatives(const systems::Context< T > &context, systems::ContinuousState< T > *derivatives) const override
Definition: maliput_railcar.cc:231
optional< bool > DoHasDirectFeedthrough(int, int) const override
Returns true if there is direct-feedthrough from the given input_port to the given output_port...
Definition: maliput_railcar.cc:300
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
std::unique_ptr< systems::AbstractValues > AllocateAbstractState() const override
Reserves the abstract state as required by CreateDefaultContext.
Definition: maliput_railcar.cc:291
const systems::OutputPort< T > & state_output() const
Definition: maliput_railcar.cc:108
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
Definition: automotive_demo.cc:88
void DoCalcNextUpdateTime(const systems::Context< T > &context, systems::CompositeEventCollection< T > *, T *time) const override
Definition: maliput_railcar.cc:335
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
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:349
MaliputRailcarParams< T > & get_mutable_parameters(systems::Context< T > *context) const
Returns a mutable pointer to the parameters in the given context.
Definition: maliput_railcar.cc:128
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
stx::optional< T > optional
Definition: drake_optional.h:14
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:67
static constexpr T kDefaultInitialS
Definition: maliput_railcar.h:121
const systems::OutputPort< T > & pose_output() const
Definition: maliput_railcar.cc:118
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:123
const systems::OutputPort< T > & lane_state_output() const
Definition: maliput_railcar.cc:113
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
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:22
const double time
Definition: robot_plan_interpolator_test.cc:64
MaliputRailcar(const MaliputRailcar &)=delete
void DoCalcUnrestrictedUpdate(const systems::Context< T > &context, const std::vector< const systems::UnrestrictedUpdateEvent< T > * > &, systems::State< T > *state) const override
Definition: maliput_railcar.cc:377
void SetDefaultState(const systems::Context< T > &context, systems::State< T > *state) const override
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 container for all the continuous state variables xc.
Definition: continuous_state.h:44
const systems::InputPortDescriptor< T > & command_input() const
Getter methods for input and output ports.
Definition: maliput_railcar.cc:103
#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
Provides careful macros to selectively enable or disable the special member functions for copy-constr...