collision.hpp

Go to the documentation of this file.

/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * 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 copyright holder 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 HOLDER 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.
 *********************************************************************/
#ifndef COLLISION_HPP
#define COLLISION_HPP
 
#include <utility>
#include <unordered_map>
#include <tuple>
#include <limits>
#include <armadillo>
 
#include <ergodic_exploration/grid.hpp>
 
namespace ergodic_exploration
{
using arma::vec;
using std::tuple;
 
enum class CollisionMsg
{
  crash,
  obstacle,
  none
};
 
struct CollisionConfig
{
  CollisionConfig(int rb, int rc, int rm, int cx, int cy)
    : r_bnd(rb), r_col(rc), r_max(rm), cx(cx), cy(cy), dx(0), dy(0), sqrd_obs(-1)
  {
  }
 
  const int r_bnd, r_col, r_max;  // bounding , collision, and max search radii
  const int cx, cy;               // circle center
  int dx, dy;                     // displacement from circle center to obstacle
  int sqrd_obs;                   // squared distance circle center to obstacle
};
 
class Collision
{
public:
  Collision(double boundary_radius, double search_radius, double obstacle_threshold,
            double occupied_threshold);
 
  tuple<CollisionMsg, double> minDistance(const GridMap& grid, const vec& pose) const;
 
  tuple<CollisionMsg, vec> minDirection(const GridMap& grid, const vec& pose) const;
 
  bool collisionCheck(const GridMap& grid, const vec& pose) const;
 
  double totalPadding() const;
 
private:
  bool search(CollisionConfig& cfg, const GridMap& grid) const;
 
  bool bresenhamCircle(CollisionConfig& cfg, const GridMap& grid, int r) const;
 
  bool checkCell(CollisionConfig& cfg, const GridMap& grid, unsigned int cj,
                 unsigned int ci) const;
 
private:
  double boundary_radius_;     // circular radius around robot
  double search_radius_;       // search radius for obstacles
  double obstacle_threshold_;  // collision distance threshold
  double occupied_threshold_;  // probaility cell is occupied
};
}  // namespace ergodic_exploration
#endif