Drake
automotive_simulator.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <map>
4 #include <memory>
5 #include <string>
6 #include <utility>
7 #include <vector>
8 
11 #include "drake/automotive/gen/maliput_railcar_state.h"
12 #include "drake/automotive/gen/trajectory_car_state.h"
23 #include "drake/lcmt_viewer_load_robot.hpp"
32 
33 namespace drake {
34 namespace automotive {
35 
36 /// AutomotiveSimulator is a helper class for constructing and running
37 /// automotive-related simulations.
38 ///
39 /// @tparam T must be a valid Eigen ScalarType.
40 ///
41 /// Instantiated templates for the following ScalarTypes are provided:
42 /// - double
43 ///
44 /// They are already available to link against in the containing library.
45 template <typename T>
47  public:
49 
50  /// A constructor that configures this object to use DrakeLcm, which
51  /// encapsulates a _real_ LCM instance.
53  explicit AutomotiveSimulator(std::unique_ptr<lcm::DrakeLcmInterface> lcm);
55 
56  /// Returns the LCM object used by this AutomotiveSimulator.
58 
59  /// Returns the DiagramBuilder.
60  /// @pre Start() has NOT been called.
62 
63  /// Adds a SimpleCar to this simulation visualized as a Toyota Prius. This
64  /// includes its DrivingCommand LCM input.
65  ///
66  /// @pre Start() has NOT been called.
67  ///
68  /// @param name The car's name, which must be unique among all cars. Otherwise
69  /// a std::runtime_error will be thrown.
70  ///
71  /// @param channel_name The SimpleCar will subscribe to an LCM channel of
72  /// this name to receive commands. It must be non-empty.
73  ///
74  /// @param initial_state The SimpleCar's initial state.
75  ///
76  /// @return The ID of the car that was just added to the simulation.
78  const std::string& name, const std::string& channel_name,
79  const SimpleCarState<T>& initial_state = SimpleCarState<T>());
80 
81  /// Adds a SimpleCar to this simulation controlled by a MOBIL planner coupled
82  /// with a PurePursuitController to perform lateral control of the vehicle,
83  /// along with an IDM longitudinal controller. The car is visualized as a
84  /// Toyota Prius.
85  ///
86  /// @pre Start() has NOT been called.
87  ///
88  /// @pre SetRoadGeometry() was called. Otherwise, a std::runtime_error will be
89  /// thrown.
90  ///
91  /// @param name The car's name, which must be unique among all cars.
92  /// Otherwise a std::runtime_error will be thrown.
93  ///
94  /// @param initial_with_s Initial travel direction in the lane. (See
95  /// MobilPlanner documentation.)
96  ///
97  /// @param initial_state The SimpleCar's initial state.
98  ///
99  /// @return The ID of the car that was just added to the simulation.
101  const std::string& name, bool initial_with_s,
102  const SimpleCarState<T>& initial_state = SimpleCarState<T>());
103 
104  /// Adds a TrajectoryCar to this simulation visualized as a Toyota Prius.
105  ///
106  /// @pre Start() has NOT been called.
107  ///
108  /// @param name The car's name, which must be unique among all cars. Otherwise
109  /// a std::runtime_error will be thrown.
110  ///
111  /// @param curve See documentation of TrajectoryCar::TrajectoryCar.
112  ///
113  /// @param speed See documentation of TrajectoryCar::TrajectoryCar.
114  ///
115  /// @param start_time See documentation of TrajectoryCar::TrajectoryCar.
116  ///
117  /// @return The ID of the car that was just added to the simulation.
118  int AddPriusTrajectoryCar(const std::string& name,
119  const Curve2<double>& curve, double speed,
120  double start_time);
121 
122  /// Adds a MaliputRailcar to this simulation visualized as a Toyota Prius.
123  ///
124  /// @pre Start() has NOT been called.
125  ///
126  /// @pre SetRoadGeometry() was called. Otherwise, a std::runtime_error will be
127  /// thrown.
128  ///
129  /// @param name The car's name, which must be unique among all cars. Otherwise
130  /// a std::runtime_error will be thrown.
131  ///
132  /// @param initial_lane_direction The MaliputRailcar's initial lane and
133  /// direction on the lane. The lane in this parameter must be part of the
134  /// maliput::api::RoadGeometry that is added via SetRoadGeometry(). Otherwise
135  /// a std::runtime_error will be thrown.
136  ///
137  /// @param params The MaliputRailcar's parameters. This is an optional
138  /// parameter. Defaults are used if this parameter is not provided.
139  ///
140  /// @param initial_state The MaliputRailcar's initial state. This is an
141  /// optional parameter. Defaults are used if this parameter is not provided.
142  ///
143  /// @return The ID of the car that was just added to the simulation.
145  const std::string& name, const LaneDirection& initial_lane_direction,
146  const MaliputRailcarParams<T>& params = MaliputRailcarParams<T>(),
147  const MaliputRailcarState<T>& initial_state = MaliputRailcarState<T>());
148 
149  /// Adds a MaliputRailcar to this simulation visualized as a Toyota Prius that
150  /// is controlled via an IdmController.
151  ///
152  /// @pre Start() has NOT been called.
153  ///
154  /// @pre SetRoadGeometry() was called. Otherwise, a std::runtime_error will be
155  /// thrown.
156  ///
157  /// @param name The car's name, which must be unique among all cars. Otherwise
158  /// a std::runtime_error will be thrown.
159  ///
160  /// @param initial_lane_direction The MaliputRailcar's initial lane and
161  /// direction on the lane. The lane in this parameter must be part of the
162  /// maliput::api::RoadGeometry that is added via SetRoadGeometry(). Otherwise
163  /// a std::runtime_error will be thrown.
164  ///
165  /// @param params The MaliputRailcar's parameters. This is an optional
166  /// parameter. Defaults are used if this parameter is not provided.
167  ///
168  /// @param initial_state The MaliputRailcar's initial state. This is an
169  /// optional parameter. Defaults are used if this parameter is not provided.
170  ///
171  /// @return The ID of the car that was just added to the simulation.
173  const std::string& name, const LaneDirection& initial_lane_direction,
174  const MaliputRailcarParams<T>& params = MaliputRailcarParams<T>(),
175  const MaliputRailcarState<T>& initial_state = MaliputRailcarState<T>());
176 
177  /// Sets the acceleration command of a particular MaliputRailcar.
178  ///
179  /// @param id The ID of the MaliputRailcar. This is the ID that was returned
180  /// by the method that added the MaliputRailcar to the simulation. If no
181  /// MaliputRailcar with such an ID exists, a std::runtime_error is thrown.
182  ///
183  /// @param acceleration The acceleration command to issue to the
184  /// MaliputRailcar.
185  ///
186  /// @pre Start() has been called.
188 
189  /// Sets the RoadGeometry for this simulation.
190  ///
191  /// @pre Start() has NOT been called.
193  std::unique_ptr<const maliput::api::RoadGeometry> road);
194 
195  /// Finds and returns a pointer to a lane with the specified name. This method
196  /// throws a std::runtime_error if no such lane exists.
197  ///
198  /// @pre SetRoadGeometry() was called.
199  ///
200  const maliput::api::Lane* FindLane(const std::string& name) const;
201 
202  /// Returns the System whose name matches @p name. Throws an exception if no
203  /// such system has been added, or multiple such systems have been added.
204  //
205  /// This is the builder variant of the method. It can only be used prior to
206  /// Start() being called.
207  ///
208  /// @pre Start() has NOT been called.
209  systems::System<T>& GetBuilderSystemByName(std::string name);
210 
211  /// Returns the System whose name matches @p name. Throws an exception if no
212  /// such system has been added, or multiple such systems have been added.
213  ///
214  /// This is the diagram variant of the method, which can only be used after
215  /// Start() is called.
216  ///
217  /// @pre Start() has been called.
218  const systems::System<T>& GetDiagramSystemByName(std::string name) const;
219 
220  /// Builds the Diagram. No further changes to the diagram may occur after
221  /// this has been called.
222  ///
223  /// @pre Build() has NOT been called.
224  void Build();
225 
226  /// Returns the System containing the entire AutomotiveSimulator diagram.
227  ///
228  /// @pre Build() has been called.
230  DRAKE_DEMAND(diagram_ != nullptr);
231  return *diagram_;
232  }
233 
234  /// Calls Build() on the diagram (if it has not been build already) and
235  /// initializes the Simulator. No further changes to the diagram may occur
236  /// after this has been called.
237  ///
238  /// @pre Start() has NOT been called.
239  ///
240  /// @param target_realtime_rate This value is passed to
241  /// systems::Simulator::set_target_realtime_rate().
242  //
243  // TODO(jwnimmer-tri) Perhaps our class should be AutomotiveSimulatorBuilder?
244  // Port a few more demo programs, then decide what looks best.
245  void Start(double target_realtime_rate = 0.0);
246 
247  /// Returns whether the automotive simulator has started.
248  bool has_started() const { return simulator_ != nullptr; }
249 
250  /// Advances simulated time by the given @p time_step increment in seconds.
251  void StepBy(const T& time_step);
252 
253  /// Returns the current poses of all vehicles in the simulation.
254  ///
255  /// @pre Start() has been called.
257 
258  private:
259  int allocate_vehicle_number();
260 
261  // Verifies that the provided `name` of a car is unique among all cars that
262  // have been added to the `AutomotiveSimulator`. Throws a std::runtime_error
263  // if it is not unique meaning a car of the same name was already added.
264  void CheckNameUniqueness(const std::string& name);
265 
266  // Connects the provided pose and velocity output ports of a vehicle model to
267  // the PoseAggregator and adds a PriusVis for visualizing the vehicle.
268  void ConnectCarOutputsAndPriusVis(int id,
269  const systems::OutputPort<T>& pose_output,
270  const systems::OutputPort<T>& velocity_output);
271 
272  // Adds an LCM publisher for the given @p system.
273  // @pre Start() has NOT been called.
274  void AddPublisher(const MaliputRailcar<T>& system, int vehicle_number);
275 
276  // Adds an LCM publisher for the given @p system.
277  // @pre Start() has NOT been called.
278  void AddPublisher(const SimpleCar<T>& system, int vehicle_number);
279 
280  // Adds an LCM publisher for the given @p system.
281  // @pre Start() has NOT been called.
282  void AddPublisher(const TrajectoryCar<T>& system, int vehicle_number);
283 
284  // Generates the URDF model of the road network and loads it into the
285  // `RigidBodyTree`. Member variable `road_` must be set prior to calling this
286  // method.
287  void GenerateAndLoadRoadNetworkUrdf();
288 
289  // Creates a lcmt_load_robot message containing all visual elements in the
290  // simulation and sends it to the drake-visualizer.
291  void TransmitLoadMessage();
292 
293  void SendLoadRobotMessage(const lcmt_viewer_load_robot& message);
294 
295  void InitializeTrajectoryCars();
296  void InitializeSimpleCars();
297  void InitializeMaliputRailcars();
298 
299  // For both building and simulation.
300  std::unique_ptr<lcm::DrakeLcmInterface> lcm_{};
301  std::unique_ptr<const maliput::api::RoadGeometry> road_{};
302 
303  // === Start for building. ===
304  std::unique_ptr<RigidBodyTree<T>> tree_{std::make_unique<RigidBodyTree<T>>()};
305 
306  std::unique_ptr<systems::DiagramBuilder<T>> builder_{
307  std::make_unique<systems::DiagramBuilder<T>>()};
308 
309  // Holds the desired initial states of each TrajectoryCar. It is used to
310  // initialize the simulation's diagram's state.
311  std::map<const TrajectoryCar<T>*, TrajectoryCarState<T>>
312  trajectory_car_initial_states_;
313 
314  // Holds the desired initial states of each SimpleCar. It is used to
315  // initialize the simulation's diagram's state.
316  std::map<const SimpleCar<T>*, SimpleCarState<T>> simple_car_initial_states_;
317 
318  // Holds the desired initial states of each MaliputRailcar. It is used to
319  // initialize the simulation's diagram's state.
320  std::map<const MaliputRailcar<T>*,
321  std::pair<MaliputRailcarParams<T>, MaliputRailcarState<T>>>
322  railcar_configs_;
323 
324  // The output port of the Diagram that contains pose bundle information.
325  int pose_bundle_output_port_{};
326 
327  // === End for building. ===
328 
329  // Adds the PoseAggregator.
331 
332  // Takes the poses of the vehicles and outputs the poses of the visual
333  // elements that make up the visualization of the vehicles. For a system-level
334  // architecture diagram, see #5541.
335  CarVisApplicator<T>* car_vis_applicator_{};
336 
337  // Takes the output of car_vis_applicator_ and creates an lcmt_viewer_draw
338  // message containing the latest poses of the visual elements.
340 
341  // Takes the output of bundle_to_draw_ and passes it to lcm_ for publishing.
342  systems::lcm::LcmPublisherSystem* lcm_publisher_{};
343 
344  int next_vehicle_number_{0};
345 
346  // Maps a vehicle id to a pointer to the system that implements the vehicle.
347  std::map<int, systems::System<T>*> vehicles_;
348 
349  // For simulation.
350  std::unique_ptr<systems::Diagram<T>> diagram_{};
351  std::unique_ptr<systems::Simulator<T>> simulator_{};
352 };
353 
354 } // namespace automotive
355 } // namespace drake
AutomotiveSimulator is a helper class for constructing and running automotive-related simulations...
Definition: automotive_simulator.h:46
void Start(double target_realtime_rate=0.0)
Calls Build() on the diagram (if it has not been build already) and initializes the Simulator...
Definition: automotive_simulator.cc:483
CarVisApplicator takes as input a PoseVector containing vehicle poses.
Definition: car_vis_applicator.h:42
PoseBundleToDrawMessage converts a PoseBundle on its single abstract-valued input port to a Drake Vis...
Definition: pose_bundle_to_draw_message.h:22
Definition: automotive_demo.cc:88
Curve2 represents a path through two-dimensional Cartesian space.
Definition: curve2.h:31
void Build()
Builds the Diagram.
Definition: automotive_simulator.cc:460
int AddPriusTrajectoryCar(const std::string &name, const Curve2< double > &curve, double speed, double start_time)
Adds a TrajectoryCar to this simulation visualized as a Toyota Prius.
Definition: automotive_simulator.cc:199
PoseBundle is a container for a set of poses, represented by an Isometry3, and corresponding velociti...
Definition: pose_bundle.h:40
MaliputRailcar models a vehicle that follows a maliput::api::Lane as if it were on rails and neglecti...
Definition: maliput_railcar.h:63
~AutomotiveSimulator()
Definition: automotive_simulator.cc:72
Abstract API for the geometry of a road network, including both the network topology and the geometry...
Definition: road_geometry.h:26
AutomotiveSimulator()
A constructor that configures this object to use DrakeLcm, which encapsulates a real LCM instance...
Definition: automotive_simulator.cc:50
int AddIdmControlledPriusMaliputRailcar(const std::string &name, const LaneDirection &initial_lane_direction, const MaliputRailcarParams< T > &params=MaliputRailcarParams< T >(), const MaliputRailcarState< T > &initial_state=MaliputRailcarState< T >())
Adds a MaliputRailcar to this simulation visualized as a Toyota Prius that is controlled via an IdmCo...
Definition: automotive_simulator.cc:267
PoseAggregator is a multiplexer for heterogeneous sources of poses and the velocities of those poses...
Definition: pose_aggregator.h:72
const maliput::api::RoadGeometry * SetRoadGeometry(std::unique_ptr< const maliput::api::RoadGeometry > road)
Sets the RoadGeometry for this simulation.
Definition: automotive_simulator.cc:317
A Lane represents a lane of travel in a road network.
Definition: lane.h:34
DiagramBuilder is a factory class for Diagram.
Definition: diagram.h:33
Publishes an LCM message containing information from its input port.
Definition: lcm_publisher_system.h:23
const maliput::api::Lane * FindLane(const std::string &name) const
Finds and returns a pointer to a lane with the specified name.
Definition: automotive_simulator.cc:326
double acceleration
Definition: system_identification_test.cc:205
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:67
systems::rendering::PoseBundle< T > GetCurrentPoses() const
Returns the current poses of all vehicles in the simulation.
Definition: automotive_simulator.cc:588
A pure virtual interface that enables LCM to be mocked.
Definition: drake_lcm_interface.h:15
int AddPriusMaliputRailcar(const std::string &name, const LaneDirection &initial_lane_direction, const MaliputRailcarParams< T > &params=MaliputRailcarParams< T >(), const MaliputRailcarState< T > &initial_state=MaliputRailcarState< T >())
Adds a MaliputRailcar to this simulation visualized as a Toyota Prius.
Definition: automotive_simulator.cc:228
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
const systems::System< T > & GetDiagramSystemByName(std::string name) const
Returns the System whose name matches name.
Definition: automotive_simulator.cc:416
bool has_started() const
Returns whether the automotive simulator has started.
Definition: automotive_simulator.h:248
LaneDirection holds the lane that a MaliputRailcar is traversing and the direction in which it is mov...
Definition: lane_direction.h:13
SimpleCar models an idealized response to driving commands, neglecting all physics.
Definition: simple_car.h:52
A superclass template for systems that receive input, maintain state, and produce output of a given m...
Definition: input_port_descriptor.h:12
int AddPriusSimpleCar(const std::string &name, const std::string &channel_name, const SimpleCarState< T > &initial_state=SimpleCarState< T >())
Adds a SimpleCar to this simulation visualized as a Toyota Prius.
Definition: automotive_simulator.cc:107
systems::DiagramBuilder< T > * get_builder()
Returns the DiagramBuilder.
Definition: automotive_simulator.cc:84
systems::System< T > & GetBuilderSystemByName(std::string name)
Returns the System whose name matches name.
Definition: automotive_simulator.cc:401
int AddMobilControlledSimpleCar(const std::string &name, bool initial_with_s, const SimpleCarState< T > &initial_state=SimpleCarState< T >())
Adds a SimpleCar to this simulation controlled by a MOBIL planner coupled with a PurePursuitControlle...
Definition: automotive_simulator.cc:136
void SetMaliputRailcarAccelerationCommand(int id, double acceleration)
Sets the acceleration command of a particular MaliputRailcar.
Definition: automotive_simulator.cc:292
#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
TrajectoryCar models a car that follows a pre-established trajectory.
Definition: trajectory_car.h:62
lcm::DrakeLcmInterface * get_lcm()
Returns the LCM object used by this AutomotiveSimulator.
Definition: automotive_simulator.cc:79
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
void StepBy(const T &time_step)
Advances simulated time by the given time_step increment in seconds.
Definition: automotive_simulator.cc:565
const systems::System< T > & GetDiagram() const
Returns the System containing the entire AutomotiveSimulator diagram.
Definition: automotive_simulator.h:229