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  /// The default constructor that configures this object with a default-
51  /// constructed DrakeLcm instance.
53 
54  /// A constructor that configures this object to use a DrakeLcmInterface
55  /// instance. If nullptr, no visualization is produced.
56  explicit AutomotiveSimulator(std::unique_ptr<lcm::DrakeLcmInterface> lcm);
57 
59 
60  /// Returns the LCM object used by this AutomotiveSimulator.
62 
63  /// Returns the DiagramBuilder.
64  /// @pre Start() has NOT been called.
66 
67  /// Adds a SimpleCar to this simulation visualized as a Toyota Prius. This
68  /// includes its DrivingCommand LCM input.
69  ///
70  /// @pre Start() has NOT been called.
71  ///
72  /// @param name The car's name, which must be unique among all cars. Otherwise
73  /// a std::runtime_error will be thrown.
74  ///
75  /// @param channel_name The SimpleCar will subscribe to an LCM channel of this
76  /// name to receive commands. If empty, then the input port is fixed to a
77  /// DrivingCommand created using its default constructor.
78  ///
79  /// @param initial_state The SimpleCar's initial state.
80  ///
81  /// @return The ID of the car that was just added to the simulation.
83  const std::string& name, const std::string& channel_name = std::string(),
84  const SimpleCarState<T>& initial_state = SimpleCarState<T>());
85 
86  /// Adds a SimpleCar to this simulation controlled by a MOBIL planner coupled
87  /// with a PurePursuitController to perform lateral control of the vehicle,
88  /// along with an IDM longitudinal controller. The car is visualized as a
89  /// Toyota Prius.
90  ///
91  /// @pre Start() has NOT been called.
92  ///
93  /// @pre SetRoadGeometry() was called. Otherwise, a std::runtime_error will be
94  /// thrown.
95  ///
96  /// @param name The car's name, which must be unique among all cars.
97  /// Otherwise a std::runtime_error will be thrown.
98  ///
99  /// @param initial_with_s Initial travel direction in the lane. (See
100  /// MobilPlanner documentation.)
101  ///
102  /// @param path_or_branches If ScanStrategy::kBranches, performs IDM
103  /// computations using vehicles detected in confluent branches; if
104  /// ScanStrategy::kPath, limits to vehicles on the default path.
105  ///
106  /// @param road_position_strategy Determines whether or not to memorize
107  /// RoadPosition. See `calc_ongoing_road_position.h`.
108  ///
109  /// @param period_sec The update period to use if road_position_strategy ==
110  /// RoadPositionStrategy::kCache.
111  ///
112  /// @param initial_state The SimpleCar's initial state.
113  ///
114  /// @return The ID of the car that was just added to the simulation.
116  const std::string& name, bool initial_with_s,
117  ScanStrategy path_or_branches,
118  RoadPositionStrategy road_position_strategy, double period_sec,
119  const SimpleCarState<T>& initial_state = SimpleCarState<T>());
120 
121  /// Adds a TrajectoryCar to this simulation visualized as a Toyota Prius.
122  ///
123  /// @pre Start() has NOT been called.
124  ///
125  /// @param name The car's name, which must be unique among all cars. Otherwise
126  /// a std::runtime_error will be thrown.
127  ///
128  /// @param curve See documentation of TrajectoryCar::TrajectoryCar.
129  ///
130  /// @param speed See documentation of TrajectoryCar::TrajectoryCar.
131  ///
132  /// @param start_time See documentation of TrajectoryCar::TrajectoryCar.
133  ///
134  /// @return The ID of the car that was just added to the simulation.
135  int AddPriusTrajectoryCar(const std::string& name,
136  const Curve2<double>& curve, double speed,
137  double start_time);
138 
139  /// Adds a lane-following SimpleCar with IdmController and
140  /// PurePursuitController to this simulation that takes as input a constant
141  /// source that contains the @p goal_lane as the destination lane for the car.
142  /// The car is visualized as a Toyota Prius. This includes its
143  /// EulerFloatingJoint output.
144  ///
145  /// @pre Start() has NOT been called.
146  ///
147  /// @param name The car's name, which must be unique among all cars. Otherwise
148  /// a std::runtime_error will be thrown.
149  ///
150  /// @param initial_with_s Initial travel direction in the lane. True means
151  /// that the car is initially oriented in the positive-s direction.
152  ///
153  /// @param initial_state The SimpleCar's initial state.
154  ///
155  /// @param goal_lane The goal lane for the car. If goal_lane is nullptr or not
156  /// a member of the road supplied via SetRoadGeometry(), a std::runtime_error
157  /// will be thrown.
158  ///
159  /// @param path_or_branches If ScanStrategy::kBranches, performs IDM
160  /// computations using vehicles detected in confluent branches; if
161  /// ScanStrategy::kPath, limits to vehicles on the default path.
162  ///
163  /// @param road_position_strategy Determines whether or not to memorize
164  /// RoadPosition. See `calc_ongoing_road_position.h`.
165  ///
166  /// @param period_sec The update period to use if road_position_strategy ==
167  /// RoadPositionStrategy::kCache.
168  ///
169  /// @return The ID of the car that was just added to the simulation.
170  int AddIdmControlledCar(const std::string& name,
171  bool initial_with_s,
172  const SimpleCarState<T>& initial_state,
173  const maliput::api::Lane* goal_lane,
174  ScanStrategy path_or_branches,
175  RoadPositionStrategy road_position_strategy,
176  double period_sec);
177 
178  /// Adds a MaliputRailcar to this simulation visualized as a Toyota Prius.
179  ///
180  /// @pre Start() has NOT been called.
181  ///
182  /// @pre SetRoadGeometry() was called. Otherwise, a std::runtime_error will be
183  /// thrown.
184  ///
185  /// @param name The car's name, which must be unique among all cars. Otherwise
186  /// a std::runtime_error will be thrown.
187  ///
188  /// @param initial_lane_direction The MaliputRailcar's initial lane and
189  /// direction on the lane. The lane in this parameter must be part of the
190  /// maliput::api::RoadGeometry that is added via SetRoadGeometry(). Otherwise
191  /// a std::runtime_error will be thrown.
192  ///
193  /// @param params The MaliputRailcar's parameters. This is an optional
194  /// parameter. Defaults are used if this parameter is not provided.
195  ///
196  /// @param initial_state The MaliputRailcar's initial state. This is an
197  /// optional parameter. Defaults are used if this parameter is not provided.
198  ///
199  /// @return The ID of the car that was just added to the simulation.
201  const std::string& name, const LaneDirection& initial_lane_direction,
202  const MaliputRailcarParams<T>& params = MaliputRailcarParams<T>(),
203  const MaliputRailcarState<T>& initial_state = MaliputRailcarState<T>());
204 
205  /// Adds a MaliputRailcar to this simulation visualized as a Toyota Prius that
206  /// is controlled via an IdmController.
207  ///
208  /// @pre Start() has NOT been called.
209  ///
210  /// @pre SetRoadGeometry() was called. Otherwise, a std::runtime_error will be
211  /// thrown.
212  ///
213  /// @param name The car's name, which must be unique among all cars. Otherwise
214  /// a std::runtime_error will be thrown.
215  ///
216  /// @param initial_lane_direction The MaliputRailcar's initial lane and
217  /// direction on the lane. The lane in this parameter must be part of the
218  /// maliput::api::RoadGeometry that is added via SetRoadGeometry(). Otherwise
219  /// a std::runtime_error will be thrown.
220  ///
221  /// @param path_or_branches If ScanStrategy::kBranches, performs IDM
222  /// computations using vehicles detected in confluent branches; if
223  /// ScanStrategy::kPath, limits to vehicles on the default path.
224  ///
225  /// @param road_position_strategy Determines whether or not to memorize
226  /// RoadPosition. See `calc_ongoing_road_position.h`.
227  ///
228  /// @param period_sec The update period to use if road_position_strategy ==
229  /// RoadPositionStrategy::kCache.
230  ///
231  /// @param params The MaliputRailcar's parameters. This is an optional
232  /// parameter. Defaults are used if this parameter is not provided.
233  ///
234  /// @param initial_state The MaliputRailcar's initial state. This is an
235  /// optional parameter. Defaults are used if this parameter is not provided.
236  ///
237  /// @return The ID of the car that was just added to the simulation.
239  const std::string& name, const LaneDirection& initial_lane_direction,
240  ScanStrategy path_or_branches,
241  RoadPositionStrategy road_position_strategy, double period_sec,
242  const MaliputRailcarParams<T>& params = MaliputRailcarParams<T>(),
243  const MaliputRailcarState<T>& initial_state = MaliputRailcarState<T>());
244 
245  /// Sets the acceleration command of a particular MaliputRailcar.
246  ///
247  /// @param id The ID of the MaliputRailcar. This is the ID that was returned
248  /// by the method that added the MaliputRailcar to the simulation. If no
249  /// MaliputRailcar with such an ID exists, a std::runtime_error is thrown.
250  ///
251  /// @param acceleration The acceleration command to issue to the
252  /// MaliputRailcar.
253  ///
254  /// @pre Start() has been called.
256 
257  /// Sets the RoadGeometry for this simulation.
258  ///
259  /// @pre Start() has NOT been called.
261  std::unique_ptr<const maliput::api::RoadGeometry> road);
262 
263  /// Finds and returns a pointer to a lane with the specified name. This method
264  /// throws a std::runtime_error if no such lane exists.
265  ///
266  /// @pre SetRoadGeometry() was called.
267  ///
268  const maliput::api::Lane* FindLane(const std::string& name) const;
269 
270  /// Returns the System whose name matches @p name. Throws an exception if no
271  /// such system has been added, or multiple such systems have been added.
272  //
273  /// This is the builder variant of the method. It can only be used prior to
274  /// Start() being called.
275  ///
276  /// @pre Start() has NOT been called.
277  systems::System<T>& GetBuilderSystemByName(std::string name);
278 
279  /// Returns the System whose name matches @p name. Throws an exception if no
280  /// such system has been added, or multiple such systems have been added.
281  ///
282  /// This is the diagram variant of the method, which can only be used after
283  /// Start() is called.
284  ///
285  /// @pre Start() has been called.
286  const systems::System<T>& GetDiagramSystemByName(std::string name) const;
287 
288  /// Builds the Diagram. No further changes to the diagram may occur after
289  /// this has been called.
290  ///
291  /// @pre Build() and BuildandInitialize() have NOT been called.
292  void Build();
293 
294  /// Builds the Diagram and intializes the Diagram Context to the predefined
295  /// initial states.
296  ///
297  /// @pre Build() and BuildandInitialize() have NOT been called.
298  void BuildAndInitialize(
299  std::unique_ptr<systems::Context<double>> initial_context = nullptr);
300 
301  /// Returns the System containing the entire AutomotiveSimulator diagram.
302  ///
303  /// @pre either Build() or BuildandInitialize() have been called.
305  DRAKE_DEMAND(diagram_ != nullptr);
306  return *diagram_;
307  }
308 
309  /// Calls BuildAndInitialize() on the diagram (if it has not been build
310  /// already) and initializes the Simulator. No further changes to the diagram
311  /// may occur after this has been called.
312  ///
313  /// @pre Start() has NOT been called.
314  ///
315  /// @param target_realtime_rate This value is passed to
316  /// systems::Simulator::set_target_realtime_rate().
317  //
318  // TODO(jwnimmer-tri) Perhaps our class should be AutomotiveSimulatorBuilder?
319  // Port a few more demo programs, then decide what looks best.
320  void Start(double target_realtime_rate = 0.0,
321  std::unique_ptr<systems::Context<double>>
322  initial_context = nullptr);
323 
324  /// Returns whether the automotive simulator has started.
325  bool has_started() const { return simulator_ != nullptr; }
326 
327  /// Advances simulated time by the given @p time_step increment in seconds.
328  void StepBy(const T& time_step);
329 
330  /// Returns the current poses of all vehicles in the simulation.
331  ///
332  /// @pre Start() has been called.
334 
335  private:
336  int allocate_vehicle_number();
337 
338  // Verifies that the provided `name` of a car is unique among all cars that
339  // have been added to the `AutomotiveSimulator`. Throws a std::runtime_error
340  // if it is not unique meaning a car of the same name was already added.
341  void CheckNameUniqueness(const std::string& name);
342 
343  // Connects the provided pose and velocity output ports of a vehicle model to
344  // the PoseAggregator and adds a PriusVis for visualizing the vehicle.
345  void ConnectCarOutputsAndPriusVis(int id,
346  const systems::OutputPort<T>& pose_output,
347  const systems::OutputPort<T>& velocity_output);
348 
349  // Adds an LCM publisher for the given @p system.
350  // @pre Start() has NOT been called.
351  void AddPublisher(const MaliputRailcar<T>& system, int vehicle_number);
352 
353  // Adds an LCM publisher for the given @p system.
354  // @pre Start() has NOT been called.
355  void AddPublisher(const SimpleCar<T>& system, int vehicle_number);
356 
357  // Adds an LCM publisher for the given @p system.
358  // @pre Start() has NOT been called.
359  void AddPublisher(const TrajectoryCar<T>& system, int vehicle_number);
360 
361  // Generates the URDF model of the road network and loads it into the
362  // `RigidBodyTree`. Member variable `road_` must be set prior to calling this
363  // method.
364  void GenerateAndLoadRoadNetworkUrdf();
365 
366  // Creates a lcmt_load_robot message containing all visual elements in the
367  // simulation and sends it to the drake-visualizer.
368  void TransmitLoadMessage();
369 
370  void SendLoadRobotMessage(const lcmt_viewer_load_robot& message);
371 
372  void InitializeTrajectoryCars();
373  void InitializeSimpleCars();
374  void InitializeMaliputRailcars();
375 
376  // For both building and simulation.
377  std::unique_ptr<lcm::DrakeLcmInterface> lcm_{};
378  std::unique_ptr<const maliput::api::RoadGeometry> road_{};
379 
380  // === Start for building. ===
381  std::unique_ptr<RigidBodyTree<T>> tree_{std::make_unique<RigidBodyTree<T>>()};
382 
383  std::unique_ptr<systems::DiagramBuilder<T>> builder_{
384  std::make_unique<systems::DiagramBuilder<T>>()};
385 
386  // Holds the desired initial states of each TrajectoryCar. It is used to
387  // initialize the simulation's diagram's state.
388  std::map<const TrajectoryCar<T>*, TrajectoryCarState<T>>
389  trajectory_car_initial_states_;
390 
391  // Holds the desired initial states of each SimpleCar. It is used to
392  // initialize the simulation's diagram's state.
393  std::map<const SimpleCar<T>*, SimpleCarState<T>> simple_car_initial_states_;
394 
395  // Holds the desired initial states of each MaliputRailcar. It is used to
396  // initialize the simulation's diagram's state.
397  std::map<const MaliputRailcar<T>*,
398  std::pair<MaliputRailcarParams<T>, MaliputRailcarState<T>>>
399  railcar_configs_;
400 
401  // The output port of the Diagram that contains pose bundle information.
402  int pose_bundle_output_port_{};
403 
404  // === End for building. ===
405 
406  // Adds the PoseAggregator.
408 
409  // Takes the poses of the vehicles and outputs the poses of the visual
410  // elements that make up the visualization of the vehicles. For a system-level
411  // architecture diagram, see #5541.
412  CarVisApplicator<T>* car_vis_applicator_{};
413 
414  // Takes the output of car_vis_applicator_ and creates an lcmt_viewer_draw
415  // message containing the latest poses of the visual elements.
417 
418  // Takes the output of bundle_to_draw_ and passes it to lcm_ for publishing.
419  systems::lcm::LcmPublisherSystem* lcm_publisher_{};
420 
421  int next_vehicle_number_{0};
422 
423  // Maps a vehicle id to a pointer to the system that implements the vehicle.
424  std::map<int, systems::System<T>*> vehicles_;
425 
426  // For simulation.
427  std::unique_ptr<systems::Diagram<T>> diagram_{};
428  std::unique_ptr<systems::Simulator<T>> simulator_{};
429 };
430 
431 } // namespace automotive
432 } // namespace drake
AutomotiveSimulator is a helper class for constructing and running automotive-related simulations...
Definition: automotive_simulator.h:46
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
int AddMobilControlledSimpleCar(const std::string &name, bool initial_with_s, ScanStrategy path_or_branches, RoadPositionStrategy road_position_strategy, double period_sec, 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:144
Definition: automotive_demo.cc:90
Curve2 represents a path through two-dimensional Cartesian space.
Definition: curve2.h:31
void Build()
Builds the Diagram.
Definition: automotive_simulator.cc:551
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:214
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:76
Abstract API for the geometry of a road network, including both the network topology and the geometry...
Definition: road_geometry.h:30
AutomotiveSimulator()
The default constructor that configures this object with a default- constructed DrakeLcm instance...
Definition: automotive_simulator.cc:52
PoseAggregator is a multiplexer for heterogeneous sources of poses and the velocities of those poses...
Definition: pose_aggregator.h:81
const maliput::api::RoadGeometry * SetRoadGeometry(std::unique_ptr< const maliput::api::RoadGeometry > road)
Sets the RoadGeometry for this simulation.
Definition: automotive_simulator.cc:408
int AddIdmControlledCar(const std::string &name, bool initial_with_s, const SimpleCarState< T > &initial_state, const maliput::api::Lane *goal_lane, ScanStrategy path_or_branches, RoadPositionStrategy road_position_strategy, double period_sec)
Adds a lane-following SimpleCar with IdmController and PurePursuitController to this simulation that ...
Definition: automotive_simulator.cc:245
A Lane represents a lane of travel in a road network.
Definition: lane.h:35
DiagramBuilder is a factory class for Diagram.
Definition: diagram.h:34
Publishes an LCM message containing information from its input port.
Definition: lcm_publisher_system.h:25
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:417
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:71
RoadPositionStrategy
If kCache, configures a planning system (e.g.
Definition: pose_selector.h:52
systems::rendering::PoseBundle< T > GetCurrentPoses() const
Returns the current poses of all vehicles in the simulation.
Definition: automotive_simulator.cc:694
A pure virtual interface that enables LCM to be mocked.
Definition: drake_lcm_interface.h:22
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:315
#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:511
bool has_started() const
Returns whether the automotive simulator has started.
Definition: automotive_simulator.h:325
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
Base class for all System functionality that is dependent on the templatized scalar type T for input...
Definition: input_port.h:13
systems::DiagramBuilder< T > * get_builder()
Returns the DiagramBuilder.
Definition: automotive_simulator.cc:88
systems::System< T > & GetBuilderSystemByName(std::string name)
Returns the System whose name matches name.
Definition: automotive_simulator.cc:496
int AddIdmControlledPriusMaliputRailcar(const std::string &name, const LaneDirection &initial_lane_direction, ScanStrategy path_or_branches, RoadPositionStrategy road_position_strategy, double period_sec, 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:354
void Start(double target_realtime_rate=0.0, std::unique_ptr< systems::Context< double >> initial_context=nullptr)
Calls BuildAndInitialize() on the diagram (if it has not been build already) and initializes the Simu...
Definition: automotive_simulator.cc:594
void BuildAndInitialize(std::unique_ptr< systems::Context< double >> initial_context=nullptr)
Builds the Diagram and intializes the Diagram Context to the predefined initial states.
Definition: automotive_simulator.cc:576
void SetMaliputRailcarAccelerationCommand(int id, double acceleration)
Sets the acceleration command of a particular MaliputRailcar.
Definition: automotive_simulator.cc:383
int AddPriusSimpleCar(const std::string &name, const std::string &channel_name=std::string(), const SimpleCarState< T > &initial_state=SimpleCarState< T >())
Adds a SimpleCar to this simulation visualized as a Toyota Prius.
Definition: automotive_simulator.cc:113
#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
ScanStrategy
Specifies whether to check ongoing lanes or both ongoing lanes and confluent branches for traffic...
Definition: pose_selector.h:44
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:83
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:671
const systems::System< T > & GetDiagram() const
Returns the System containing the entire AutomotiveSimulator diagram.
Definition: automotive_simulator.h:304