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 
23 #include "drake/lcmt_viewer_load_robot.hpp"
32 
33 namespace drake {
34 namespace automotive {
35 
45 template <typename T>
47  public:
49 
50 
53  explicit AutomotiveSimulator(std::unique_ptr<lcm::DrakeLcmInterface> lcm);
55 
58 
62 
78  const std::string& name, const std::string& channel_name,
79  const SimpleCarState<T>& initial_state = SimpleCarState<T>());
80 
101  const std::string& name, bool initial_with_s,
102  const SimpleCarState<T>& initial_state = SimpleCarState<T>());
103 
118  int AddPriusTrajectoryCar(const std::string& name,
119  const Curve2<double>& curve, double speed,
120  double start_time);
121 
145  const std::string& name, const LaneDirection& initial_lane_direction,
147  const MaliputRailcarState<T>& initial_state = MaliputRailcarState<T>());
148 
173  const std::string& name, const LaneDirection& initial_lane_direction,
175  const MaliputRailcarState<T>& initial_state = MaliputRailcarState<T>());
176 
188 
193  std::unique_ptr<const maliput::api::RoadGeometry> road);
194 
200  const maliput::api::Lane* FindLane(const std::string& name) const;
201 
204  //
209  systems::System<T>& GetBuilderSystemByName(std::string name);
210 
218  const systems::System<T>& GetDiagramSystemByName(std::string name) const;
219 
224  void Build();
225 
230  DRAKE_DEMAND(diagram_ != nullptr);
231  return *diagram_;
232  }
233 
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 
248  bool has_started() const { return simulator_ != nullptr; }
249 
251  void StepBy(const T& time_step);
252 
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
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:32
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:24
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:589
A pure virtual interface that enables LCM to be mocked.
Definition: drake_lcm_interface.h:14
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:47
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
Specializes BasicVector with specific getters and setters.
Definition: maliput_railcar_state.h:37
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:10
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
Specializes BasicVector with specific getters and setters.
Definition: simple_car_state.h:39
Specializes BasicVector with specific getters and setters.
Definition: maliput_railcar_params.h:39
#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:35
Specializes BasicVector with specific getters and setters.
Definition: trajectory_car_state.h:37
TrajectoryCar models a car that follows a pre-established trajectory.
Definition: trajectory_car.h:63
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:566
const systems::System< T > & GetDiagram() const
Returns the System containing the entire AutomotiveSimulator diagram.
Definition: automotive_simulator.h:229