コード例 #1
0
ファイル: MSLaneChanger.cpp プロジェクト: planetsumo/sumo
bool
MSLaneChanger::changeOpposite(std::pair<MSVehicle*, SUMOReal> leader) {
    if (!myChangeToOpposite) {
        return false;
    }
    myCandi = findCandidate();
    MSVehicle* vehicle = veh(myCandi);
    MSLane* source = vehicle->getLane();
    if (vehicle->isStopped()) {
        // stopped vehicles obviously should not change lanes. Usually this is
        // prevent by appropriate bestLane distances
        return false;
    }
    const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
    if (!isOpposite && leader.first == 0) {
        // no reason to change unless there is a leader
        // or we are changing back to the propper direction
        // XXX also check whether the leader is so far away as to be irrelevant
        return false;
    }
    MSLane* opposite = source->getOpposite();
    if (opposite == 0) {
        return false;
    }

    // changing into the opposite direction is always to the left (XXX except for left-hand networkds)
    int direction = isOpposite ? -1 : 1;
    std::pair<MSVehicle*, SUMOReal> neighLead((MSVehicle*)0, -1);

    // preliminary sanity checks for overtaking space
    SUMOReal timeToOvertake;
    SUMOReal spaceToOvertake;
    if (!isOpposite) {
        assert(leader.first != 0);
        // find a leader vehicle with sufficient space ahead for merging back
        const SUMOReal overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
        const SUMOReal mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
        std::pair<MSVehicle*, SUMOReal> columnLeader = leader;
        SUMOReal egoGap = leader.second;
        bool foundSpaceAhead = false;
        SUMOReal seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
        std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
        while (!foundSpaceAhead) {
            const SUMOReal requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
                        columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
                    + vehicle->getVehicleType().getLengthWithGap());


            // all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles
            const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->getEdge());
            std::pair<MSVehicle* const, SUMOReal> leadLead = columnLeader.first->getLane()->getLeader(
                        columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap, 
                        checkTmpVehicles);

#ifdef DEBUG_CHANGE_OPPOSITE
            if (DEBUG_COND) {
                std::cout << "   leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
            }
#endif
            if (leadLead.first == 0) {
                foundSpaceAhead = true;
            } else {
                const SUMOReal requiredSpace = (requiredSpaceAfterLeader
                                                + vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
                if (leadLead.second > requiredSpace) {
                    foundSpaceAhead = true;
                } else {
#ifdef DEBUG_CHANGE_OPPOSITE
                    if (DEBUG_COND) {
                        std::cout << "   not enough space after columnLeader=" << columnLeader.first->getID() << " required=" << requiredSpace << "\n";
                    }
#endif
                    seen += MAX2((SUMOReal)0, leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
                    if (seen > OPPOSITE_OVERTAKING_MAX_LOOKAHEAD) {
#ifdef DEBUG_CHANGE_OPPOSITE
                        if (DEBUG_COND) {
                            std::cout << "   cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << columnLeader.first->getID() << ")\n";
                        }
#endif
                        return false;
                    }
                    // see if merging after leadLead is possible
                    egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
                    columnLeader = leadLead;
#ifdef DEBUG_CHANGE_OPPOSITE
                    if (DEBUG_COND) {
                        std::cout << "   new columnLeader=" << columnLeader.first->getID() << "\n";
                    }
#endif
                }
            }
        }
#ifdef DEBUG_CHANGE_OPPOSITE
        if (DEBUG_COND) {
            std::cout << "   compute time/space to overtake for columnLeader=" << columnLeader.first->getID() << " gap=" << columnLeader.second << "\n";
        }
#endif
        computeOvertakingTime(vehicle, columnLeader.first, egoGap, timeToOvertake, spaceToOvertake);
        // check for upcoming stops
        if (vehicle->nextStopDist() < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
            if (DEBUG_COND) {
                std::cout << "   cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
            }
#endif
            return false;
        }
        neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake, true);

#ifdef DEBUG_CHANGE_OPPOSITE
        if (DEBUG_COND) {
            std::cout << SIMTIME
                      << " veh=" << vehicle->getID()
                      << " changeOpposite opposite=" << opposite->getID()
                      << " lead=" << Named::getIDSecure(leader.first)
                      << " timeToOvertake=" << timeToOvertake
                      << " spaceToOvertake=" << spaceToOvertake
                      << "\n";
        }
#endif

        // check for dangerous oncoming leader
        if (neighLead.first != 0) {
            const MSVehicle* oncoming = neighLead.first;

#ifdef DEBUG_CHANGE_OPPOSITE
            if (DEBUG_COND) {
                std::cout << SIMTIME
                          << " oncoming=" << oncoming->getID()
                          << " oncomingGap=" << neighLead.second
                          << " leaderGap=" << leader.second
                          << "\n";
            }
#endif
            if (neighLead.second - spaceToOvertake - timeToOvertake * oncoming->getSpeed() < 0) {

#ifdef DEBUG_CHANGE_OPPOSITE
                if (DEBUG_COND) {
                    std::cout << "   cannot changeOpposite due to dangerous oncoming\n";
                }
#endif
                return false;
            }
        }
    } else {
        timeToOvertake = -1;
        // look forward as far as possible
        spaceToOvertake = std::numeric_limits<SUMOReal>::max();
        leader = source->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD, true);
        // -1 will use getMaximumBrakeDist() as look-ahead distance
        neighLead = opposite->getOppositeLeader(vehicle, -1, false); 
    }

    // compute remaining space on the opposite side
    // 1. the part that remains on the current lane
    SUMOReal usableDist = isOpposite ? vehicle->getPositionOnLane() : source->getLength() - vehicle->getPositionOnLane();
    if (usableDist < spaceToOvertake) {
        // look forward along the next lanes
        const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
        assert(bestLaneConts.size() >= 1);
        std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
        while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
#ifdef DEBUG_CHANGE_OPPOSITE
            if (DEBUG_COND) {
                std::cout << "      usableDist=" << usableDist << " opposite=" << Named::getIDSecure((*it)->getOpposite()) << "\n";
            }
#endif
            if ((*it)->getOpposite() == 0) {
                // opposite lane ends
                break;
            }
            // do not overtake past a minor link or turn
            if (*(it - 1) != 0) {
                MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
                if (link == 0 || !link->havePriority() || link->getState() == LINKSTATE_ZIPPER || link->getDirection() != LINKDIR_STRAIGHT) {
                    break;
                }
            }
            usableDist += (*it)->getLength();
            ++it;
        }
    }
    if (!isOpposite && usableDist < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
        if (DEBUG_COND) {
            std::cout << "   cannot changeOpposite due to insufficient space (seen=" << usableDist << " spaceToOvertake=" << spaceToOvertake << ")\n";
        }
#endif
        return false;
    }
#ifdef DEBUG_CHANGE_OPPOSITE
    if (DEBUG_COND) {
        std::cout << "   usableDist=" << usableDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
    }
#endif

    // compute wish to change
    std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
    if (isOpposite) {
        // compute the remaining distance that can be drive on the opposite side
        // this value will put into LaneQ.length of the leftmost lane
        // @note: length counts from the start of the current lane
        // @note: see MSLCM_LC2013::_wantsChange @1092 (isOpposite()
        MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
        // position on the target lane 
        const SUMOReal forwardPos = source->getOppositePos(vehicle->getPositionOnLane());

        // consider usableDist (due to minor links or end of opposite lanes)
        laneQ.length = MIN2(laneQ.length, usableDist + forwardPos);
        // consider upcoming stops
        laneQ.length = MIN2(laneQ.length, vehicle->nextStopDist() + forwardPos);
        // consider oncoming leaders
        if (leader.first != 0) {
            laneQ.length = MIN2(laneQ.length, leader.second / 2 + forwardPos);
#ifdef DEBUG_CHANGE_OPPOSITE
        if (DEBUG_COND) {
            std::cout << SIMTIME << " found oncoming leader=" << leader.first->getID() << " gap=" << leader.second << "\n";
        }
#endif
            leader.first = 0; // ignore leader after this
        } 
#ifdef DEBUG_CHANGE_OPPOSITE
        if (DEBUG_COND) {
            std::cout << SIMTIME << " veh=" << vehicle->getID() << " remaining dist=" << laneQ.length - forwardPos << " forwardPos=" << forwardPos << " laneQ.length=" << laneQ.length << "\n";
        }
#endif
    }
    std::pair<MSVehicle* const, SUMOReal> neighFollow = opposite->getOppositeFollower(vehicle);
    int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);

    bool changingAllowed = (state & LCA_BLOCKED) == 0;
    // change if the vehicle wants to and is allowed to change
    if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed
            // do not change to the opposite direction for cooperative reasons
            && (isOpposite || (state & LCA_COOPERATIVE) == 0)) {
        vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
        /// XXX use a dedicated transformation function
        vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
        /// XXX compute a better lateral position
        opposite->forceVehicleInsertion(vehicle, vehicle->getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, 0);
        if (!isOpposite) {
            vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
        }
#ifdef DEBUG_CHANGE_OPPOSITE
        if (DEBUG_COND) {
            std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
        }
#endif
        return true;
    }
#ifdef DEBUG_CHANGE_OPPOSITE
    if (DEBUG_COND) {
        std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction 
            << " opposite=" << Named::getIDSecure(opposite) << " state=" << toString((LaneChangeAction)state) << "\n";
    }
#endif
    return false;
}
コード例 #2
0
ファイル: MSLaneChanger.cpp プロジェクト: cbrafter/sumo
bool
MSLaneChanger::changeOpposite(std::pair<MSVehicle*, SUMOReal> leader) {
    if (!myChangeToOpposite) {
        return false;
    }
    myCandi = findCandidate();
    MSVehicle* vehicle = veh(myCandi);
    MSLane* source = vehicle->getLane();
    if (vehicle->isStopped()) {
        // stopped vehicles obviously should not change lanes. Usually this is
        // prevent by appropriate bestLane distances
        return false;
    }
    const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
    if (!isOpposite && leader.first == 0) {
        // no reason to change unless there is a leader
        // or we are changing back to the propper direction
        // XXX also check whether the leader is so far away as to be irrelevant
        return false;
    }
    if (!source->getEdge().canChangeToOpposite()) {
        return false;
    }
    MSLane* opposite = source->getOpposite();
    if (opposite == 0) {
        return false;
    }

    // changing into the opposite direction is always to the left (XXX except for left-hand networkds)
    int direction = vehicle->getLaneChangeModel().isOpposite() ? -1 : 1;
    std::pair<MSVehicle*, SUMOReal> neighLead((MSVehicle*)0, -1);

    // preliminary sanity checks for overtaking space
    if (!isOpposite) {
        assert(leader.first != 0);
        // find a leader vehicle with sufficient space ahead for merging back
        const SUMOReal overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
        const SUMOReal mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
        const SUMOReal maxLookAhead = 150; // just a guess
        std::pair<MSVehicle*, SUMOReal> columnLeader = leader;
        SUMOReal egoGap = leader.second;
        bool foundSpaceAhead = false;
        SUMOReal seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
        std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
        while (!foundSpaceAhead) {
            const SUMOReal requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
                        columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
                    + vehicle->getVehicleType().getLengthWithGap());


            std::pair<MSVehicle* const, SUMOReal> leadLead = columnLeader.first->getLane()->getLeader(
                        columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap, true);

#ifdef DEBUG_CHANGE_OPPOSITE
            if (DEBUG_COND) {
            	std::cout << "   leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
            }
#endif
            if (leadLead.first == 0) {
                foundSpaceAhead = true;
            } else {
                const SUMOReal requiredSpace = (requiredSpaceAfterLeader
                                                + vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
                if (leadLead.second > requiredSpace) {
                    foundSpaceAhead = true;
                } else {
#ifdef DEBUG_CHANGE_OPPOSITE
                	if (DEBUG_COND) {
                		std::cout << "   not enough space after columnLeader=" << leadLead.first->getID() << " gap=" << leadLead.second << " required=" << requiredSpace << "\n";
                	}
#endif
                    seen += leadLead.second + leadLead.first->getVehicleType().getLengthWithGap();
                    if (seen > maxLookAhead) {
#ifdef DEBUG_CHANGE_OPPOSITE
                    	if (DEBUG_COND) {
                    		std::cout << "   cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << leadLead.first->getID() << ")\n";
                    	}
#endif
                        return false;
                    }
                    // see if merging after leadLead is possible
                    egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
                    columnLeader = leadLead;
#ifdef DEBUG_CHANGE_OPPOSITE
    if (DEBUG_COND) {
                        std::cout << "   new columnLeader=" << columnLeader.first->getID() << "\n";
                    }
#endif
                }
            }
        }
#ifdef DEBUG_CHANGE_OPPOSITE
    if (DEBUG_COND) {
            std::cout << "   compute time/space to overtake for columnLeader=" << columnLeader.first->getID() << " gap=" << columnLeader.second << "\n";
        }
#endif
        SUMOReal timeToOvertake;
        SUMOReal spaceToOvertake;
        computeOvertakingTime(vehicle, columnLeader.first, egoGap, timeToOvertake, spaceToOvertake);
        // check for upcoming stops
        if (vehicle->nextStopDist() < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
    if (DEBUG_COND) {
                std::cout << "   cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
            }
#endif
            return false;
        }
        neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake);

#ifdef DEBUG_CHANGE_OPPOSITE
    if (DEBUG_COND) {
            std::cout << SIMTIME
                      << " veh=" << vehicle->getID()
                      << " changeOpposite opposite=" << opposite->getID()
                      << " lead=" << Named::getIDSecure(leader.first)
                      << " oncoming=" << Named::getIDSecure(neighLead.first)
                      << " timeToOvertake=" << timeToOvertake
                      << " spaceToOvertake=" << spaceToOvertake
                      << "\n";
        }
#endif

        // check for dangerous oncoming leader
        if (!vehicle->getLaneChangeModel().isOpposite() && neighLead.first != 0) {
            const MSVehicle* oncoming = neighLead.first;
            /// XXX what about overtaking multiple vehicles?

#ifdef DEBUG_CHANGE_OPPOSITE
            if (DEBUG_COND) {
            	std::cout << SIMTIME
            			<< " timeToOvertake=" << timeToOvertake
            			<< " spaceToOvertake=" << spaceToOvertake
            			<< " oncomingGap=" << neighLead.second
            			<< " leaderGap=" << leader.second
            			<< "\n";
            }
#endif
            if (neighLead.second - spaceToOvertake - timeToOvertake * oncoming->getSpeed() < 0) {

#ifdef DEBUG_CHANGE_OPPOSITE
            	if (DEBUG_COND) {
            		std::cout << "   cannot changeOpposite due to dangerous oncoming\n";
            	}
#endif
                return false;
            }
        }
        // check for sufficient space on the opposite side
        seen = source->getLength() - vehicle->getPositionOnLane();
        if (!vehicle->getLaneChangeModel().isOpposite() && seen < spaceToOvertake) {
            const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
            assert(bestLaneConts.size() >= 1);
            std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
            while (seen < spaceToOvertake && it != bestLaneConts.end()) {
                if ((*it)->getOpposite() == 0) {
                    break;
                }
                // do not overtake past a minor link
                if (*(it - 1) != 0) {
                    MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
                    if (link == 0 || !link->havePriority() || link->getState() == LINKSTATE_ZIPPER) {
                        break;
                    }
                }
                seen += (*it)->getLength();
            }
            if (seen < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
            	if (DEBUG_COND) {
            		std::cout << "   cannot changeOpposite due to insufficient space (seen=" << seen << " spaceToOvertake=" << spaceToOvertake << ")\n";
            	}
#endif
                return false;
            }
#ifdef DEBUG_CHANGE_OPPOSITE
            if (DEBUG_COND) {
            	std::cout << "   seen=" << seen << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
            }
#endif
        }
    } else {
        /// XXX compute sensible distance
        leader = source->getOppositeLeader(vehicle, 200);
        neighLead = opposite->getOppositeLeader(vehicle, -1);
    }

    // compute wish to change
    std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
    if (isOpposite && leader.first != 0) {
        MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
        /// XXX compute sensible usable dist
        laneQ.length -= MIN2(laneQ.length, opposite->getOppositePos(vehicle->getPositionOnLane()) + leader.second / 2);
        leader.first = 0; // ignore leader
    }
    std::pair<MSVehicle* const, SUMOReal> neighFollow = opposite->getOppositeFollower(vehicle);
    int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);

    bool changingAllowed = (state & LCA_BLOCKED) == 0;
    // change if the vehicle wants to and is allowed to change
    if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed) {
        vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
        /// XXX use a dedicated transformation function
        vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
        vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
        /// XXX compute a bette lateral position
        opposite->forceVehicleInsertion(vehicle, vehicle->getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, 0);
#ifdef DEBUG_CHANGE_OPPOSITE
        if (DEBUG_COND) {
        	std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
        }
#endif
        return true;
    }
#ifdef DEBUG_CHANGE_OPPOSITE
    if (DEBUG_COND) {
    	std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
    }
#endif
    return false;
}