segment_expander.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2017, <copyright holder> <email>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *     * Neither the name of the <organization> nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY <copyright holder> <email> ''AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <copyright holder> <email> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */
 
#include <tuw_global_router/segment_expander.h>
#include <tuw_global_router/route_coordinator.h>
#include <iostream>
#define TIME_OVERLAP (1)
 
namespace multi_robot_router
{
SegmentExpander::SegmentExpander(const CollisionResolverType _cRes) : avr_(TIME_OVERLAP), btr_(TIME_OVERLAP), er_()
{
    setCollisionResolver(_cRes);
}
 
void SegmentExpander::setCollisionResolver(const CollisionResolverType _cRes)
{
    crType_ = _cRes;
    if (_cRes == CollisionResolverType::backtracking)
    {
        collision_resolution_ = &btr_;
    }
    else if (_cRes == CollisionResolverType::avoidance)
    {
        collision_resolution_ = &avr_;
    }
    else //if(_cRes == CollisionResolverType::none)
    {
        collision_resolution_ = &er_;
    }
}
 
SegmentExpander::CollisionResolverType SegmentExpander::getCollisionResolver() const
{
    return crType_;
}
 
void SegmentExpander::reset()
{
    collisions_robots_.clear();
    startSegments_.clear();
}
 
void SegmentExpander::addStartExpansionCandidate(Vertex &_start, Vertex &_current, Vertex &_next, Vertex &_end)
{
    if (_current.potential == -1)
        return;
 
    if (_next.potential >= 0)
        return;
 
    int32_t collision = -1;
 
    if (route_querry_->checkSegment(_next, _current.potential - TIME_OVERLAP, _current.potential + pCalc_.CalculatePotential(_next) + TIME_OVERLAP, diameter_, collision))
    {
        float pot = _current.potential + pCalc_.CalculatePotential(_next);
        float h = hx_.calcHeuristic(_next, _end);
        float weight = pot + h;
 
        _next.weight = weight;
        _next.potential = pot;
        _next.predecessor_ = &_current;
        _next.successor_ = &_start;
        _next.collision = -1;
 
        seg_queue_.push(&_next);
    }
}
 
void SegmentExpander::addExpansoionCandidate(Vertex &_current, Vertex &_next, Vertex &_end)
{
    if (_current.potential == -1)
        return;
 
    if (_next.potential >= 0)
        return;
 
    int32_t collision;
 
    if (!route_querry_->checkSegment(_next, _current.potential - TIME_OVERLAP, _current.potential + pCalc_.CalculatePotential(_next) + TIME_OVERLAP, diameter_, collision))
    {
        if (collision != -1)
        {
            std::vector<std::reference_wrapper<Vertex>> resolutions = collision_resolution_->resolve(_current, _next, collision);
 
            for (Vertex &res : resolutions)
            {
                float h = hx_.calcHeuristic(res, _end);
                res.weight = res.potential + h;
 
                if (res.getSegment().getSegmentId() == _end.getSegment().getSegmentId()) //Should not happen but safety first :D
                {
                    res.weight = 0;
                }
 
                seg_queue_.push(&res);
            }
        }
 
        return;
    }
 
    float pot = _current.potential + pCalc_.CalculatePotential(_next);
    float h = hx_.calcHeuristic(_next, _end);
    float weight = pot + h;
 
    _next.weight = weight;
    _next.potential = pot;
 
    if (_next.getSegment().getSegmentId() == _end.getSegment().getSegmentId())
        _next.weight = 0;
 
    _next.predecessor_ = &_current;
    _next.collision = -1;
 
    seg_queue_.push(&_next);
}
 
bool SegmentExpander::calculatePotentials(const RouteCoordinatorWrapper *_p, Vertex &_start, Vertex &_end, const uint32_t _maxIterations, const uint32_t _diameter)
{
    route_querry_ = _p;
 
    collisions_robots_.clear();
    collision_resolution_->resetSession(_p, &pCalc_, _diameter);
    diameter_ = _diameter;
 
    Vertex *foundEnd = expandVoronoi(_start, _end, _maxIterations);
 
    if (foundEnd == NULL)
        return false;
 
    //Save actual planning status from parallel ends :D
    if (&_end != foundEnd)
        _end.updateVertex(*foundEnd);
 
    return (foundEnd->getSegment().getSegmentId() == _end.getSegment().getSegmentId());
}
 
void SegmentExpander::resolveStartCollision(Vertex &_start, Vertex &_end)
{
    int collision = -1;
    _start.collision = -1;
    _start.predecessor_ = NULL;
    _start.potential = -1;
 
    clearpq(seg_queue_);
 
    //If the robot wants to stay on its pose we have to add a additional segment to enable avoiding higher prioritized robots
    if (_start.getSegment().getSegmentId() == _end.getSegment().getSegmentId() && !route_querry_->checkSegment(_start, 0, -1, diameter_, collision) && collision != -1)
    {
        // Force the robot to move away from the segment
        startSegments_.emplace_back(std::make_unique<Vertex>(_start));
        Vertex &start = *(startSegments_.back().get());
        start.predecessor_ = NULL;
        start.potential = pCalc_.CalculatePotential(start);
        start.collision = -1;
 
        const std::vector<std::reference_wrapper<Vertex>> &n_succ = start.getPlanningSuccessors();
 
        for (Vertex &v : n_succ)
        {
            //One to move back to start
            startSegments_.emplace_back(std::make_unique<Vertex>(v));
            addStartExpansionCandidate(_start, start, *(startSegments_.back().get()), _end);
 
            //One to expand Normal (Not working because we are blocking our own path) //TODO find fix
            //All adjacent vertices to start are allready expanded. Hence there is no way back to start
            //addExpansoionCandidate(start, v, _end);
        }
 
        const std::vector<std::reference_wrapper<Vertex>> &n_pred = start.getPlanningPredecessors();
 
        for (Vertex &v : n_pred)
        {
            //One to move back to start
            startSegments_.emplace_back(std::make_unique<Vertex>(v));
            addStartExpansionCandidate(_start, start, *(startSegments_.back().get()), _end);
 
            //One to expand Normal (Not working well because we are blocking our own path) //TODO find fix
            //All adjacent vertices to start are allready expanded. Hence there is no way back to start
            //addExpansoionCandidate(start, v, _end);
        }
    }
    else
    {
        _start.collision = -1;
        _start.predecessor_ = NULL;
        _start.potential = pCalc_.CalculatePotential(_start);
        clearpq(seg_queue_);
        int32_t collision = -1;
 
        if (route_querry_->checkSegment(_start, 0, _start.potential + TIME_OVERLAP, diameter_, collision))
        {
            seg_queue_.push(&_start);
        }
        else if (collision != -1)
        {
            collision_resolution_->saveCollision(collision);
        }
    }
}
 
bool SegmentExpander::containsVertex(const Vertex &_v, const std::vector<std::reference_wrapper<Vertex>> &_list) const
{
    for (const Vertex &v : _list)
    {
        if (_v.getSegment().getSegmentId() == v.getSegment().getSegmentId())
            return true;
    }
 
    return false;
}
 
void SegmentExpander::setSpeed(const float &_speed)
{
    pCalc_.SetMultiplier(_speed);
}
 
Vertex *SegmentExpander::expandVoronoi(Vertex &_start, Vertex &_end, const uint32_t _cycles)
{
    startSegments_.clear();
 
    uint32_t cycle = 0; //For having a high startpotential
    resolveStartCollision(_start, _end);
 
    Vertex *current = NULL;
 
    while (!(seg_queue_.empty()) && (cycle < _cycles) && (current == NULL || _end.getSegment().getSegmentId() != current->getSegment().getSegmentId()))
    {
        if (seg_queue_.empty())
            return NULL;
 
        current = seg_queue_.top();
        seg_queue_.pop();
 
        //The segment expander is forced to expand to the successor (generated by the collision resolution)
        if (current->successor_ != NULL)
        {
            addExpansoionCandidate(*current, *(current->successor_), _end);
        }
        else
        {
            const std::vector<std::reference_wrapper<Vertex>> &n_succ = current->getPlanningSuccessors();
 
            if (current->predecessor_ == NULL || !containsVertex(*(current->predecessor_), n_succ))
            {
                for (Vertex &v : n_succ)
                {
                    addExpansoionCandidate(*current, v, _end);
                }
            }
 
            const std::vector<std::reference_wrapper<Vertex>> &n_pred = current->getPlanningPredecessors();
 
            if (current->predecessor_ == NULL || !containsVertex(*(current->predecessor_), n_pred))
            {
                for (Vertex &v : n_pred)
                {
                    addExpansoionCandidate(*current, v, _end);
                }
            }
        }
 
        cycle++;
    }
 
    if (current == NULL || current->getSegment().getSegmentId() != _end.getSegment().getSegmentId())
        return NULL;
 
    return current;
}
 
const std::vector<uint32_t> &SegmentExpander::getRobotCollisions() const
{
    return collision_resolution_->getRobotCollisions();
}
 
} // namespace multi_robot_router