shortest_path.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * 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 Willow Garage, Inc. 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 THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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 <occupancy_grid_utils/shortest_path.h>
#include <ros/assert.h>
#include <queue>

namespace occupancy_grid_utils
{

namespace nm=nav_msgs;

using std::vector;
using boost::optional;
using std::max;

struct ShortestPathResult
{
  nm::MapMetaData info;
  index_t src_ind;
  vector<optional<index_t> > back_pointers;
  vector<optional<double> > potential;
};

TerminationCondition::TerminationCondition () 
{}

TerminationCondition::TerminationCondition (const double max_distance) :
  max_distance_(max_distance)
{}

TerminationCondition::TerminationCondition (const Cells& goals) :
  goals_(goals)
{}

TerminationCondition::TerminationCondition (const Cells& goals, const double max_distance) :
  max_distance_(max_distance), goals_(goals)
{}


typedef vector<Cell> Path;
typedef boost::shared_ptr<nm::OccupancyGrid> GridPtr;

boost::optional<double> distance(ResultPtr res, const Cell& dest)
{
  return res->potential[cellIndex(res->info, dest)];
}



boost::optional<Path> extractPath(ResultPtr res, const Cell& dest)
{
  boost::optional<Path> p;
  index_t current=cellIndex(res->info, dest);
  if (!res->back_pointers[current])
    return p; // No path exists

  index_t max=res->back_pointers.size();
  index_t n=0;
  p = Path();

  do {
    if (n++ > max) {
      ROS_FATAL("Cycle in extractPath");
      ROS_BREAK();
    }
    p->push_back(indexCell(res->info, current));
    ROS_ASSERT_MSG(res->back_pointers[current], "Back pointer not found for %u", current);
    current=*(res->back_pointers[current]);
  } while (current!=res->src_ind);
  p->push_back(indexCell(res->info, current));
  reverse(p->begin(), p->end());
  return p;
}

    

struct QueueItem
{
  index_t ind;
  double potential;
  QueueItem (const index_t ind, const double potential) :
    ind(ind), potential(potential) {}
};

bool operator< (const QueueItem& i1, const QueueItem& i2)
{
  return i1.potential > i2.potential;
}


ResultPtr singleSourceShortestPaths (const nm::OccupancyGrid& g, const Cell& src)
{
  return singleSourceShortestPaths(g, src, TerminationCondition());
}

ResultPtr singleSourceShortestPaths (const nm::OccupancyGrid& g, const Cell& src, const TerminationCondition& t)
{
  ShortestPathResult* res = new ShortestPathResult();
  ResultPtr result(res);
  res->info = g.info;
  res->src_ind = cellIndex(g.info, src);
  res->back_pointers.resize(g.data.size());
  res->potential.resize(g.data.size());

  std::priority_queue<QueueItem> q;
  q.push(QueueItem(res->src_ind, 0.0));
  res->potential[res->src_ind] = 0.0;
  ROS_DEBUG_NAMED ("shortest_path", "Computing single source shortest paths from %d, %d", src.x, src.y);

  Cells remaining_goals;
  if (t.goals_)
    remaining_goals = *t.goals_;

  while (!q.empty()) {
    const QueueItem i=q.top();
    q.pop();

    const Cell cell = indexCell(g.info, i.ind);
    const double dist = i.potential;
    if (t.max_distance_ && *t.max_distance_ < dist)
      break;
    if (t.goals_) {
      remaining_goals.erase(cell);
      if (remaining_goals.empty())
        break;
    }
    
    for (int dx=-1; dx<=1; dx++) {
      for (int dy=-1; dy<=1; dy++) {
        if ((dx==0) && (dy==0))
          continue;
        const double d = ((dx==0) || (dy==0)) ? 1 : sqrt(2);
        const Cell neighbor(cell.x+dx, cell.y+dy);
        if (withinBounds(g.info, neighbor)) {
          const index_t ind=cellIndex(g.info, neighbor);
          const double new_dist = dist+d;
          if (g.data[ind]==UNOCCUPIED &&
              (!res->potential[ind] || *(res->potential[ind]) > new_dist) &&
              (!t.max_distance_ || *t.max_distance_ >= new_dist)) {
            ROS_DEBUG_NAMED ("shortest_path_propagation", "Adding cell %d, %d, at distance %.2f",
                             neighbor.x, neighbor.y, new_dist);
            res->potential[ind] = new_dist;
            res->back_pointers[ind] = i.ind;
            q.push(QueueItem(ind, new_dist));
          }
        }
      }
    }
  }
  ROS_DEBUG_NAMED ("shortest_path", "Done computing single source shortest paths from %d, %d", src.x, src.y);
  return result;
}

          
void propagateValue (nm::OccupancyGrid* g, const coord_t x0, const coord_t y0, 
                     const signed char val, const int r)
{
  for (int dx=-r; dx<=r; dx++) {
    for (int dy=-r; dy<=r; dy++) {
      if (dx*dx + dy*dy <= r*r) {
        const int x=x0+dx;
        const int y=y0+dy;
        const Cell cell(x, y);
        if ((x>=0) && (x<(int)g->info.width) && (y>=0) && (y<(int)g->info.height) &&
            withinBounds(g->info, cell)) {
          const int ind=cellIndex(g->info, cell);
          g->data[ind] = max(g->data[ind], val);
          ROS_ASSERT(g->data[ind]>=1 && g->data[ind]<=100); // Can take this out eventually
        }
      }
    }
  }
}
        
        
GridPtr inflateObstacles (const nm::OccupancyGrid& g, const double r)
{
  ROS_ASSERT (r>0);
  const int radius = ceil(r/g.info.resolution);
  GridPtr g2(new nm::OccupancyGrid(g));

  for (coord_t x=0; x<(int)g.info.width; x++) {
    for (coord_t y=0; y<(int)g.info.height; y++) {
      const Cell cell(x, y);
      if (withinBounds(g.info, cell)) {
        const char val=g.data[cellIndex(g.info, cell)];
        if (val >= 1) {
          ROS_ASSERT_MSG (val <= 100, "Unexpected cell value %d", val);
          propagateValue(g2.get(), x, y, val, radius);
        }
      }
    }
  }
  
  return g2;
}

/************************************************************
 * A*
 ***********************************************************/

typedef std::pair<Path, double> AStarResult;
struct PQItem
{
  index_t ind;
  double g_cost;
  double h_cost;
  PQItem (index_t ind, double g_cost, double h_cost) :
    ind(ind), g_cost(g_cost), h_cost(h_cost) {}

  bool operator< (const PQItem& i2) const
  {
    return ((g_cost + h_cost) > (i2.g_cost + i2.h_cost));
  }
};


inline double manhattanHeuristic (const Cell& c1, const Cell& c2)
{
  return fabs(c1.x-c2.x) + fabs(c1.y-c2.y);
}

optional<AStarResult> shortestPath(const nm::OccupancyGrid& g, const Cell& src, const Cell& dest)
{
  std::priority_queue<PQItem> open_list;
  const unsigned num_cells = g.info.height*g.info.width;
  std::vector<bool> seen(num_cells); // Default initialized to all false
  open_list.push(PQItem(cellIndex(g.info, src), 0, manhattanHeuristic(src, dest)));
  std::vector<index_t> parent(num_cells);
  const index_t dest_ind = cellIndex(g.info, dest);

  optional<AStarResult> res;
  ROS_DEBUG_STREAM_NAMED ("shortest_path", "Computing shortest path from " << src << " to " << dest);
  
  while (!open_list.empty()) {
    const PQItem current = open_list.top();
    open_list.pop();
    const Cell c = indexCell(g.info, current.ind);
    if (seen[current.ind])
      continue;
    seen[current.ind] = true;
    ROS_DEBUG_STREAM_NAMED ("shortest_path_internal", "  Considering " << c << " with cost " <<
                            current.g_cost << " + " << current.h_cost);
    if (current.ind == dest_ind) {
      res = AStarResult();
      res->second = current.g_cost;
      // Todo fill in path
      break;
    }
      
    for (int d=-1; d<=1; d+=2) {
      for (int vertical=0; vertical<2; vertical++) {
        const int cx = c.x + d*(1-vertical);
        const int cy = c.y + d*vertical;
        if (cx>=0 && cy>=0) {
          const Cell c2((coord_t) cx, (coord_t) cy);
          if (withinBounds(g.info, c2)) {
            const index_t ind = cellIndex(g.info, c2);
            if (g.data[ind]==UNOCCUPIED && !seen[ind]) {
              open_list.push(PQItem(ind, current.g_cost + 1, manhattanHeuristic(c2, dest)));
              parent[ind] = current.ind;
            }
            ROS_DEBUG_STREAM_COND_NAMED (g.data[ind]!=UNOCCUPIED, "shortest_path_internal",
                                         "  Skipping cell " << indexCell(g.info, ind) <<
                                         " with cost " << (unsigned) g.data[ind]);
          }
        }
      }
    }
  }

  ROS_DEBUG_STREAM_NAMED ("shortest_path", "Computed shortest path.  Found = " << (bool) res.is_initialized());
  return res;
}



/************************************************************
 * Ros message conversion
 ************************************************************/

ResultPtr shortestPathResultFromMessage (const NavigationFunction& msg)
{
  ShortestPathResult* res = new ShortestPathResult();
  ResultPtr retval(res); // retval is a pointer to const, so we modify via res
  res->info = msg.info;
  res->src_ind = msg.source;

  const index_t num_cells = msg.valid.size();
  ROS_ASSERT (num_cells == msg.back_pointers.size());
  ROS_ASSERT (num_cells == msg.potential.size());
  res->back_pointers.resize(num_cells);
  res->potential.resize(num_cells);

  for (index_t i=0; i<num_cells; i++) {
    if (i==msg.source) {
      // Sanity check
      ROS_ASSERT (msg.back_pointers[i] == 123456);
      ROS_ASSERT (msg.potential[i] == -1.0);
      res->potential[i] = 0.0;
    }
    else if (msg.valid[i]) {
      res->back_pointers[i] = msg.back_pointers[i];
      res->potential[i] = msg.potential[i];
    }
    else {
      // Sanity check
      ROS_ASSERT (msg.back_pointers[i] == 234567);
      ROS_ASSERT (msg.potential[i] == -2.0);
    }
  }

  return retval;
}

NavigationFunction shortestPathResultToMessage (ResultPtr res)
{
  NavigationFunction msg;
  msg.info = res->info;
  msg.source = res->src_ind;
  
  const index_t num_cells=res->back_pointers.size();
  ROS_ASSERT (num_cells == res->potential.size());
  msg.valid.resize(num_cells);
  msg.back_pointers.resize(num_cells);
  msg.potential.resize(num_cells);

  for (index_t i=0; i<num_cells; i++) {
    if (i==msg.source) {
      msg.valid[i] = false;
      msg.potential[i] = -1.0;
      msg.back_pointers[i] = 123456;
    }
    else if (res->potential[i]) {
      msg.valid[i] = true;
      msg.potential[i] = *res->potential[i];
      msg.back_pointers[i] = *res->back_pointers[i];
    }
    else {
      msg.valid[i] = false;
      msg.potential[i] = -2.0;
      msg.back_pointers[i] = 234567;
    }
  }
  
  return msg;
}


  
} // namespace occupancy_grid_utils