Skip to content

Commit

Permalink
[JuPedSim] Added unboarding with a grid to solve constraint violations.
Browse files Browse the repository at this point in the history
  • Loading branch information
@bcoueraud87 @behrisch
bcoueraud87 authored and behrisch committed Mar 22, 2024
1 parent 30a7800 commit e18378f
Show file tree
Hide file tree
Showing 20 changed files with 238 additions and 106 deletions.
4 changes: 2 additions & 2 deletions src/libsumo/Person.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -609,7 +609,7 @@ Person::convertTraCIStage(const TraCIStage& stage, const std::string personID) {
arrivalPos = to->getLength();
}
}
return new MSStageDriving(nullptr, to, bs, arrivalPos, StringTokenizer(stage.line).getVector());
return new MSStageDriving(nullptr, to, bs, arrivalPos, 0.0, StringTokenizer(stage.line).getVector());
}

case STAGE_WALKING: {
Expand Down Expand Up @@ -686,7 +686,7 @@ Person::appendDrivingStage(const std::string& personID, const std::string& toEdg
throw TraCIException("Invalid stopping place id '" + stopID + "' for person: '" + personID + "'");
}
}
p->appendStage(new MSStageDriving(nullptr, edge, bs, edge->getLength() - NUMERICAL_EPS, StringTokenizer(lines).getVector()));
p->appendStage(new MSStageDriving(nullptr, edge, bs, edge->getLength() - NUMERICAL_EPS, 0.0, StringTokenizer(lines).getVector()));
}


Expand Down
2 changes: 1 addition & 1 deletion src/microsim/MSRouteHandler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1153,7 +1153,7 @@ MSRouteHandler::addRideOrTransport(const SUMOSAXAttributes& attrs, const SumoXML
const std::string intendedVeh = attrs.getOpt<std::string>(SUMO_ATTR_INTENDED, nullptr, ok, "");
const SUMOTime intendedDepart = attrs.getOptSUMOTimeReporting(SUMO_ATTR_DEPART, nullptr, ok, -1);
arrivalPos = SUMOVehicleParameter::interpretEdgePos(arrivalPos, to->getLength(), SUMO_ATTR_ARRIVALPOS, agent + " '" + aid + "' takes a " + mode + " to edge '" + to->getID() + "'");
myActiveTransportablePlan->push_back(new MSStageDriving(from, to, s, arrivalPos, st.getVector(), group, intendedVeh, intendedDepart));
myActiveTransportablePlan->push_back(new MSStageDriving(from, to, s, arrivalPos, 0.0, st.getVector(), group, intendedVeh, intendedDepart));
myParamStack.push_back(myActiveTransportablePlan->back());
} catch (ProcessError&) {
deleteActivePlanAndVehicleParameter();
Expand Down
54 changes: 45 additions & 9 deletions src/microsim/MSTrainHelper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@


const double MSTrainHelper::CARRIAGE_DOOR_WIDTH = 1.5;
const double MSTrainHelper::PEDESTRIAN_RADIUS_EXTRA_TOLERANCE = 0.01;

// ===========================================================================
// method definitions
Expand Down Expand Up @@ -66,7 +67,7 @@ MSTrainHelper::computeTrainDimensions(double exaggeration, int vehicleQuality) {


void
MSTrainHelper::computeCarriages(bool secondaryShape, bool reversed) {
MSTrainHelper::computeCarriages(bool reversed, bool secondaryShape) {
myCarriages.clear();

const MSLane* lane = myTrain->getLane(); // Lane on which the carriage's front is situated.
Expand Down Expand Up @@ -136,13 +137,6 @@ MSTrainHelper::computeCarriages(bool secondaryShape, bool reversed) {

carriage->front = lane->getShape(secondaryShape).positionAtOffset(carriageOffset * lane->getLengthGeometryFactor(secondaryShape), lateralOffset);
carriage->back = backLane->getShape(secondaryShape).positionAtOffset(carriageBackOffset * lane->getLengthGeometryFactor(secondaryShape), lateralOffset);
Position direction = carriage->front - carriage->back;
direction.norm2D();
double carriageLength = (i == myFirstCarriageNo ? myLocomotiveLength : myCarriageLengthWithGap - myCarriageGap);
for (int j = 1; j <= myCarriageDoors; j++) {
const double doorOffset = j * carriageLength / (myCarriageDoors + 1);
carriage->doors.push_back(carriage->front - direction*doorOffset);
}
myCarriages.push_back(carriage);

carriageOffset -= (curCLength + myCarriageGap);
Expand All @@ -151,6 +145,49 @@ MSTrainHelper::computeCarriages(bool secondaryShape, bool reversed) {
}


void
MSTrainHelper::computeDoorPositions() {
for (Carriage* carriage : myCarriages) {
Position dir = carriage->front - carriage->back;
const double carriageLength = dir.length2D();
if (carriageLength > 0.0) {
dir.norm2D();
for (int j = 1; j <= myCarriageDoors; j++) {
const double doorOffset = j * carriageLength / (myCarriageDoors + 1);
carriage->doorPositions.push_back(carriage->front - dir*doorOffset);
}
}
}
}


void
MSTrainHelper::computeUnboardingPositions(double passengerRadius, std::list<Position>& unboardingPositions) {
passengerRadius += PEDESTRIAN_RADIUS_EXTRA_TOLERANCE;
for (Carriage* carriage : myCarriages) {
Position dir = carriage->front - carriage->back;
const double carriageLength = dir.length2D();
if (carriageLength > 0.0) {
dir.norm2D();
const Position perp = Position(-dir.y(), dir.x());
double nbrLongitudinalCells, longitudinalOffset;
longitudinalOffset = std::modf((carriageLength - 2.0*passengerRadius) / (2.0*passengerRadius), &nbrLongitudinalCells);
double nbrLateralCells, lateralOffset;
lateralOffset = std::modf((myHalfWidth*2.0 - 2.0*passengerRadius) / (2.0*passengerRadius), &nbrLateralCells);
const Position gridOrigin = carriage->back + dir*(passengerRadius + 0.5*longitudinalOffset) - perp*(myHalfWidth - passengerRadius - 0.5*lateralOffset);
for (unsigned int i = 0; i <= (unsigned int)nbrLongitudinalCells; i++) {
for (unsigned int j = 0; j <= (unsigned int)nbrLateralCells; j++) {
carriage->unboardingPositions.push_back(gridOrigin + dir * i * 2.0*passengerRadius + perp * j * 2.0*passengerRadius);
}
}
}
// Shuffle the positions upstream so that we won't have to sample later on, just pop the last element.
RandHelper::shuffle(carriage->unboardingPositions);
std::copy(carriage->unboardingPositions.begin(), carriage->unboardingPositions.end(), std::back_inserter(unboardingPositions));
}
}


double
MSTrainHelper::getUpscaleLength(double upscale, double length, int vehicleQuality) {
if (upscale > 1 && length > 5 && vehicleQuality != 4) {
Expand All @@ -159,4 +196,3 @@ MSTrainHelper::getUpscaleLength(double upscale, double length, int vehicleQualit
return upscale;
}
}

68 changes: 41 additions & 27 deletions src/microsim/MSTrainHelper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@

#include <utils/geom/Position.h>
#include <utils/geom/PositionVector.h>

#include "MSVehicleControl.h"

// ===========================================================================
// class declarations
Expand All @@ -35,15 +35,23 @@ class MSVehicle;
// ===========================================================================
/**
* @class MSTrainHelper
* @brief A class that helps computing positions of a train's carriages.
* @brief A class that helps computing positions of a train's carriages
* and additional structures.
*
*/
class MSTrainHelper {
public:
MSTrainHelper(const MSVehicle* vehicle, bool reversed=false, bool secondaryShape=false, double exaggeration=1.0, int vehicleQuality = 3)
: myTrain(vehicle) {
struct Carriage {
Position front;
Position back;
std::vector<Position> doorPositions;
std::vector<Position> unboardingPositions;
};

MSTrainHelper(const MSVehicle* vehicle, bool reversed = false, bool secondaryShape = false, double exaggeration = 1.0, int vehicleQuality = 3)
: myTrain(vehicle) {
computeTrainDimensions(exaggeration, vehicleQuality);
computeCarriages(secondaryShape, reversed);
computeCarriages(reversed, secondaryShape);
}

~MSTrainHelper() {
Expand All @@ -52,58 +60,67 @@ class MSTrainHelper {
}
}

inline double getUpscaleLength(void) const {
inline double getUpscaleLength() const {
return myUpscaleLength;
}

inline double getHalfWidth(void) const {
inline double getHalfWidth() const {
return myHalfWidth;
}

inline int getNumCarriages(void) const {
inline int getNumCarriages() const {
return myNumCarriages;
}

inline double getCarriageLength(void) const {
inline double getCarriageLength() const {
return myCarriageLength;
}

inline double getFirstCarriageLength(void) const {
inline double getFirstCarriageLength() const {
return myFirstCarriageLength;
}

inline int getCarriageDoors(void) const {
inline int getCarriageDoors() const {
return myCarriageDoors;
}

inline int getFirstCarriageNo(void) const {
inline int getFirstCarriageNo() const {
return myFirstCarriageNo;
}

inline int getFirstPassengerCarriage(void) const {
inline int getFirstPassengerCarriage() const {
return myFirstPassengerCarriage;
}

inline bool isReversed(void) const {
inline bool isReversed() const {
return myIsReversed;
}

struct Carriage {
Position front;
Position back;
std::vector<Position> doors;
};

inline const std::vector<Carriage*>& getCarriages(void) const {
inline const std::vector<Carriage*>& getCarriages() const {
return myCarriages;
}

/// @brief compute door positions on demand and fills the carriage structures
/// @remark need to be called before retrieving carriages if door positions needed
void computeDoorPositions();

static const double CARRIAGE_DOOR_WIDTH;

/// @brief compute unboarding positions on demand and fills the carriage structures
/// @remark need to be called before retrieving carriages if unboarding positions needed
void computeUnboardingPositions(double passengerRadius, std::list<Position>& unboardingPositions);

/// @brief return length exaggeration factor (special for long vehicles)
static double getUpscaleLength(double upscale, double length, int vehicleQuality);


/// @brief average door width used to compute doors positions
static const double CARRIAGE_DOOR_WIDTH;

/// @brief small extra tolerance used to avoid constraint violations
static const double PEDESTRIAN_RADIUS_EXTRA_TOLERANCE;

private:
void computeTrainDimensions(double exaggeration, int vehicleQuality);
void computeCarriages(bool reversed, bool secondaryShape);

const MSVehicle* myTrain;
double myUpscaleLength;
double myLocomotiveLength;
Expand All @@ -120,7 +137,4 @@ class MSTrainHelper {
int myFirstPassengerCarriage;
bool myIsReversed;
std::vector<Carriage*> myCarriages;

void computeTrainDimensions(double exaggeration, int vehicleQuality = 3);
void computeCarriages(bool secondaryShape, bool reversed);
};
4 changes: 3 additions & 1 deletion src/microsim/MSVehicle.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -991,7 +991,8 @@ MSVehicle::MSVehicle(SUMOVehicleParameter* pars, ConstMSRoutePtr route,
myJunctionEntryTimeNeverYield(SUMOTime_MAX),
myJunctionConflictEntryTime(SUMOTime_MAX),
myTimeSinceStartup(TIME2STEPS(3600 * 24)),
myInfluencer(nullptr) {
myInfluencer(nullptr),
myUnboardingPositions(new std::list<Position>()) {
myCFVariables = type->getCarFollowModel().createVehicleVariables();
myNextDriveItem = myLFLinkLanes.begin();
}
Expand All @@ -1005,6 +1006,7 @@ MSVehicle::~MSVehicle() {
}
delete myInfluencer;
delete myCFVariables;
delete myUnboardingPositions;
}


Expand Down
9 changes: 8 additions & 1 deletion src/microsim/MSVehicle.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1725,6 +1725,10 @@ class MSVehicle : public MSBaseVehicle {
double dist, double leaveSpeed);
//@}

std::vector<Position>& getUnboardingPositions() const {
return *myUnboardingPositions;
}

protected:

/// @name Interaction with move reminders
Expand Down Expand Up @@ -1935,9 +1939,12 @@ class MSVehicle : public MSBaseVehicle {
SUMOTime myJunctionEntryTimeNeverYield;
SUMOTime myJunctionConflictEntryTime;

/// @brief duration of driving (speed > SUMO_const_haltingSpeed) after the last halting eposide
/// @brief duration of driving (speed > SUMO_const_haltingSpeed) after the last halting episode
SUMOTime myTimeSinceStartup;

/// @brief unboarding positions of passengers if vehicle is a train
std::vector<Position>* const myUnboardingPositions;

protected:

/// @brief Drive process items represent bounds on the safe velocity
Expand Down
5 changes: 3 additions & 2 deletions src/microsim/transportables/MSPModel_JuPedSim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -420,7 +420,8 @@ MSPModel_JuPedSim::execute(SUMOTime time) {
std::vector<GEOSGeometry*> rampPolygons;
for (const MSVehicle* train : allStoppedTrains) {
if (train->getLeavingPersonNumber() > 0) {
const MSTrainHelper trainHelper(train);
MSTrainHelper trainHelper = MSTrainHelper(train);
trainHelper.computeDoorPositions();
const std::vector<MSTrainHelper::Carriage*>& carriages = trainHelper.getCarriages();
for (const MSTrainHelper::Carriage* carriage : carriages) {
Position dir = carriage->front - carriage->back;
Expand All @@ -440,7 +441,7 @@ MSPModel_JuPedSim::execute(SUMOTime time) {
// Create ramps geometry.
p += CARRIAGE_RAMP_WIDTH;
const double d = 0.5 * MSTrainHelper::CARRIAGE_DOOR_WIDTH;
for (const Position& door : carriage->doors) {
for (const Position& door : carriage->doorPositions) {
PositionVector rampShape;
rampShape.push_back(door - perp * p + dir * d);
rampShape.push_back(door - perp * p - dir * d);
Expand Down
48 changes: 31 additions & 17 deletions src/microsim/transportables/MSPerson.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,9 @@
* MSPerson::MSPersonStage_Access - methods
* ----------------------------------------------------------------------- */
MSPerson::MSPersonStage_Access::MSPersonStage_Access(const MSEdge* destination, MSStoppingPlace* toStop,
const double arrivalPos, const double dist, const bool isExit, const Position& startPos, const Position& endPos) :
MSStage(destination, toStop, arrivalPos, MSStageType::ACCESS),
const double arrivalPos, const double arrivalPosLat, const double dist, const bool isExit,
const Position& startPos, const Position& endPos) :
MSStage(MSStageType::ACCESS, destination, toStop, arrivalPos, arrivalPosLat),
myDist(dist), myAmExit(isExit) {
myPath.push_back(startPos);
myPath.push_back(endPos);
Expand All @@ -68,7 +69,7 @@ MSPerson::MSPersonStage_Access::~MSPersonStage_Access() {}

MSStage*
MSPerson::MSPersonStage_Access::clone() const {
return new MSPersonStage_Access(myDestination, myDestinationStop, myArrivalPos, myDist, myAmExit, myPath.front(), myPath.back());
return new MSPersonStage_Access(myDestination, myDestinationStop, myArrivalPos, myArrivalPosLat, myDist, myAmExit, myPath.front(), myPath.back());
}

void
Expand Down Expand Up @@ -164,25 +165,38 @@ MSPerson::checkAccess(const MSStage* const prior, const bool waitAtStop) {
if (waitAtStop) {
const MSEdge* const stopEdge = &prevStop->getLane().getEdge();
const double arrivalAtBs = (prevStop->getBeginLanePosition() + prevStop->getEndLanePosition()) / 2;
newStage = new MSPersonStage_Access(stopEdge, prevStop, arrivalAtBs, access->length, false,
newStage = new MSPersonStage_Access(stopEdge, prevStop, arrivalAtBs, 0.0, access->length, false,
lane->geometryPositionAtOffset(access->endPos),
prevStop->getLane().geometryPositionAtOffset(arrivalAtBs));
} else {
const double startPos = prior->getStageType() == MSStageType::TRIP ? prior->getEdgePos(0) : prior->getArrivalPos();
const Position& trainExit = prevStop->getLane().geometryPositionAtOffset(startPos);
const double arrivalPos = access->useDoors ? lane->getShape().nearest_offset_to_point2D(trainExit) : access->endPos;
Position platformEntry = lane->geometryPositionAtOffset(arrivalPos);
if (access->useDoors) {
// find the closer side of the platform to enter
const double halfWidth = lane->getWidth() / 2. - MAX2(getVehicleType().getLength(), getVehicleType().getWidth()) / 2. - POSITION_EPS;
platformEntry = lane->geometryPositionAtOffset(arrivalPos, halfWidth);
const Position& plat2 = lane->geometryPositionAtOffset(arrivalPos, -halfWidth);
if (trainExit.distanceSquaredTo2D(plat2) < trainExit.distanceSquaredTo2D(platformEntry)) {
platformEntry = plat2;
const OptionsCont& oc = OptionsCont::getOptions();
const std::string model = oc.getString("pedestrian.model");
if (model != "jupedsim") {
const double startPos = prior->getStageType() == MSStageType::TRIP ? prior->getEdgePos(0) : prior->getArrivalPos();
const Position& trainExit = prevStop->getLane().geometryPositionAtOffset(startPos);
const double arrivalPos = access->useDoors ? lane->getShape().nearest_offset_to_point2D(trainExit) : access->endPos;
Position platformEntry = lane->geometryPositionAtOffset(arrivalPos);
if (access->useDoors) {
// find the closer side of the platform to enter
const double halfWidth = lane->getWidth() / 2. - MAX2(getVehicleType().getLength(), getVehicleType().getWidth()) / 2. - POSITION_EPS;
platformEntry = lane->geometryPositionAtOffset(arrivalPos, halfWidth);
const Position& plat2 = lane->geometryPositionAtOffset(arrivalPos, -halfWidth);
if (trainExit.distanceSquaredTo2D(plat2) < trainExit.distanceSquaredTo2D(platformEntry)) {
platformEntry = plat2;
}
}
}
newStage = new MSPersonStage_Access(accessEdge, prevStop, arrivalPos, access->length, true,
newStage = new MSPersonStage_Access(accessEdge, prevStop, arrivalPos, 0.0, access->length, true,
trainExit, platformEntry);
}
else {
const double startPos = prior->getStageType() == MSStageType::TRIP ? prior->getEdgePos(0) : prior->getArrivalPos();
const double startPosLat = prior->getStageType() == MSStageType::TRIP ? prior->getEdgePosLat(0) : prior->getArrivalPosLat();
// The start and end attributes of the access stage are equal in this case, but we need to compute the arrival position relatively
// to the current lane and not the lane of the previous stage.
const Position start = prevStop->getLane().geometryPositionAtOffset(startPos, startPosLat);
const Position end = lane->getShape().transformToVectorCoordinates(start);
newStage = new MSPersonStage_Access(accessEdge, prevStop, end.x(), -end.y(), access->length, true, start, start);
}
}
myStep = myPlan->insert(myStep, newStage);
return true;
Expand Down
2 changes: 1 addition & 1 deletion src/microsim/transportables/MSPerson.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class MSPerson : public MSTransportable {
public:
/// constructor
MSPersonStage_Access(const MSEdge* destination, MSStoppingPlace* toStop,
const double arrivalPos, const double dist, const bool isExit,
const double arrivalPos, const double arrivalPosLat, const double dist, const bool isExit,
const Position& startPos, const Position& endPos);

/// destructor
Expand Down

0 comments on commit e18378f

Please sign in to comment.