Skip to content

Add new calcAngleOffset() with known isChangingLanes()-bug #12201

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Dec 11, 2022
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 3 additions & 20 deletions src/microsim/MSLaneChangerSublane.cpp
Original file line number Diff line number Diff line change
@@ -284,7 +284,7 @@ MSLaneChangerSublane::abortLCManeuver(MSVehicle* vehicle) {
vehicle->getLaneChangeModel().updateTargetLane();
if (updatedSpeedLat) {
// update angle after having reset lateral speed
//vehicle->computeAngle();
vehicle->setAngle(vehicle->computeAngle());
}
}

@@ -475,24 +475,7 @@ MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, do
// compute new angle of the vehicle from the x- and y-distances travelled within last time step
// (should happen last because primaryLaneChanged() also triggers angle computation)
// this part of the angle comes from the orientation of our current lane
double laneAngle = vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane())) ;
if (vehicle->getLane()->getShape().length2D() == 0) {
if (vehicle->getFurtherLanes().size() == 0) {
laneAngle = vehicle->getAngle();
} else {
laneAngle = vehicle->getFurtherLanes().front()->getShape().rotationAtOffset(-NUMERICAL_EPS);
}
}
// this part of the angle comes from the vehicle's lateral movement
double changeAngle = 0;
// avoid flicker
if (fabs(latDist) > NUMERICAL_EPS) {
// angle is between vehicle front and vehicle back (and depending on travelled distance)
changeAngle = atan2(DIST2SPEED(latDist), vehicle->getVehicleType().getLength() + vehicle->getSpeed());
if (MSGlobals::gLefthand) {
changeAngle *= -1;
}
}
double laneAngle = vehicle->computeAngle();
if (vehicle->getLaneChangeModel().isOpposite()) {
// reverse lane angle
laneAngle += M_PI;
@@ -517,7 +500,7 @@ MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, do
<< "\n";
}
#endif
vehicle->setAngle(laneAngle + changeAngle, completedManeuver);
vehicle->setAngle(laneAngle, completedManeuver);

// check if a traci maneuver must continue
// getOwnState is reset to 0 when changing lanes so we use the stored reason
7 changes: 4 additions & 3 deletions src/microsim/MSVehicle.cpp
Original file line number Diff line number Diff line change
@@ -1440,9 +1440,9 @@ MSVehicle::computeAngle() const {
}
double result = (p1 != p2 ? p2.angleTo2D(p1) :
myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
if (myLaneChangeModel->isChangingLanes()) {
result += lefthandSign * DEG2RAD(myLaneChangeModel->getAngleOffset());
}

result += lefthandSign * myLaneChangeModel->calcAngleOffset();

#ifdef DEBUG_FURTHER
if (DEBUG_COND) {
std::cout << SIMTIME << " computeAngle veh=" << getID() << " p1=" << p1 << " p2=" << p2 << " angle=" << RAD2DEG(result) << " naviDegree=" << GeomHelper::naviDegree(result) << "\n";
@@ -4320,6 +4320,7 @@ MSVehicle::executeMove() {
}
#endif
}
myLaneChangeModel->setPreviousAngleOffset(myLaneChangeModel->getAngleOffset());
myAngle = computeAngle();
}

19 changes: 13 additions & 6 deletions src/microsim/lcmodels/MSAbstractLaneChangeModel.cpp
Original file line number Diff line number Diff line change
@@ -109,6 +109,8 @@ MSAbstractLaneChangeModel::MSAbstractLaneChangeModel(MSVehicle& v, const LaneCha
myCanceledStateLeft(LCA_NONE),
mySpeedLat(0),
myAccelerationLat(0),
myAngleOffset(0),
myPreviousAngleOffset(0),
myCommittedSpeed(0),
myLaneChangeCompletion(1.0),
myLaneChangeDirection(0),
@@ -734,13 +736,18 @@ MSAbstractLaneChangeModel::determineTargetLane(int& targetDir) const {


double
MSAbstractLaneChangeModel::getAngleOffset() const {
const double totalDuration = (myVehicle.getVehicleType().wasSet(VTYPEPARS_MAXSPEED_LAT_SET)
? SUMO_const_laneWidth / myVehicle.getVehicleType().getMaxSpeedLat()
: STEPS2TIME(MSGlobals::gLaneChangeDuration));
MSAbstractLaneChangeModel::calcAngleOffset() {
double result = 0.;
if (!(fabs(mySpeedLat) < NUMERICAL_EPS && fabs(myPreviousAngleOffset * 180 / PI) < NUMERICAL_EPS)) {
if (myVehicle.getLength() < sqrt(SPEED2DIST(mySpeedLat) * SPEED2DIST(mySpeedLat) + SPEED2DIST(myVehicle.getSpeed()) * SPEED2DIST(myVehicle.getSpeed()))) {
result = atan2(mySpeedLat, myVehicle.getSpeed());
} else {
result = myPreviousAngleOffset + asin((sin(PI / 2 - myPreviousAngleOffset) * (SPEED2DIST(mySpeedLat) - tan(myPreviousAngleOffset) * SPEED2DIST(myVehicle.getSpeed()))) / myVehicle.getLength());
}
}

const double angleOffset = 60 / totalDuration * (pastMidpoint() ? 1 - myLaneChangeCompletion : myLaneChangeCompletion);
return myLaneChangeDirection * angleOffset;
myAngleOffset = result;
return result;
}


23 changes: 22 additions & 1 deletion src/microsim/lcmodels/MSAbstractLaneChangeModel.h
Original file line number Diff line number Diff line change
@@ -468,7 +468,22 @@ class MSAbstractLaneChangeModel {
int getShadowDirection() const;

/// @brief return the angle offset during a continuous change maneuver
double getAngleOffset() const;
double calcAngleOffset();

/// @brief return the angle offset resulting from lane change and sigma
inline double getAngleOffset() const {
return myAngleOffset;
}

/// @brief set the angle offset resulting from lane change and sigma
inline void setAngleOffset(const double angleOffset) {
myAngleOffset = angleOffset;
}

/// @brief set the angle offset of the previous time step
inline void setPreviousAngleOffset(const double angleOffset) {
myPreviousAngleOffset = angleOffset;
}

/// @brief reset the flag whether a vehicle already moved to false
inline bool alreadyChanged() const {
@@ -654,6 +669,12 @@ class MSAbstractLaneChangeModel {
/// @brief the current lateral acceleration
double myAccelerationLat;

/// @brief the current angle offset resulting from lane change and sigma
double myAngleOffset;

/// @brief the angle offset of the previous time step resulting from lane change and sigma
double myPreviousAngleOffset;

/// @brief the speed when committing to a change maneuver
double myCommittedSpeed;