Add an python example to change speeds

python/delphyne/agents.cc

@@ -7,6 +7,7 @@
 ** Includes
 *****************************************************************************/
 
+#include <string>
 #include <vector>
 
 #include <pybind11/functional.h>
@@ -53,7 +54,8 @@ PYBIND11_MODULE(agents, m) {
            py::arg("lane"), py::arg("direction_of_travel"),
            py::arg("longitudinal_position"), py::arg("lateral_offset"),
            py::arg("speed"), py::arg("nominal_speed"),
-           py::arg("road_geometry"));
+           py::arg("road_geometry"))
+      .def("set_speed", &delphyne::RailCar::SetSpeed);
 
   py::class_<delphyne::SimpleCar, delphyne::Agent>(m, "SimpleCar")
       .def(py::init<const std::string&, double, double, double, double>(),

python/delphyne/demos/crash.py

@@ -56,12 +56,6 @@ def add_collision_callback(_, method):
     def _method_wrapper(self, callback):
         @wraps(callback)
         def _callback_wrapper(agents_in_collision):
-            simulator = self.get_simulator()
-            agents_in_collision = [
-                (simulator.get_mutable_agent_by_id(agent1_id),
-                 simulator.get_mutable_agent_by_id(agent2_id))
-                for agent1_id, agent2_id in agents_in_collision
-            ]
             callback(self, agents_in_collision)
         return method(self, _callback_wrapper)
     return _method_wrapper

python/delphyne/demos/scriptlets.py

@@ -18,9 +18,12 @@
 
 from __future__ import print_function
 
+import os.path
+
 import random
 import time
 
+import delphyne.maliput as maliput
 import delphyne.simulation as simulation
 import delphyne.utilities as utilities
 
@@ -89,6 +92,29 @@ def parse_arguments():
     )
     return parser.parse_args()
 
+
+class TimeMonitor(object):
+    '''
+    A class to monitor the time on every callback and perform an action after
+    ten seconds have elapsed.
+    '''
+    def __init__(self, simulator, agent):
+        self.simulator = simulator
+        self.agent = agent
+        self.changed_speed = False
+
+    def check_tick(self):
+        '''
+        The callback called on every simulator iteration.  It checks
+        to see if the elapsed simulator_time is greater than 10 seconds, and
+        once it is, it changes the speed of the agent.
+        '''
+        if self.simulator.get_current_simulation_time() >= 10.0 \
+           and not self.changed_speed:
+            self.agent.set_speed(20.0)
+            self.changed_speed = True
+
+
 ##############################################################################
 # Main
 ##############################################################################
@@ -101,26 +127,64 @@ def main():
     stats = SimulationStats()
 
     simulator = simulation.AutomotiveSimulator()
+
+    filename = "{0}/roads/circuit.yaml".format(
+        utilities.get_delphyne_resource_root())
+
+    if not os.path.isfile(filename):
+        print("Required file {} not found."
+              " Please, make sure to install the latest delphyne-gui."
+              .format(os.path.abspath(filename)))
+        quit()
+
+    # The road geometry
+    road_geometry = simulator.set_road_geometry(
+        maliput.create_multilane_from_file(
+            file_path=filename
+        )
+    )
+
     utilities.add_simple_car(
         simulator,
         name=str(0),
         position_x=0.0,
         position_y=0.0)
 
+    # Setup railcar
+    railcar_speed = 4.0  # (m/s)
+    railcar_s = 0.0      # (m)
+    robot_id = 1
+    lane_1 = road_geometry.junction(2).segment(0).lane(0)
+    myagent = utilities.add_rail_car(
+        simulator,
+        name=str(robot_id),
+        lane=lane_1,
+        position=railcar_s,
+        offset=0.0,
+        speed=railcar_speed,
+        road_geometry=road_geometry)
+
     runner = simulation.SimulatorRunner(
         simulator=simulator,
         time_step=0.001,  # (secs)
         realtime_rate=args.realtime_rate,
-        paused=args.paused
+        paused=args.paused,
+        log=args.log,
+        logfile_name=args.logfile_name
     )
 
+    monitor = TimeMonitor(simulator, myagent)
+
     with utilities.launch_interactive_simulation(runner) as launcher:
         # Add a callback to record and print statistics
         runner.add_step_callback(stats.record_tick)
 
         # Add a second callback that prints a message roughly every 500 calls
         runner.add_step_callback(random_print)
 
+        # Add a third callback to check on the time elapsed and change speed
+        runner.add_step_callback(monitor.check_tick)
+
         stats.start()
 
         if args.duration < 0:

python/delphyne/maliput.cc

@@ -8,7 +8,6 @@
 *****************************************************************************/
 
 #include <limits>
-#include <memory>
 
 #include <pybind11/functional.h>
 #include <pybind11/pybind11.h>

python/delphyne/simulation.cc

@@ -123,18 +123,19 @@ PYBIND11_MODULE(simulation, m) {
   py::class_<AutomotiveSimulator<double>>(m, "AutomotiveSimulator")
       .def(py::init(
           [](void) { return std::make_unique<AutomotiveSimulator<double>>(); }))
-      .def("add_agent", &AutomotiveSimulator<double>::AddAgent)
-      .def("get_agent_by_id",
-           &AutomotiveSimulator<double>::GetAgentById,
+      .def("add_agent", &AutomotiveSimulator<double>::AddAgent,
            py::return_value_policy::reference_internal)
-      .def("get_mutable_agent_by_id",
-           &AutomotiveSimulator<double>::GetMutableAgentById,
+      .def("get_collisions", &AutomotiveSimulator<double>::GetCollisions,
            py::return_value_policy::reference_internal)
-      .def("get_collisions", &AutomotiveSimulator<double>::GetCollisions)
       .def("start", &AutomotiveSimulator<double>::Start)
       .def("set_road_geometry", &AutomotiveSimulator<double>::SetRoadGeometry,
            "Transfer a road geometry to the control of the simulator",
-           py::arg("road_geometry"));
+           py::arg("road_geometry"))
+      .def("get_current_simulation_time",
+           &AutomotiveSimulator<double>::GetCurrentSimulationTime)
+      .def("get_mutable_context",
+           &AutomotiveSimulator<double>::GetMutableContext,
+           py::return_value_policy::reference_internal);
 }
 
 /*****************************************************************************

python/delphyne/utilities.py

@@ -147,9 +147,9 @@ def add_rail_car(simulator, name, lane, position, offset,
         longitudinal_position=position,  # lane s-coordinate (m)
         lateral_offset=offset,           # lane r-coordinate (m)
         speed=speed,                     # initial speed in s-direction (m/s)
-        nominal_speed=5.0,               # nominal_speed (m/s)
+        nominal_speed=20.0,              # nominal_speed (m/s)
         road_geometry=road_geometry)     # maliput road geometry
-    simulator.add_agent(agent)
+    return simulator.add_agent(agent)
 
 
 # pylint: disable=too-many-arguments

src/agents/rail_car.cc

@@ -22,6 +22,7 @@
 #include <drake/automotive/maliput/api/segment.h>
 #include <drake/common/eigen_types.h>
 #include <drake/systems/framework/context.h>
+#include <drake/systems/primitives/constant_vector_source.h>
 
 // public headers
 #include "delphyne/macros.h"
@@ -31,6 +32,8 @@
 #include "systems/rail_follower.h"
 #include "systems/rail_follower_params.h"
 #include "systems/rail_follower_state.h"
+#include "systems/speed_system.h"
+#include "systems/vector_source.h"
 
 /*****************************************************************************
  ** Namespaces
@@ -127,6 +130,21 @@ std::unique_ptr<Agent::DiagramBundle> RailCar::BuildDiagram() const {
           context_numeric_parameters));
   rail_follower_system->set_name(name_ + "_system");
 
+  vel_setter_ = builder.template AddSystem(
+      std::make_unique<delphyne::VectorSource<double>>(-1));
+
+  delphyne::SpeedSystem<double>* speed_system = builder.AddSystem(
+      std::make_unique<delphyne::SpeedSystem<double>>());
+
+  builder.Connect(speed_system->acceleration_output(),
+                  rail_follower_system->command_input());
+
+  builder.Connect(rail_follower_system->velocity_output(),
+                  speed_system->feedback_input());
+
+  builder.Connect(vel_setter_->output(),
+                  speed_system->command_input());
+
   /*********************
    * Diagram Outputs
    *********************/
@@ -137,4 +155,8 @@ std::unique_ptr<Agent::DiagramBundle> RailCar::BuildDiagram() const {
   return std::move(builder.Build());
 }
 
+void RailCar::SetSpeed(double new_speed_mps) {
+  vel_setter_->Set(new_speed_mps);
+}
+
 }  // namespace delphyne

src/agents/rail_car.h

@@ -16,9 +16,12 @@
 #include <drake/automotive/maliput/api/lane.h>
 #include <drake/automotive/maliput/api/road_geometry.h>
 #include <drake/common/drake_copyable.h>
+#include <drake/systems/primitives/constant_vector_source.h>
 
 // public headers
 #include "delphyne/mi6/agent_base.h"
+#include "systems/speed_system.h"
+#include "systems/vector_source.h"
 
 /*****************************************************************************
 ** Namespaces
@@ -65,6 +68,12 @@ class RailCar : public delphyne::Agent {
 
   std::unique_ptr<DiagramBundle> BuildDiagram() const;
 
+  ///
+  /// @brief Change the speed of this agent.
+  ///
+  /// @param new_speed_mps[in] The new speed for the agent in meters/second
+  void SetSpeed(double new_speed_mps);
+
  private:
   // Container for the agent's initial configuration.
   //
@@ -90,6 +99,8 @@ class RailCar : public delphyne::Agent {
           nominal_speed(nominal_speed) {}
   } initial_parameters_;
 
+  mutable delphyne::VectorSource<double>* vel_setter_;
+
   const drake::maliput::api::RoadGeometry& road_geometry_;
 };
 

src/backend/automotive_simulator.cc

@@ -112,12 +112,9 @@ std::unique_ptr<ignition::msgs::Scene> AutomotiveSimulator<T>::GetScene() {
   return std::move(scene_msg);
 }
 
-// TODO(jwnimmer-tri): Modify the various vehicle model systems to be more
-// uniform so common code from the following AddFooCar() methods can be moved
-// into a shared method.
-
 template <typename T>
-int AutomotiveSimulator<T>::AddAgent(std::unique_ptr<AgentBase<T>> agent) {
+delphyne::AgentBase<T>* AutomotiveSimulator<T>::AddAgent(
+    std::unique_ptr<AgentBase<T>> agent) {
   /*********************
    * Checks
    *********************/
@@ -167,7 +164,8 @@ int AutomotiveSimulator<T>::AddAgent(std::unique_ptr<AgentBase<T>> agent) {
 
   // save a handle to it
   agents_[id] = std::move(agent);
-  return id;
+
+  return agents_[id].get();
 }
 
 template <typename T>
@@ -180,22 +178,6 @@ const RoadGeometry* AutomotiveSimulator<T>::SetRoadGeometry(
   return road_geometry_.get();
 }
 
-template <typename T>
-const delphyne::AgentBase<T>&
-AutomotiveSimulator<T>::GetAgentById(int agent_id) const {
-  DELPHYNE_VALIDATE(agents_.count(agent_id) != 0, std::runtime_error,
-                    "No agent found with the given ID.");
-  return *agents_.at(agent_id);
-}
-
-template <typename T>
-delphyne::AgentBase<T>* AutomotiveSimulator<T>::GetMutableAgentById(
-    int agent_id) {
-  DELPHYNE_VALIDATE(agents_.count(agent_id) != 0, std::runtime_error,
-                    "No agent found with the given ID.");
-  return agents_[agent_id].get();
-}
-
 template <typename T>
 void AutomotiveSimulator<T>::GenerateAndLoadRoadNetworkUrdf() {
   std::string filename = road_geometry_->id().string();
@@ -240,16 +222,17 @@ struct IsSourceOf {
 }  // namespace
 
 template <typename T>
-const std::vector<std::pair<int, int>> AutomotiveSimulator<T>::GetCollisions()
-    const {
+const std::vector<std::pair<delphyne::AgentBase<T>*, delphyne::AgentBase<T>*>>
+AutomotiveSimulator<T>::GetCollisions() {
   DELPHYNE_VALIDATE(has_started(), std::runtime_error,
                     "Can only get collisions on a running simulation");
   using drake::geometry::GeometryId;
   using drake::geometry::QueryObject;
   using drake::geometry::PenetrationAsPointPair;
   const std::vector<PenetrationAsPointPair<T>> collisions =
       scene_query_->GetValue<QueryObject<T>>().ComputePointPairPenetration();
-  std::vector<std::pair<int, int>> agents_colliding;
+  std::vector<std::pair<delphyne::AgentBase<T>*, delphyne::AgentBase<T>*>>
+      agents_colliding;
   for (const auto& collision : collisions) {
     const auto it_A = std::find_if(agents_.begin(), agents_.end(),
                                    IsSourceOf<T, GeometryId>(collision.id_A));
@@ -259,7 +242,8 @@ const std::vector<std::pair<int, int>> AutomotiveSimulator<T>::GetCollisions()
                                    IsSourceOf<T, GeometryId>(collision.id_B));
     DELPHYNE_VALIDATE(it_B != agents_.end(), std::runtime_error,
                       "Could not find second agent in list of agents");
-    agents_colliding.emplace_back(it_A->first, it_B->first);
+    agents_colliding.emplace_back(agents_[it_A->first].get(),
+                                  agents_[it_B->first].get());
   }
   return agents_colliding;
 }
@@ -447,7 +431,7 @@ void AutomotiveSimulator<T>::StepBy(const T& time_step) {
 }
 
 template <typename T>
-double AutomotiveSimulator<T>::get_current_simulation_time() const {
+double AutomotiveSimulator<T>::GetCurrentSimulationTime() const {
   return drake::ExtractDoubleOrThrow(simulator_->get_context().get_time());
 }
 
@@ -477,6 +461,11 @@ PoseBundle<T> AutomotiveSimulator<T>::GetCurrentPoses() const {
   return pose_bundle;
 }
 
+template <typename T>
+drake::systems::Context<T>* AutomotiveSimulator<T>::GetMutableContext() {
+  return &simulator_->get_mutable_context();
+}
+
 template class AutomotiveSimulator<double>;
 
 }  // namespace delphyne