Commit 40521dd7 authored by Michael Duhme's avatar Michael Duhme
Browse files

use clang-format on source files

parent 7deb22f8
......@@ -61,4 +61,3 @@ Standard: Cpp11
TabWidth: 4
UseTab: Never
...
#pragma once
#include <cmath>
#include <utility>
#include <unordered_set>
#include <utility>
//////////////////
/// Point
//////////////////
struct Point {
struct Point
{
int x;
int y;
Point() = default;
constexpr Point(const int x, const int y) noexcept
: x(x), y(y) {}
: x(x)
, y(y)
{
}
bool operator==(const Point &b) const { return x == b.x && y == b.y; }
bool operator==(const Point& b) const { return x == b.x && y == b.y; }
int squaredDistanceTo(const Point &b) const;
int squaredDistanceTo(const Point& b) const;
};
struct PointHash {
std::size_t operator()(const Point &pc) const {
struct PointHash
{
std::size_t operator()(const Point& pc) const
{
return std::hash<int>()(pc.x) ^ std::hash<int>()(pc.y);
}
};
......@@ -34,22 +40,34 @@ using PointSet = std::unordered_set<Point, PointHash>;
/// Line
//////////////////
struct Line {
struct Line
{
int x0, y0, x1, y1;
Line(int x0, int y0, int x1, int y1) : x0(x0), y0(y0), x1(x1), y1(y1) {}
Line(int x0, int y0, int x1, int y1)
: x0(x0)
, y0(y0)
, x1(x1)
, y1(y1)
{
}
bool operator==(const Line &b) const { return x0 == b.x0 && y0 == b.y0 && x1 == b.x1 && y1 == b.y1; }
bool operator==(const Line& b) const
{
return x0 == b.x0 && y0 == b.y0 && x1 == b.x1 && y1 == b.y1;
}
double squaredDistanceTo(const Point &p) const;
double squaredDistanceTo(const Point& p) const;
int squaredLength() const;
double squaredRadius() const;
};
struct LineHash {
std::size_t operator()(const Line &line) const {
struct LineHash
{
std::size_t operator()(const Line& line) const
{
std::hash<int> h;
return h(line.x0) ^ h(line.x1) ^ h(line.y0) ^ h(line.y1);
}
......@@ -57,16 +75,18 @@ struct LineHash {
using LineSet = std::unordered_set<Line, LineHash>;
class CircularArea {
class CircularArea
{
const double r2;
const double c_x, c_y;
public:
explicit CircularArea(const Line &line)
: r2(line.squaredRadius()), c_x(static_cast<double>(line.x0 + line.x1) / 2), c_y(
static_cast<double>(line.y0 + line.y1) / 2) {}
explicit CircularArea(const Line& line)
: r2(line.squaredRadius())
, c_x(static_cast<double>(line.x0 + line.x1) / 2)
, c_y(static_cast<double>(line.y0 + line.y1) / 2)
{
}
bool containsLine(const Line &l) const;
bool containsLine(const Line& l) const;
};
......@@ -4,64 +4,69 @@
#include <nav_msgs/OccupancyGrid.h>
class OccupancyGridWrapper {
nav_msgs::OccupancyGrid grid_;
int grid_width_, grid_height_;
int obstacle_threshold_;
class OccupancyGridWrapper
{
nav_msgs::OccupancyGrid grid_;
int grid_width_, grid_height_;
int obstacle_threshold_;
public:
constexpr static int MAP_VOID = -1;
constexpr static int MAP_MAX = 100;
public:
constexpr static int MAP_VOID = -1;
constexpr static int MAP_MAX = 100;
OccupancyGridWrapper() : grid_width_(0), grid_height_(0), obstacle_threshold_(50) {}
OccupancyGridWrapper()
: grid_width_(0)
, grid_height_(0)
, obstacle_threshold_(50)
{
}
void setGrid(const nav_msgs::OccupancyGrid &grid) {
grid_ = grid;
grid_width_ = static_cast<int>(grid.info.width);
grid_height_ = static_cast<int>(grid.info.height);
}
void setGrid(const nav_msgs::OccupancyGrid& grid)
{
grid_ = grid;
grid_width_ = static_cast<int>(grid.info.width);
grid_height_ = static_cast<int>(grid.info.height);
}
void setObstacleThreshold(const int obstacle_threshold) {
obstacle_threshold_ = obstacle_threshold;
}
void setObstacleThreshold(const int obstacle_threshold)
{
obstacle_threshold_ = obstacle_threshold;
}
/// grid width in pixels
int getWidth() const { return grid_width_; }
/// grid width in pixels
int getWidth() const { return grid_width_; }
/// grid height in pixels
int getHeight() const { return grid_height_; }
/// grid height in pixels
int getHeight() const { return grid_height_; }
int operator[](const int idx) const {
return static_cast<int>(grid_.data[idx]);
}
int operator[](const int idx) const { return static_cast<int>(grid_.data[idx]); }
/// world x-,y-coordinates to OccupancyGrid pixel coordinates
Point getWorldToPixel(double x, double y) const;
/// world x-,y-coordinates to OccupancyGrid pixel coordinates
Point getWorldToPixel(double x, double y) const;
/// pose to pixel coordinates
Point getPoseCoords(const geometry_msgs::Pose &pose) const;
/// pose to pixel coordinates
Point getPoseCoords(const geometry_msgs::Pose& pose) const;
/// pixel coordinates to world coordinates
geometry_msgs::Point getPixelToWorld(const Point &pt) const;
/// pixel coordinates to world coordinates
geometry_msgs::Point getPixelToWorld(const Point& pt) const;
/// \return the occupancy value of a cell and x-,y-coordinates
int getCellContent(int x, int y) const;
/// \return the occupancy value of a cell and x-,y-coordinates
int getCellContent(int x, int y) const;
/// pixel/cell size in meters
double getCellSize() const;
/// pixel/cell size in meters
double getCellSize() const;
/// conversion pixel to meters using cell size
double pixelToMeters(double pixel_length) const;
/// conversion pixel to meters using cell size
double pixelToMeters(double pixel_length) const;
/// conversion meters to pixels using cell size
double metersToPixels(double meter_length) const;
/// conversion meters to pixels using cell size
double metersToPixels(double meter_length) const;
/// test if cell content is greater than obstacle threshold or -1 (undetected space)
bool isObstacleOrVoid(int cell_content) const;
/// test if cell content is greater than obstacle threshold or -1 (undetected space)
bool isObstacleOrVoid(int cell_content) const;
/// test if cell content is greater than obstacle threshold
bool isObstacle(int cell_content) const;
/// test if cell content is greater than obstacle threshold
bool isObstacle(int cell_content) const;
constexpr static bool isVoid(int cell_content);
constexpr static bool isVoid(int cell_content);
};
......@@ -5,56 +5,60 @@
#include <vector>
struct OccupancyGridCell {
Point coords;
bool is_obstacle;
struct OccupancyGridCell
{
Point coords;
bool is_obstacle;
constexpr OccupancyGridCell(const int x, const int y, const bool is_obstacle = false) noexcept
: coords({x, y}), is_obstacle(is_obstacle) {}
constexpr OccupancyGridCell(const int x, const int y, const bool is_obstacle = false) noexcept
: coords({ x, y })
, is_obstacle(is_obstacle)
{
}
};
class RayCastHitResult {
std::vector<OccupancyGridCell> path_;
public:
explicit RayCastHitResult(std::vector<OccupancyGridCell> &&path)
: path_(std::move(path)) {}
class RayCastHitResult
{
std::vector<OccupancyGridCell> path_;
const std::vector<OccupancyGridCell> &getPath() const { return path_; }
public:
explicit RayCastHitResult(std::vector<OccupancyGridCell>&& path)
: path_(std::move(path))
{
}
const OccupancyGridCell &getStartCell() const { return path_.front(); }
const std::vector<OccupancyGridCell>& getPath() const { return path_; }
const OccupancyGridCell &getEndCell() const { return path_.back(); }
const OccupancyGridCell& getStartCell() const { return path_.front(); }
/// \return True if the ray hit an obstacle.
bool obstacleHit() const {
return path_.back().is_obstacle;
}
const OccupancyGridCell& getEndCell() const { return path_.back(); }
/// \return True if the ray hit an obstacle.
bool obstacleHit() const { return path_.back().is_obstacle; }
};
class RayCaster final {
public:
RayCaster() = delete;
/// Generate ray end points in all directions (equiangular) from a given point.
/// \param start_x start x-coord
/// \param start_y start y-coord
/// \param pixel_radius length for all rays
/// \param num_rays number of rays to generate
/// \return list of ray end points
static std::vector<Point> getRayEndPoints(int start_x, int start_y, int pixel_radius, int num_rays);
/// Cast ray until obstacle is hit.
/// \param grid Grid to check if a cell is an obstacle.
/// \param x1 start position x-coord
/// \param y1 start position y-coord
/// \param x2 end position x-coord
/// \param y2 end position y-coord
/// \param exclude_source_point if start point is an obstacle it will be excluded
/// \return hit result that contains the path and last visited point
static RayCastHitResult castRay(const OccupancyGridWrapper &grid,
int x1,
int y1,
int x2,
int y2,
bool exclude_source_point = true);
class RayCaster final
{
public:
RayCaster() = delete;
/// Generate ray end points in all directions (equiangular) from a given point.
/// \param start_x start x-coord
/// \param start_y start y-coord
/// \param pixel_radius length for all rays
/// \param num_rays number of rays to generate
/// \return list of ray end points
static std::vector<Point> getRayEndPoints(
int start_x, int start_y, int pixel_radius, int num_rays);
/// Cast ray until obstacle is hit.
/// \param grid Grid to check if a cell is an obstacle.
/// \param x1 start position x-coord
/// \param y1 start position y-coord
/// \param x2 end position x-coord
/// \param y2 end position y-coord
/// \param exclude_source_point if start point is an obstacle it will be excluded
/// \return hit result that contains the path and last visited point
static RayCastHitResult castRay(const OccupancyGridWrapper& grid, int x1, int y1, int x2,
int y2, bool exclude_source_point = true);
};
#include "homer_navigation/narrow_passage_detection/geometry.h"
int Point::squaredDistanceTo(const Point &b) const {
const auto d_x = b.x - x;
const auto d_y = b.y - y;
return d_x * d_x + d_y * d_y;
int Point::squaredDistanceTo(const Point& b) const
{
const auto d_x = b.x - x;
const auto d_y = b.y - y;
return d_x * d_x + d_y * d_y;
}
double Line::squaredDistanceTo(const Point &p) const {
const auto d_x = x1 - x0;
const auto d_y = y1 - y0;
const auto v0 = d_y * p.x - d_x * p.y + x1 * y0 - y1 * x0;
const auto v1 = d_x * d_x + d_y * d_y;
return static_cast<double>(v0) / v1;
double Line::squaredDistanceTo(const Point& p) const
{
const auto d_x = x1 - x0;
const auto d_y = y1 - y0;
const auto v0 = d_y * p.x - d_x * p.y + x1 * y0 - y1 * x0;
const auto v1 = d_x * d_x + d_y * d_y;
return static_cast<double>(v0) / v1;
}
int Line::squaredLength() const {
const auto d_x = x1 - x0;
const auto d_y = y1 - y0;
return d_x * d_x + d_y * d_y;
int Line::squaredLength() const
{
const auto d_x = x1 - x0;
const auto d_y = y1 - y0;
return d_x * d_x + d_y * d_y;
}
double Line::squaredRadius() const {
double Line::squaredRadius() const
{
const auto d_x = static_cast<double>(x1 - x0) / 2;
const auto d_y = static_cast<double>(y1 - y0) / 2;
return d_x * d_x + d_y * d_y;
}
bool CircularArea::containsLine(const Line &l) const {
const auto p_x0 = l.x0 - c_x;
const auto p_y0 = l.y0 - c_y;
const auto p_x1 = l.x1 - c_x;
const auto p_y1 = l.y1 - c_y;
return ((p_x0 * p_x0 + p_y0 * p_y0) < r2) && ((p_x1 * p_x1 + p_y1 * p_y1) < r2);
bool CircularArea::containsLine(const Line& l) const
{
const auto p_x0 = l.x0 - c_x;
const auto p_y0 = l.y0 - c_y;
const auto p_x1 = l.x1 - c_x;
const auto p_y1 = l.y1 - c_y;
return ((p_x0 * p_x0 + p_y0 * p_y0) < r2) && ((p_x1 * p_x1 + p_y1 * p_y1) < r2);
}
#include "homer_navigation/narrow_passage_detection/grid_wrapper.h"
Point OccupancyGridWrapper::getWorldToPixel(const double x, const double y) const {
const auto cell_size = static_cast<double>(grid_.info.resolution);
const auto robot_x = x - grid_.info.origin.position.x;
const auto robot_y = y - grid_.info.origin.position.y;
const auto i_x = static_cast<int>(robot_x / cell_size);
const auto i_y = static_cast<int>(robot_y / cell_size);
return {i_x, i_y};
Point OccupancyGridWrapper::getWorldToPixel(const double x, const double y) const
{
const auto cell_size = static_cast<double>(grid_.info.resolution);
const auto robot_x = x - grid_.info.origin.position.x;
const auto robot_y = y - grid_.info.origin.position.y;
const auto i_x = static_cast<int>(robot_x / cell_size);
const auto i_y = static_cast<int>(robot_y / cell_size);
return { i_x, i_y };
}
Point OccupancyGridWrapper::getPoseCoords(const geometry_msgs::Pose &pose) const {
return getWorldToPixel(pose.position.x, pose.position.y);
Point OccupancyGridWrapper::getPoseCoords(const geometry_msgs::Pose& pose) const
{
return getWorldToPixel(pose.position.x, pose.position.y);
}
geometry_msgs::Point OccupancyGridWrapper::getPixelToWorld(const Point &pt) const {
const auto cell_size = static_cast<double>(grid_.info.resolution);
geometry_msgs::Point pt_world;
pt_world.x = pt.x * cell_size + grid_.info.origin.position.x;
pt_world.y = pt.y * cell_size + grid_.info.origin.position.y;
return pt_world;
geometry_msgs::Point OccupancyGridWrapper::getPixelToWorld(const Point& pt) const
{
const auto cell_size = static_cast<double>(grid_.info.resolution);
geometry_msgs::Point pt_world;
pt_world.x = pt.x * cell_size + grid_.info.origin.position.x;
pt_world.y = pt.y * cell_size + grid_.info.origin.position.y;
return pt_world;
}
int OccupancyGridWrapper::getCellContent(const int x, const int y) const {
if (x < 0 || y < 0 || x >= grid_width_ || y >= grid_height_)
return MAP_VOID;
const auto idx = x + y * grid_width_;
const auto v = static_cast<int>(grid_.data[idx]);
return v;
int OccupancyGridWrapper::getCellContent(const int x, const int y) const
{
if (x < 0 || y < 0 || x >= grid_width_ || y >= grid_height_)
return MAP_VOID;
const auto idx = x + y * grid_width_;
const auto v = static_cast<int>(grid_.data[idx]);
return v;
}
double OccupancyGridWrapper::getCellSize() const {
return static_cast<double>(grid_.info.resolution);
double OccupancyGridWrapper::getCellSize() const
{
return static_cast<double>(grid_.info.resolution);
}
double OccupancyGridWrapper::pixelToMeters(double pixel_length) const {
return pixel_length * getCellSize();
double OccupancyGridWrapper::pixelToMeters(double pixel_length) const
{
return pixel_length * getCellSize();
}
double OccupancyGridWrapper::metersToPixels(double meter_length) const {
return meter_length / getCellSize();
double OccupancyGridWrapper::metersToPixels(double meter_length) const
{
return meter_length / getCellSize();
}
bool OccupancyGridWrapper::isObstacleOrVoid(const int cell_content) const {
return isObstacle(cell_content) || isVoid(cell_content);
bool OccupancyGridWrapper::isObstacleOrVoid(const int cell_content) const
{
return isObstacle(cell_content) || isVoid(cell_content);
}
bool OccupancyGridWrapper::isObstacle(const int cell_content) const {
return cell_content > obstacle_threshold_;
bool OccupancyGridWrapper::isObstacle(const int cell_content) const
{
return cell_content > obstacle_threshold_;
}
constexpr bool OccupancyGridWrapper::isVoid(const int cell_content) {
return cell_content == MAP_VOID;
constexpr bool OccupancyGridWrapper::isVoid(const int cell_content)
{
return cell_content == MAP_VOID;
}
......@@ -2,55 +2,57 @@
constexpr double pi = 3.14159265358979323846;
std::vector<Point> RayCaster::getRayEndPoints(const int start_x,
const int start_y,
const int pixel_radius,
const int num_rays) {
std::vector<Point> v;
v.reserve(num_rays);
for (auto i = 0; i < num_rays; i++) {
const auto x = static_cast<int>(std::round(pixel_radius * std::cos(2 * pi * i / num_rays)));
const auto y = static_cast<int>(std::round(pixel_radius * std::sin(2 * pi * i / num_rays)));
v.emplace_back(start_x + x, start_y + y);
}
return v;
std::vector<Point> RayCaster::getRayEndPoints(
const int start_x, const int start_y, const int pixel_radius, const int num_rays)
{
std::vector<Point> v;
v.reserve(num_rays);
for (auto i = 0; i < num_rays; i++)
{
const auto x = static_cast<int>(std::round(pixel_radius * std::cos(2 * pi * i / num_rays)));
const auto y = static_cast<int>(std::round(pixel_radius * std::sin(2 * pi * i / num_rays)));
v.emplace_back(start_x + x, start_y + y);
}
return v;
}
RayCastHitResult RayCaster::castRay(const OccupancyGridWrapper &grid,
const int x1,
const int y1,
const int x2,
const int y2,
const bool exclude_source_point) {
std::vector<OccupancyGridCell> cells;
const auto dx = std::abs(x2 - x1);
const auto dy = std::abs(y2 - y1);
const auto sx = x1 < x2 ? 1 : -1;
const auto sy = y1 < y2 ? 1 : -1;
auto err = (dx > dy ? dx : -dy) / 2;
auto x = x1, y = y1;
while (true) {
if (grid.isObstacleOrVoid(grid.getCellContent(x, y))) {
if (!exclude_source_point || (x != x1 || y != y1)) {
cells.emplace_back(x, y, true);
break;
}
}
RayCastHitResult RayCaster::castRay(const OccupancyGridWrapper& grid, const int x1, const int y1,
const int x2, const int y2, const bool exclude_source_point)
{
std::vector<OccupancyGridCell> cells;
const auto dx = std::abs(x2 - x1);
const auto dy = std::abs(y2 - y1);
const auto sx = x1 < x2 ? 1 : -1;
const auto sy = y1 < y2 ? 1 : -1;
auto err = (dx > dy ? dx : -dy) / 2;
auto x = x1, y = y1;
while (true)
{
if (grid.isObstacleOrVoid(grid.getCellContent(x, y)))
{
if (!exclude_source_point || (x != x1 || y != y1))
{
cells.emplace_back(x, y, true);
break;
}
}
cells.emplace_back(x, y);
cells.emplace_back(x, y);
if (x == x2 && y == y2)
bre