32 #define TIME_INFINITY (-1) 44 for (uint32_t i = 0; i < _path.size(); i++)
51 begin = _path[i - 1].potential;
54 if (i != _path.size() - 1)
56 end = _path[i].potential;
59 if (!
timeline_.
addSegment(begin, end, _path[i].getSegment().getSegmentId(), _robotId, _diameterPixel,
true))
65 std::vector<uint32_t> pred = _path[i].getSegment().getPredecessors();
67 if (_path[i].overlapPredecessor)
69 for (
const uint32_t idx : pred)
79 std::vector<uint32_t> succ = _path[i].getSegment().getSuccessors();
81 if (_path[i].overlapSuccessor)
83 for (
const uint32_t idx : succ)
112 bool RouteCoordinatorTimed::checkSegment(
const Vertex &_next,
const uint32_t _startTime,
const int32_t _endTime,
const uint32_t _diameterPixel, int32_t &_collisionRobot,
const uint32_t _robotId,
bool _ignoreGoal)
const 116 int32_t endTime = _endTime;
118 if (
isGoal(_next, _robotId) && !_ignoreGoal)
123 if (!
checkSegmentSingle(_next, _startTime, endTime, _diameterPixel, _collisionRobot, _robotId, _ignoreGoal))
132 for (
const uint32_t idx : pred)
145 for (
const uint32_t idx : succ)
159 if (
isGoal(_next, _robotId) && !_ignoreGoal)
181 for (
int i = 0; i < _goalSegments.size(); i++)
193 for (
int i = 0; i < _startSegments.size(); i++)
229 nrRobots_ = _nrRobots;
231 segmentSpace_.clear();
234 for (
int i = 0; i < _graph.size(); i++)
237 segmentSpace_.push_back(_graph[i].width());
245 if (!
checkSegment(_startTime, _endTime, _segId, _robotNr, _robotSize, collision))
250 int freeSpace = segmentSpace_[_segId];
252 if (_endTime > maxTime_)
258 if (robotSegments_.size() <= _robotNr)
260 robotSegments_.resize(_robotNr + 1);
263 robotSegments_[_robotNr].push_back(_segId);
264 timeline_[_segId].emplace_back(_robotNr, (
float)_robotSize, _startTime, _endTime, _mainSeg);
271 if (_robotSize > segmentSpace_[_segId])
273 return addSegment(_startTime, _endTime, _segId, _robotNr, segmentSpace_[_segId], _mainSeg);
276 return addSegment(_startTime, _endTime, _segId, _robotNr, _robotSize, _mainSeg);
281 if (_robotSize > segmentSpace_[_segId])
283 return checkSegment(_startTime, _endTime, _segId, _robotNr, segmentSpace_[_segId], _lastCollisionRobot);
286 return checkSegment(_startTime, _endTime, _segId, _robotNr, _robotSize, _lastCollisionRobot);
291 _lastCollisionRobot = -1;
294 int freeSpace = segmentSpace_[_segId];
296 if (freeSpace < _robotSize)
301 std::vector<bool> checkedRobots_(nrRobots_,
false);
305 if (occupation.robot != _robotNr && !checkedRobots_[occupation.robot])
311 freeSpace -= occupation.spaceOccupied;
312 _lastCollisionRobot = occupation.robot;
313 checkedRobots_[occupation.robot] =
true;
315 if (freeSpace < _robotSize)
320 else if (_endTime ==
TIME_INFINITY && _startTime <= occupation.endTime)
322 freeSpace -= occupation.spaceOccupied;
323 _lastCollisionRobot = occupation.robot;
324 checkedRobots_[occupation.robot] =
true;
326 if (freeSpace < _robotSize)
331 else if (occupation.endTime ==
TIME_INFINITY && _endTime >= occupation.startTime)
333 freeSpace -= occupation.spaceOccupied;
334 _lastCollisionRobot = occupation.robot;
335 checkedRobots_[occupation.robot] =
true;
337 if (freeSpace < _robotSize)
346 if ((_startTime <= occupation.startTime && _endTime >= occupation.startTime) || (_startTime <= occupation.endTime && _endTime >= occupation.endTime))
348 freeSpace -= occupation.spaceOccupied;
349 _lastCollisionRobot = occupation.robot;
350 checkedRobots_[occupation.robot] =
true;
352 if (freeSpace < _robotSize)
357 else if ((_startTime >= occupation.startTime && _endTime <= occupation.endTime))
359 freeSpace -= occupation.spaceOccupied;
360 _lastCollisionRobot = occupation.robot;
361 checkedRobots_[occupation.robot] =
true;
363 if (freeSpace < _robotSize)
373 _lastCollisionRobot = -1;
389 for (
const int &
s : robotSegments_[_robot])
393 if (occupation.robot == _robot && _timestep >= occupation.startTime && _timestep <= occupation.endTime && occupation.mainSeg)
407 for (
const auto &occupation :
timeline_[_segId])
409 if (occupation.robot == _robotNr)
411 ret = occupation.endTime;
420 std::vector<std::pair<uint32_t, float>> robots;
424 for (
const auto &occupation :
timeline_[_segId])
426 if (occupation.robot == _robot)
428 occupationRobot = occupation;
433 if (occupationRobot.
robot != _robot)
436 for (
const auto &occupation :
timeline_[_segId])
438 if (occupation.robot != _robot && occupation.startTime < _potential)
440 if (segmentSpace_[_segId] - occupation.spaceOccupied < occupationRobot.
spaceOccupied)
443 robots.push_back({occupation.robot, occupation.endTime});
455 for (std::vector<seg_occupation> &occ :
timeline_)
457 occ.erase(std::remove_if(occ.begin(), occ.end(), [_robot](
seg_occupation x) {
return x.robot == _robot; }), occ.end());
462 for (std::vector<seg_occupation> &occ : timeline_)
466 if (o.endTime > maxTime_)
467 maxTime_ = o.endTime;
471 if (robotSegments_.size() > _robot)
472 robotSegments_[_robot].clear();
uint32_t getSegmentId() const
void reset(const std::vector< Segment > &_graph, const uint32_t _nrRobots)
resets the planning session of multiple robots
void removeRobot(const uint32_t _robot)
removes a Robot from the Routing Table
uint32_t getSize() const
returns the length of the timeline (max Time)
const uint32_t getEnd(const uint32_t _robotId) const
returns the Goal of a robot
std::vector< std::pair< uint32_t, float > > getListOfRobotsHigherPrioritizedRobots(const uint32_t _robot, const uint32_t _segId, const int32_t _potential) const
returns all robots which pass the segment before the given robot and a given time ...
bool setStartSegments(const std::vector< uint32_t > &_startSegments)
updates the start segments of the planning attempt
int32_t findSegId(const int32_t _robot, const uint32_t _timestep) const
returns the segment id at which the robot is at timepoint _potential
int32_t findSegNr(const uint32_t _robot, const uint32_t _potential) const
returns the segment id at which the robot is at timepoint _potential
std::vector< uint32_t > startSegments_
std::vector< std::pair< uint32_t, float > > getListOfRobotsHigherPrioritizedRobots(const uint32_t _robot, const uint32_t _segId, const int32_t _potential) const
returns all robots which pass the segment before the given robot and a given time ...
std::vector< uint32_t > goalSegments_
RouteCoordinatorTimed()
constructor
void removeRobot(const uint32_t _robot)
removes a Robot from the Routing Table
bool checkSegmentSingle(const Vertex &_next, const uint32_t _startTime, const int32_t _endTime, const uint32_t _diameterPixel, int32_t &_collisionRobot, const uint32_t _robotId, const bool &_ignoreGoal) const
bool checkCrossingSegment(const uint32_t _startTime, const int32_t _endTime, const uint32_t _segId, const uint32_t _robotNr, const uint32_t _robotSize, int32_t &_lastCollisionRobot) const
checks if a robot can enter a segment at a specific time if located in a crossing (neighbours > 1) ...
bool checkSegment(const uint32_t _startTime, const int32_t _endTime, const uint32_t _segId, const uint32_t _robotNr, const uint32_t _robotSize, int32_t &_lastCollisionRobot) const
checks if a robot can enter a segment at a specific time
bool addCrossingSegment(const uint32_t _startTime, const int32_t _endTime, const uint32_t _segId, const uint32_t _robotNr, const uint32_t _robotSize, const bool &_mainSeg)
adds a segment in a crossing (neighbours > 1) to the timeline and checks if the occupation is valid ...
bool isGoal(const Vertex &_seg, const uint32_t _robotId) const
returns if a Vertex is the goal vertex for a robot
TFSIMD_FORCE_INLINE const tfScalar & x() const
const std::vector< uint32_t > & getSuccessors() const
int32_t findPotentialUntilRobotOnSegment(const uint32_t _robot, const uint32_t _segId) const
returns the time point, when a robot leaves a vertex
bool addSegment(const uint32_t _startTime, const int32_t _endTime, const uint32_t _segId, const uint32_t _robotNr, const uint32_t _robotSize, bool _mainSeg)
adds a segment to the timeline and checks if the occupation is valid
const Segment & getSegment() const
bool addRoute(const std::vector< RouteVertex > &_path, const uint32_t _diameterPixel, const uint32_t _robotId)
adds a new route to the Routing table and checks if the Routing Table is valid
const uint32_t getStart(const uint32_t _robotId) const
returns the start of a robot
const std::vector< uint32_t > & getPredecessors() const
bool checkSegment(const Vertex &_next, const uint32_t _startTime, const int32_t _endTime, const uint32_t _diameterPixel, int32_t &_collisionRobot, const uint32_t _robotId, bool _ignoreGoal=false) const
checks if a robot can enter a segment at a specific time
int32_t getTimeUntilRobotOnSegment(const int32_t _robotNr, const uint32_t _segId) const
returns the time point, when a robot leaves a vertex
bool setGoalSegments(const std::vector< uint32_t > &_goalSegments)
updates the goal segments of the planning attempt
void reset(const std::vector< Segment > &_graph, const uint32_t _nrRobots)
resets the planning session of multiple robots