Merge pull request #2 from mvirgo/walk-to-vehicle

feat: Passengers walk to vehicles

README.md

@@ -12,15 +12,15 @@ The above amounts were somewhat arbitrarily chosen to make the simulation easier
 
 Below, you can see an example of the simulation running on the Downtown KC map with 10 vehicles and up to 10 passengers waiting at any given time. (You can click to watch a video.)
 
-[![KC Map 10](https://img.youtube.com/vi/KivuGvkSRUI/sddefault.jpg)](https://www.youtube.com/watch?v=KivuGvkSRUI)
+[![KC Map 10](https://img.youtube.com/vi/\_kr48qo56CY/sddefault.jpg)](https://www.youtube.com/watch?v=\_kr48qo56CY)
 
 The next example below is to show that the simulator can generalize to other maps, in this case the area near the Arc de Triomphe in Paris.
 
-[![Paris Map 10](https://img.youtube.com/vi/IibIyyhbfHs/sddefault.jpg)](https://www.youtube.com/watch?v=IibIyyhbfHs)
+[![Paris Map 10](https://img.youtube.com/vi/FNv-oBnHNto/sddefault.jpg)](https://www.youtube.com/watch?v=FNv-oBnHNto)
 
 This final example is to show the simulation can scale up fairly well - 100 vehicles, up to 100 passengers waiting for pick-up, with generation of a new waiting passenger happening every 0-1 second.
 
-[![KC Map 100](https://img.youtube.com/vi/0u3_8vQH2Xo/sddefault.jpg)](https://www.youtube.com/watch?v=0u3_8vQH2Xo)
+[![KC Map 100](https://img.youtube.com/vi/r5-mBsroAJk/sddefault.jpg)](https://www.youtube.com/watch?v=r5-mBsroAJk)
 
 ## Command Line Arguments
 
@@ -37,7 +37,7 @@ Each of the above has a default value that will be used if the related argument
 
 ## Future Improvement Areas
 
-1. Passengers currently "teleport" to the vehicle when the vehicle reaches the closest node on the road to the passenger location, and also teleport out when similarly reaching the closest road node to the final destination. This can be improved to show the passenger "walk" to and from the vehicle instead.
+1. Passengers now walk to the vehicle location when it arrives, but will disappear/teleport once at the closest rode node to their destination. To an extent, I feel this matches to an actual ridesharing app (i.e. you get dropped off near the "real" exact place you are going to at a building), but I could add an animation to make this more obvious.
 2. Vehicles currently ignore the directions of streets. "Fixing" this may cause more situations where a vehicle or passenger is "stuck".
 3. Certain routing is a bit finicky near intersections, causing a slight backtracking. The route planner likely needs some further improvement to guarantee nodes are always "forward" near an intersection.
 4. Make vehicle/passenger generation more dynamic around potential supply/demand. This could result in a vehicle/passenger leaving the map if they have to wait to long for a match, or also where more vehicles would appear if passengers are waiting longer, or vice versa.

src/concurrent/object_holder.h

@@ -27,6 +27,7 @@ class ObjectHolder {
     virtual void GenerateNew() {};
     const int MAX_OBJECTS_; // Set max number of objects to pause generation at
     RouteModel *model_;
+    double distance_per_cycle_; // max distance to move per cycle for smooth-looking movement
     int idCnt_ = 0; // Count object ids
     std::shared_ptr<RoutePlanner> route_planner_; // Route planner to use throughout the sim
 };

src/concurrent/passenger_queue.cpp

@@ -25,6 +25,8 @@ PassengerQueue::PassengerQueue(RouteModel *model,
                                int max_objects, int min_wait_time, int range_wait_time) :
                                ObjectHolder(model, route_planner, max_objects),
                                MIN_WAIT_TIME_(min_wait_time), RANGE_WAIT_TIME_(range_wait_time) {
+    // Set distance per cycle based on model's latitudes
+    distance_per_cycle_ = std::abs(model_->MaxLat() - model->MinLat()) / 3000.0;
     // Start by creating half the max number of passengers
     // Note that the while loop avoids generating less if any invalid placements occur
     while (new_passengers_.size() < MAX_OBJECTS_ / 2) {
@@ -37,7 +39,7 @@ void PassengerQueue::GenerateNew() {
     auto start = model_->GetRandomMapPosition();
     auto dest = model_->GetRandomMapPosition();
     // Set those to passenger
-    std::shared_ptr<Passenger> passenger = std::make_shared<Passenger>();
+    std::shared_ptr<Passenger> passenger = std::make_shared<Passenger>(distance_per_cycle_);
     passenger->SetPosition(start);
     passenger->SetDestination(dest);
     // Set path with route planner, and verify the path between them is valid/reachable
@@ -93,9 +95,12 @@ void PassengerQueue::WaitForRide() {
         // Read and act on any messages
         ReadMessages();
 
+        // Walk toward vehicles for passengers who have an arrived ride
+        WalkPassengersToVehicles();
+
         // Request rides for passengers in queue, if not yet requested
         for (auto passenger_pair : new_passengers_) {
-            if (!(passenger_pair.second->RideRequested())) {
+            if (passenger_pair.second->GetStatus() == Passenger::PassengerStatus::no_ride_requested) {
                 RequestRide(passenger_pair.second);
             }
         }
@@ -131,7 +136,7 @@ void PassengerQueue::ReadMessages() {
 }
 
 void PassengerQueue::RequestRide(std::shared_ptr<Passenger> passenger) {
-    passenger->SetRideRequest(true);
+    passenger->SetStatus(Passenger::PassengerStatus::ride_requested);
     if (ride_matcher_ != nullptr) {
         ride_matcher_->Message({ .message_code=RideMatcher::passenger_requests_ride, .id=passenger->Id() });
     }
@@ -142,13 +147,24 @@ void PassengerQueue::RideOnWay(int id) {
 }
 
 void PassengerQueue::RideArrived(int id) {
+    auto passenger = new_passengers_.at(id);
+    // Set as a walking passenger
+    walking_passengers_.emplace(id, passenger);
+    new_passengers_.erase(id);
+    // Vehicle will be at closest road node to passenger position
+    auto vehicle_location = model_->FindClosestNode(passenger->GetPosition());
+    passenger->SetWalkToPos(vehicle_location);
+    passenger->SetStatus(Passenger::PassengerStatus::walking);
+}
+
+void PassengerQueue::PassengerAtVehicle(int id) {
     // Send the passenger to the vehicle
     ride_matcher_->Message({ .message_code=RideMatcher::passenger_to_vehicle, .id=id });
 }
 
 void PassengerQueue::PassengerPickedUp(int id) {
     // Erase the passenger from the queue
-    new_passengers_.erase(id);
+    walking_passengers_.erase(id);
 }
 
 void PassengerQueue::PassengerFailure(int id) {
@@ -165,7 +181,20 @@ void PassengerQueue::PassengerFailure(int id) {
         std::cout << "Passenger #" << passenger->Id() <<" unreachable multiple times, leaving map." << std::endl;
     } else {
         // Make a new request by setting ride requested to false
-        passenger->SetRideRequest(false);
+        passenger->SetStatus(Passenger::PassengerStatus::no_ride_requested);
+    }
+}
+
+void PassengerQueue::WalkPassengersToVehicles() {
+    for (auto [id, passenger] : walking_passengers_) {
+        if (passenger->GetStatus() == Passenger::PassengerStatus::walking) {
+            passenger->IncrementalMove();
+            // If passenger now at ride after incremental move, send message
+            // Nesting this prevents any double-sending
+            if (passenger->GetStatus() == Passenger::PassengerStatus::at_ride) {
+                PassengerAtVehicle(id);
+            }
+        }
     }
 }
 

src/concurrent/passenger_queue.h

@@ -45,6 +45,7 @@ class PassengerQueue : public ConcurrentObject, public ObjectHolder, public Mess
 
     // Getters / Setters
     const std::unordered_map<int, std::shared_ptr<Passenger>>& NewPassengers() { return new_passengers_; }
+    const std::unordered_map<int, std::shared_ptr<Passenger>>& WalkingPassengers() { return walking_passengers_; }
     void SetRideMatcher(std::shared_ptr<RideMatcher> ride_matcher) { ride_matcher_ = ride_matcher; }
 
     // Concurrent simulation
@@ -67,9 +68,14 @@ class PassengerQueue : public ConcurrentObject, public ObjectHolder, public Mess
     void RideOnWay(int id);
     // Notification that ride has arrived for a passenger
     void RideArrived(int id);
+    // Passenger has walked to and arrived at the vehicle
+    void PassengerAtVehicle(int id);
     // Notification that a passenger was picked up by vehicle, and can be removed locally
     void PassengerPickedUp(int id);
 
+    // Passenger walking to vehicle functionality
+    void WalkPassengersToVehicles();
+
     // Message reading - take action based on given message
     void ReadMessages();
 
@@ -80,6 +86,7 @@ class PassengerQueue : public ConcurrentObject, public ObjectHolder, public Mess
     const int MIN_WAIT_TIME_; // seconds to wait between generation attempts
     const int RANGE_WAIT_TIME_; // range in seconds to wait between generation attempts
     std::unordered_map<int, std::shared_ptr<Passenger>> new_passengers_;
+    std::unordered_map<int, std::shared_ptr<Passenger>> walking_passengers_;
     std::shared_ptr<RideMatcher> ride_matcher_;
 };
 

src/concurrent/ride_matcher.cpp

@@ -48,7 +48,7 @@ void RideMatcher::VehicleHasArrived(int v_id) {
 }
 
 void RideMatcher::PassengerToVehicle(int p_id) {
-    auto passenger = passenger_queue_->NewPassengers().at(p_id);
+    auto passenger = passenger_queue_->WalkingPassengers().at(p_id);
     // Add passenger to related vehicle
     int v_id = passenger_to_vehicle_match_.at(p_id);
     vehicle_manager_->PassengerIntoVehicle(v_id, passenger);

src/concurrent/vehicle_manager.cpp

@@ -8,7 +8,6 @@
 
 #include "vehicle_manager.h"
 
-#include <cmath>
 #include <memory>
 
 #include "ride_matcher.h"
@@ -23,12 +22,12 @@ namespace rideshare {
 VehicleManager::VehicleManager(RouteModel *model,
                                std::shared_ptr<RoutePlanner> route_planner,
                                int max_objects) : ObjectHolder(model, route_planner, max_objects) {
+    // Set distance per cycle based on model's latitudes
+    distance_per_cycle_ = std::abs(model_->MaxLat() - model->MinLat()) / 1000.0;
     // Generate max number of vehicles at the start
     for (int i = 0; i < MAX_OBJECTS_; ++i) {
         GenerateNew();
     }
-    // Set distance per cycle based on model's latitudes
-    distance_per_cycle_ = std::abs(model_->MaxLat() - model->MinLat()) / 1000.0;
 }
 
 void VehicleManager::GenerateNew() {
@@ -40,7 +39,7 @@ void VehicleManager::GenerateNew() {
     auto nearest_start = model_->FindClosestNode(start);
     auto nearest_dest = model_->FindClosestNode(destination);
     // Set road position, destination and id of vehicle
-    std::shared_ptr<Vehicle> vehicle = std::make_shared<Vehicle>();
+    std::shared_ptr<Vehicle> vehicle = std::make_shared<Vehicle>(distance_per_cycle_);
     vehicle->SetPosition((Coordinate){.x = nearest_start.x, .y = nearest_start.y});
     vehicle->SetDestination((Coordinate){.x = nearest_dest.x, .y = nearest_dest.y});
     vehicle->SetId(idCnt_++);
@@ -62,34 +61,6 @@ void VehicleManager::ResetVehicleDestination(std::shared_ptr<Vehicle> vehicle, b
     vehicle->SetDestination((Coordinate){.x = nearest_dest.x, .y = nearest_dest.y});
 }
 
-// Make for smooth, incremental driving between path nodes
-void VehicleManager::IncrementalMove(std::shared_ptr<Vehicle> vehicle) {
-    // Try to get the next_pos; an error here may happen due to a race condition
-    //   where the path has been reset mid-stream
-    Model::Node next_pos;
-    try {
-        next_pos = vehicle->Path().at(vehicle->PathIndex());
-    } catch (...) {
-        // Don't increment and instead return; it will be given a new path soon
-        return;
-    }
-    // Check distance to next position vs. distance can go b/w timesteps
-    Coordinate pos = vehicle->GetPosition();
-    double distance = std::sqrt(std::pow(next_pos.x - pos.x, 2) + std::pow(next_pos.y - pos.y, 2));
-
-    if (distance <= distance_per_cycle_) {
-        // Don't need to calculate intermediate point, just set position as next_pos
-        vehicle->SetPosition((Coordinate){.x = next_pos.x, .y = next_pos.y});
-        vehicle->IncrementPathIndex();
-    } else {
-        // Calculate an intermediate position
-        double angle = std::atan2(next_pos.y - pos.y, next_pos.x - pos.x); // angle from x-axis
-        double new_pos_x = pos.x + (distance_per_cycle_ * std::cos(angle));
-        double new_pos_y = pos.y + (distance_per_cycle_ * std::sin(angle));
-        vehicle->SetPosition((Coordinate){.x = new_pos_x, .y = new_pos_y});
-    }
-}
-
 void VehicleManager::Simulate() {
     // Launch Drive function in a thread
     threads.emplace_back(std::thread(&VehicleManager::Drive, this));
@@ -128,7 +99,7 @@ void VehicleManager::Drive() {
                 continue;
             } else {
                 // Drive to current destination
-                IncrementalMove(vehicle);
+                vehicle->IncrementalMove();
             }
 
             // Check if at destination

src/concurrent/vehicle_manager.h

@@ -54,8 +54,6 @@ class VehicleManager : public ConcurrentObject, public ObjectHolder {
     // Movement
     // Handle loop cycle of movements and actions based on assignments / arrival at passengers
     void Drive();
-    // Make movement smooth between nodes on the map
-    void IncrementalMove(std::shared_ptr<Vehicle> vehicle);
     // Either gets a random map position, or uses the given destination, and aligns either to closest map node
     void ResetVehicleDestination(std::shared_ptr<Vehicle> vehicle, bool random);
     // Vehicle has encountered some type of issue reaching a given destination, without a passenger within
@@ -80,7 +78,6 @@ class VehicleManager : public ConcurrentObject, public ObjectHolder {
     std::unordered_map<int, std::shared_ptr<Passenger>> passenger_pickups_; // store passenger pickups for next cycle
     std::unordered_map<int, Coordinate> new_assignment_locations; // store new assignments for next cycle
     std::vector<int> to_remove_; // store vehicle ids of those to remove the next cycle (due to too many failures)
-    double distance_per_cycle_; // max distance to move per cycle for smooth-looking driving
     std::shared_ptr<RideMatcher> ride_matcher_;
     std::mutex passenger_pickups_mutex; // protect read/write access to passenger pickups between cycles
     std::mutex new_assignment_locations_mutex; // protect read/write access to new assignments between cycles

src/map_object/map_object.h

@@ -9,6 +9,7 @@
 #ifndef MAP_OBJECT_H_
 #define MAP_OBJECT_H_
 
+#include <cmath>
 #include <cstdlib>
 #include <vector>
 
@@ -31,7 +32,9 @@ enum DrawMarker {
 class MapObject {
   public:
     // Constructor / Destructor
-    MapObject() { SetRandomColors(); }
+    MapObject(double distance_per_cycle) : distance_per_cycle_(distance_per_cycle) {
+      SetRandomColors();
+    }
 
     // Getters / Setters
     void SetPosition(const Coordinate &position) { position_ = position; }
@@ -47,16 +50,28 @@ class MapObject {
     int Id() { return id_; }
     std::vector<Model::Node> Path() { return path_; }
 
+    // Movement
+    virtual void IncrementalMove() {};
+
     // Handling of failures (such as destination can't be reached from position)
     bool MovementFailure() {
         ++failures_;
         return failures_ >= MAX_FAILURES_;
     }
 
   protected:
+    // Get an intermediate position between current position and desired next position
+    Coordinate GetIntermediatePosition(double next_x, double next_y) {
+        double angle = std::atan2(next_y - position_.y, next_x - position_.x); // angle from x-axis
+        double new_pos_x = position_.x + (distance_per_cycle_ * std::cos(angle));
+        double new_pos_y = position_.y + (distance_per_cycle_ * std::sin(angle));
+        return (Coordinate){.x = new_pos_x, .y = new_pos_y};
+    }
+
     // Member variables
     int id_;
     int failures_ = 0;
+    const double distance_per_cycle_; // max distance to move per cycle for smooth-looking movement
     int MAX_FAILURES_ = 10; // max failures before object will be removed (likely stuck)
     Coordinate position_;
     Coordinate destination_;

src/map_object/passenger.cpp

@@ -0,0 +1,28 @@
+/**
+ * @file passenger.cpp
+ * @brief Implementation of a passenger, particularly walking movement.
+ *
+ * @copyright Copyright (c) 2021, Michael Virgo, released under the MIT License.
+ *
+ */
+
+#include "passenger.h"
+
+namespace rideshare {
+
+void Passenger::IncrementalMove() {
+  // Check distance to next position vs. distance can go b/w timesteps
+  double distance = std::sqrt(std::pow(walk_to_pos_.x - position_.x, 2) + std::pow(walk_to_pos_.y - position_.y, 2));
+
+  if (distance <= distance_per_cycle_) {
+      // Don't need to calculate intermediate point, just set position as next_pos
+      SetPosition((Coordinate){.x = walk_to_pos_.x, .y = walk_to_pos_.y});
+      // Set status as at ride
+      SetStatus(Passenger::PassengerStatus::at_ride);
+  } else {
+      // Calculate an intermediate position
+      SetPosition(GetIntermediatePosition(walk_to_pos_.x, walk_to_pos_.y));
+  }
+}
+
+}

src/map_object/passenger.h

@@ -15,18 +15,32 @@ namespace rideshare {
 
 class Passenger: public MapObject {
   public:
-    // constructor / destructor
+    // Constructor / Destructor
+    Passenger(double distance_per_cycle) : MapObject(distance_per_cycle) {}
 
-    // getters / setters
+    // Enum for statuses
+    enum PassengerStatus {
+      no_ride_requested,
+      ride_requested,
+      walking,
+      at_ride,
+    };
+
+    // Getters / Setters
     int PassShape() { return pass_shape_; }
     int DestShape() { return dest_shape_; }
-    bool RideRequested() { return ride_requested_; }
-    void SetRideRequest(bool requested) { ride_requested_ = requested; }
+    int GetStatus() { return status_; }
+    void SetStatus(int status) { status_ = status; }
+    void SetWalkToPos(Model::Node& walk_to_pos) { walk_to_pos_ = walk_to_pos; }
+
+    // Movement
+    void IncrementalMove();
 
   private:
     int pass_shape_ = DrawMarker::diamond;
     int dest_shape_ = DrawMarker::tilted_cross;
-    bool ride_requested_ = false;
+    int status_ = PassengerStatus::no_ride_requested;
+    Model::Node walk_to_pos_;
 };
 
 }  // namespace rideshare