Classes | |
struct | AngularPoint |
struct | IndexedDouble |
struct | Point2 |
Functions | |
geometry_msgs::Polygon | centeredPolygon (const geometry_msgs::Polygon &polygon) |
geometry_msgs::Polygon | centeredPolygon (const geometry_msgs::Polygon &polygon, double &dx, double &dy) |
void | centerPolygon (geometry_msgs::Polygon &polygon) |
void | centerPolygon (geometry_msgs::Polygon &polygon, double &dx, double &dy) |
int32_t | circular_index (int32_t index, size_t size) |
PlaceProfile | closedPlaceProfile (const PlaceProfile &profile, double max_frontier_width) |
void | closePlaceProfile (PlaceProfile &profile, double max_frontier_width) |
PlaceProfile | costmapToPlaceProfile (const nav_msgs::OccupancyGrid &map, double range_cutoff=0) |
PlaceProfile | curtailedPlaceProfile (const PlaceProfile &profile, double max_distance) |
void | curtailPlaceProfile (PlaceProfile &profile, double max_distance) |
double | distance (Point32 point) |
bool | firstNonFree (const nav_msgs::OccupancyGrid &map, double angle, double range_cutoff, geometry_msgs::Point32 &point) |
visualization_msgs::Marker | getCrossingCenterMarker (const std::string &frame_id, const lama_msgs::Crossing &crossing) |
visualization_msgs::Marker | getFrontiersMarker (const std::string &frame_id, const lama_msgs::Crossing &crossing) |
template<typename T > | |
double | getLength (const vector< T > &pts) |
template<typename T > | |
vector< double > | getLengths (const vector< T > &pts) |
double | getRelevance (const Point32 &pi, const Point32 &pj, const Point32 &pk) |
double | getRelevance (const std::list< IndexedDouble > &l, std::list< IndexedDouble >::const_iterator it, const vector< Point32 > &points) |
template<class ForwardIt > | |
bool | is_sorted (ForwardIt first, ForwardIt last) |
bool | isClosed (const PlaceProfile &profile, double max_frontier_width) |
bool | isClosed (const geometry_msgs::Polygon &poly, double max_frontier_width) |
PlaceProfile | laserScanToPlaceProfile (const sensor_msgs::LaserScan &scan, double range_cutoff) |
PlaceProfile | loadFromFile (const std::string &filename) |
static bool | normalizablePolygon (const vector< AngularPoint > &angular_points) |
PlaceProfile | normalizedPlaceProfile (const PlaceProfile &profile, bool sort=true) |
geometry_msgs::Polygon | normalizedPolygon (const geometry_msgs::Polygon &poly, bool &normalized) |
void | normalizeExcludeSegments (PlaceProfile &profile) |
void | normalizePlaceProfile (PlaceProfile &profile, bool sort=true) |
bool | normalizePolygon (geometry_msgs::Polygon &poly) |
sensor_msgs::PointCloud | placeProfileToPointCloud (const PlaceProfile &profile) |
geometry_msgs::Polygon | placeProfileToPolygon (const PlaceProfile &profile) |
testing::AssertionResult | pointClose (const Point32 &a, const Point32 &b, double tolerance) |
template<typename T > | |
double | pointDistance2 (const T &pa, const T &pb) |
template<typename T > | |
double | pointDistanceSquared2 (const T &pa, const T &pb) |
testing::AssertionResult | pointEqual (const Point32 &a, const Point32 &b) |
bool | pointInCCWOrder (const vector< AngularPoint > &angular_points) |
bool | pointOccupied (const nav_msgs::OccupancyGrid &map, const int index) |
testing::AssertionResult | pointsClose (const vector< Point32 > &a, const vector< Point32 > &b, double tolerance) |
testing::AssertionResult | pointsEqual (const vector< Point32 > &a, const vector< Point32 > &b) |
bool | pointUnknown (const nav_msgs::OccupancyGrid &map, const int index) |
testing::AssertionResult | polygonClose (const geometry_msgs::Polygon &a, const geometry_msgs::Polygon &b, double tolerance) |
testing::AssertionResult | polygonEqual (const geometry_msgs::Polygon &a, const geometry_msgs::Polygon &b) |
PlaceProfile | profile1 () |
PlaceProfile | profile10 () |
PlaceProfile | profile2 () |
PlaceProfile | profile3 () |
PlaceProfile | profile6 () |
PlaceProfile | profile7 () |
PlaceProfile | profile_circle_ccw () |
PlaceProfile | profile_circle_cw () |
PlaceProfile | profile_circle_growing () |
testing::AssertionResult | profileIsClosed (const PlaceProfile &profile, double max_frontier_width) |
template<typename T > | |
vector< T > | resamplePolygon (const vector< T > &polygon, unsigned int sample_count, double &delta) |
template<> | |
vector< Point32 > | resamplePolygon< Point32 > (const vector< Point32 > &polygon, unsigned int sample_count, double &delta) |
void | rotateCrossing (lama_msgs::Crossing &crossing, const double angle) |
LaserScan | scan4ccw () |
LaserScan | scan4cw () |
LaserScan | scan8cw () |
template<typename T > | |
vector< T > | scanToPolygon (const sensor_msgs::LaserScan &scan) |
PlaceProfile | shift_profile_index (const PlaceProfile &profile, size_t shift) |
PlaceProfile | simplifiedPlaceProfile (const PlaceProfile &profile, double min_relevance) |
vector< Point32 > | simplifyPath (const vector< Point32 > &points, size_t begin, size_t end, double min_relevance) |
void | simplifyPlaceProfile (PlaceProfile &profile, double min_relevance) |
TEST (TestSuite, testCenterPolygon) | |
TEST (TestSuite, testNormalizePlaceProfile) | |
TEST (TestSuite, testNormalizablePolygon) | |
TEST (TestSuite, testNormalizePolygon) | |
TEST (TestSuite, testClosePlaceProfile) | |
TEST (TestSuite, testClosedPlaceProfile) | |
TEST (TestSuite, testSimplifyPath) | |
TEST (TestSuite, testSimplifyPlaceProfile) | |
TEST (TestSuite, testRealDataSimplify) | |
TEST (TestSuite, testCurtailPlaceProfile) | |
TEST (TestSuite, testLaserScanToPlaceProfile) | |
std::vector< AngularPoint > | toAngularPoints (const geometry_msgs::Polygon &polygon) |
std::vector< AngularPoint > | toAngularPoints (const lama_msgs::PlaceProfile &profile) |
void | translate_profile (PlaceProfile &profile, double dx, double dy) |
geometry_msgs::Polygon lama_common::centeredPolygon | ( | const geometry_msgs::Polygon & | polygon | ) |
Return a copy of the polygon centered about its center of mass.
[in] | polygon | Input polygon. |
Definition at line 60 of file polygon_utils.cpp.
geometry_msgs::Polygon lama_common::centeredPolygon | ( | const geometry_msgs::Polygon & | polygon, |
double & | dx, | ||
double & | dy | ||
) |
Return a copy of the polygon centered about its center of mass.
[in] | polygon | Input polygon. |
[out] | dx | Applied translation in x-direction. |
[out] | dy | Applied translation in y-direction. |
Definition at line 74 of file polygon_utils.cpp.
void lama_common::centerPolygon | ( | geometry_msgs::Polygon & | polygon | ) |
Center a polygon about its center of mass.
Definition at line 14 of file polygon_utils.cpp.
void lama_common::centerPolygon | ( | geometry_msgs::Polygon & | polygon, |
double & | dx, | ||
double & | dy | ||
) |
Center a polygon about its center of mass.
[in] | polygon | Polygon to center. |
[out] | dx | Applied translation in x-direction. |
[out] | dy | Applied translation in y-direction. |
Definition at line 27 of file polygon_utils.cpp.
int32_t lama_common::circular_index | ( | int32_t | index, |
size_t | size | ||
) | [inline] |
Return a positive modulo (as Python does, even with negative numbers)
index must be greater than (-size).
Definition at line 31 of file place_profile_conversions.h.
PlaceProfile lama_common::closedPlaceProfile | ( | const PlaceProfile & | profile, |
double | max_frontier_width | ||
) |
Return a PlaceProfile where the largest frontier will be bounded by adding some points.
Add some points to the returned PlaceProfile so that the distance between two consecutive points will be at most max_frontier_width. The polygon will have no excluded segments.
profile[in] PlaceProfile max_frontier_width[in] maximum frontier width
Definition at line 253 of file place_profile_utils.cpp.
void lama_common::closePlaceProfile | ( | PlaceProfile & | profile, |
double | max_frontier_width | ||
) |
Modify a PlaceProfile in place, so that the largest frontier will be bounded.
Add some points to the PlaceProfile message so that the distance between two consecutive points will be at most max_frontier_width. The polygon will have no excluded segments.
profile[out] PlaceProfile max_frontier_width[in] maximum frontier width
Definition at line 200 of file place_profile_utils.cpp.
PlaceProfile lama_common::costmapToPlaceProfile | ( | const nav_msgs::OccupancyGrid & | map, |
double | range_cutoff | ||
) |
Return a PlaceProfile from an occupancy grid.
The range limit is the map border or range_cutoff, which ever comes first.
Definition at line 197 of file place_profile_conversions.cpp.
PlaceProfile lama_common::curtailedPlaceProfile | ( | const PlaceProfile & | profile, |
double | max_distance | ||
) |
Return a PlaceProfile only containing points withing a certain distance.
profile[in] PlaceProfile max_distance[in] range cutoff
Definition at line 587 of file place_profile_utils.cpp.
void lama_common::curtailPlaceProfile | ( | PlaceProfile & | profile, |
double | max_distance | ||
) |
Remove points farther than a certain distance.
profile[out] PlaceProfile max_distance[in] range cutoff
Definition at line 537 of file place_profile_utils.cpp.
double lama_common::distance | ( | Point32 | point | ) | [inline] |
bool lama_common::firstNonFree | ( | const nav_msgs::OccupancyGrid & | map, |
double | angle, | ||
double | range_cutoff, | ||
geometry_msgs::Point32 & | point | ||
) |
Return the first non-free point from map center to map border with given angle.
Return the first non-free point (i.e. first occupied or unknown point), calculated by raytracing from the map center to the map border with the Bresenham algorithm and stopping at the first encountered occupied or unknown point. If the ray is free over the distance range_cutoff, the point will be at this distance and the returned value will be false (i.e. obstacle free).
[in] | map | An occupancy grid. |
[in] | angle | The ray angle. |
[in] | range_cutoff | The max ray length, unused if 0. |
[out] | point | The first non-free point. |
Definition at line 150 of file place_profile_conversions.cpp.
visualization_msgs::Marker lama_common::getCrossingCenterMarker | ( | const std::string & | frame_id, |
const lama_msgs::Crossing & | crossing | ||
) |
Return the marker for the visualization of the crossing center
Definition at line 8 of file crossing_visualization.cpp.
visualization_msgs::Marker lama_common::getFrontiersMarker | ( | const std::string & | frame_id, |
const lama_msgs::Crossing & | crossing | ||
) |
Return the marker for the visualization of the roads
Definition at line 29 of file crossing_visualization.cpp.
double lama_common::getLength | ( | const vector< T > & | pts | ) |
Return the length of a given polygon.
Definition at line 24 of file polygon_utils.h.
vector<double> lama_common::getLengths | ( | const vector< T > & | pts | ) |
For each segment in polygon, return its length polygon: p0, p1,.., pn result: l0, l1,.., ln l0 = lenght of (p0, p1), l1 = length(p1,p2), ... , ln = length(pn, p0)
Definition at line 44 of file polygon_utils.h.
double lama_common::getRelevance | ( | const Point32 & | pi, |
const Point32 & | pj, | ||
const Point32 & | pk | ||
) | [inline] |
Return the relevance of 3 points.
Return |AB| + |BC| - |AC| for points A, B, and C, where |x| is the length of x, point B should be more or less between A and C. Relevance is 0 for colinear points and the farther B from (AC), the greater.
Definition at line 314 of file place_profile_utils.cpp.
double lama_common::getRelevance | ( | const std::list< IndexedDouble > & | l, |
std::list< IndexedDouble >::const_iterator | it, | ||
const vector< Point32 > & | points | ||
) |
Return the relevance of point in points with index it->index with its predecessor and successor
l[in] list from which to get the original indexes. it[in] iterator to the adequate element of l. points[int] list of points with original indexing.
Definition at line 337 of file place_profile_utils.cpp.
bool lama_common::is_sorted | ( | ForwardIt | first, |
ForwardIt | last | ||
) | [inline] |
Checks if the elements in range [first, last) are sorted in ascending order
is_sorted is in STL since C++11. copied from http://en.cppreference.com/w/cpp/algorithm/is_sorted and http://en.cppreference.com/w/cpp/algorithm/is_sorted_until.
Definition at line 15 of file is_sorted.h.
bool lama_common::isClosed | ( | const PlaceProfile & | profile, |
double | max_frontier_width | ||
) |
Return true is a profile has no holes larger than max_frontier_width
Holes can be an excluded segment or not.
Definition at line 169 of file place_profile_utils.cpp.
bool lama_common::isClosed | ( | const geometry_msgs::Polygon & | poly, |
double | max_frontier_width | ||
) |
Return true is a polygon has no holes larger than max_frontier_width
Definition at line 191 of file polygon_utils.cpp.
PlaceProfile lama_common::laserScanToPlaceProfile | ( | const sensor_msgs::LaserScan & | scan, |
double | range_cutoff | ||
) |
Tranform a LaserScan into a PlaceProfile
The laser beams greater than range_cutoff are removed. However, the first and the last beam of a series of beams larger than range_cutoff are kept and their length is reduced to range_cutoff.
Definition at line 43 of file place_profile_conversions.cpp.
PlaceProfile lama_common::loadFromFile | ( | const std::string & | filename | ) |
static bool lama_common::normalizablePolygon | ( | const vector< AngularPoint > & | angular_points | ) | [static] |
Return true if the polygon is normalizable.
Return true if the angles are growing or decreasing homogeneously (one jump allowed, because of angle ciclicity).
A strict copy of ../src/polygon_utils.cpp::normalizablePolygon.
Definition at line 86 of file polygon_utils.cpp.
PlaceProfile lama_common::normalizedPlaceProfile | ( | const PlaceProfile & | profile, |
bool | sort | ||
) |
Return a copy of a PlaceProfile message where point angles are sorted.
Angles within [-pi,pi[ will be sorted.
profile[in] PlaceProfile sort[in] if true, the polygon points will be sorted according to their angle.
Definition at line 158 of file place_profile_utils.cpp.
geometry_msgs::Polygon lama_common::normalizedPolygon | ( | const geometry_msgs::Polygon & | poly, |
bool & | normalized | ||
) |
Return a copy of a Polygon message where point angles are sorted.
Angles within [-pi,pi[ will be sorted.
[in] | poly | The polygon on which the returned polygon is based. |
[out] | normalized | true if the polygon was normalized, false if it was not normalizable. |
Definition at line 180 of file polygon_utils.cpp.
void lama_common::normalizeExcludeSegments | ( | PlaceProfile & | profile | ) |
Remove out-of-bound exclude_segments and doubles in exclude_segments.
Definition at line 8 of file place_profile_utils.cpp.
void lama_common::normalizePlaceProfile | ( | PlaceProfile & | profile, |
bool | sort | ||
) |
Change point order in place so that point angles are sorted.
Angles within [-pi,pi[ will be sorted from smallest to greatest (i.e. counterclock-wise), optionally. The considered angle is from the center of mass to the point. invalid exclude_segments will be removed. exclude_segments will be sorted from smallest index to greatest. exclude_segments will contain no doubles.
The algorithm is not robust against non-simple polygons.
[in] | profile | PlaceProfile |
[in] | sort | If true, the polygon points will be sorted according to their angle. If false, the output PlaceProfile is not guaranteed to be normalized. Defaults to true. |
Definition at line 99 of file place_profile_utils.cpp.
bool lama_common::normalizePolygon | ( | geometry_msgs::Polygon & | poly | ) |
Change point order in place so that point angles are sorted.
Angles within [-pi,pi[ will be sorted from smallest to greatest (i.e. counterclock-wise). The considered angle is from the center of mass to the point.
The algorithm is not robust against non-simple polygons.
[in] | poly | The polygon to sort. |
Definition at line 136 of file polygon_utils.cpp.
sensor_msgs::PointCloud lama_common::placeProfileToPointCloud | ( | const PlaceProfile & | profile | ) |
Return the point cloud corresponding to the PlaceProfile
Definition at line 27 of file place_profile_conversions.cpp.
geometry_msgs::Polygon lama_common::placeProfileToPolygon | ( | const PlaceProfile & | profile | ) |
Return the polygon corresponding to the PlaceProfile
Definition at line 11 of file place_profile_conversions.cpp.
testing::AssertionResult lama_common::pointClose | ( | const Point32 & | a, |
const Point32 & | b, | ||
double | tolerance | ||
) |
double lama_common::pointDistance2 | ( | const T & | pa, |
const T & | pb | ||
) |
double lama_common::pointDistanceSquared2 | ( | const T & | pa, |
const T & | pb | ||
) |
testing::AssertionResult lama_common::pointEqual | ( | const Point32 & | a, |
const Point32 & | b | ||
) |
bool lama_common::pointInCCWOrder | ( | const vector< AngularPoint > & | angular_points | ) | [inline] |
Return true if points are in counterclockwise order
Definition at line 69 of file place_profile_utils.cpp.
bool lama_common::pointOccupied | ( | const nav_msgs::OccupancyGrid & | map, |
const int | index | ||
) | [inline] |
Return true if the map point is occupied.
Definition at line 121 of file place_profile_conversions.cpp.
testing::AssertionResult lama_common::pointsClose | ( | const vector< Point32 > & | a, |
const vector< Point32 > & | b, | ||
double | tolerance | ||
) |
testing::AssertionResult lama_common::pointsEqual | ( | const vector< Point32 > & | a, |
const vector< Point32 > & | b | ||
) |
bool lama_common::pointUnknown | ( | const nav_msgs::OccupancyGrid & | map, |
const int | index | ||
) | [inline] |
Return true if the map point is unknown.
Definition at line 128 of file place_profile_conversions.cpp.
testing::AssertionResult lama_common::polygonClose | ( | const geometry_msgs::Polygon & | a, |
const geometry_msgs::Polygon & | b, | ||
double | tolerance | ||
) |
testing::AssertionResult lama_common::polygonEqual | ( | const geometry_msgs::Polygon & | a, |
const geometry_msgs::Polygon & | b | ||
) |
PlaceProfile lama_common::profile1 | ( | ) |
PlaceProfile lama_common::profile10 | ( | ) |
PlaceProfile lama_common::profile2 | ( | ) |
PlaceProfile lama_common::profile3 | ( | ) |
PlaceProfile lama_common::profile6 | ( | ) |
PlaceProfile lama_common::profile7 | ( | ) |
PlaceProfile lama_common::profile_circle_ccw | ( | ) |
PlaceProfile lama_common::profile_circle_cw | ( | ) |
PlaceProfile lama_common::profile_circle_growing | ( | ) |
testing::AssertionResult lama_common::profileIsClosed | ( | const PlaceProfile & | profile, |
double | max_frontier_width | ||
) |
vector<T> lama_common::resamplePolygon | ( | const vector< T > & | polygon, |
unsigned int | sample_count, | ||
double & | delta | ||
) |
Resample the given polygon
Resample the given polygon of 2D or 3D points. The third coordinate of 3D points is ignored. T must have a constructor T(double, double).
[in] | polygon | Polygon to be resampled. |
[in] | sample_count | Number of points in the returned polygon. |
[out] | delta | Average distance between two consecutive points in the output polygon. |
Definition at line 69 of file polygon_utils.h.
vector<Point32> lama_common::resamplePolygon< Point32 > | ( | const vector< Point32 > & | polygon, |
unsigned int | sample_count, | ||
double & | delta | ||
) | [inline] |
Resample the given polygon.
Specialization for geometry_msgs::Point32 that doesn't have a constructor T(double, double).
[in] | polygon | Polygon to be resampled. |
[in] | sample_count | Number of points in the returned polygon. |
[out] | delta | Average distance between two consecutive points in the output polygon. |
Definition at line 115 of file polygon_utils.h.
void lama_common::rotateCrossing | ( | lama_msgs::Crossing & | crossing, |
const double | angle | ||
) |
Apply a rotation to a crossing
Definition at line 8 of file crossing_utils.cpp.
LaserScan lama_common::scan4ccw | ( | ) |
LaserScan lama_common::scan4cw | ( | ) |
LaserScan lama_common::scan8cw | ( | ) |
geometry_msgs::Polygon lama_common::scanToPolygon | ( | const sensor_msgs::LaserScan & | scan | ) | [inline] |
Return a list of points from a LaserScan.
T must have a constructor T(double, double).
Return a geometry_msgs::Polygon from a LaserScan.
Definition at line 144 of file polygon_utils.h.
PlaceProfile lama_common::shift_profile_index | ( | const PlaceProfile & | profile, |
size_t | shift | ||
) |
PlaceProfile lama_common::simplifiedPlaceProfile | ( | const PlaceProfile & | profile, |
double | min_relevance | ||
) |
Return a PlaceProfile with reduced number of points.
A relevance filter is used.
profile[in] PlaceProfile min_relevance[in] relevance threshold, points with relevance smaller than this are removed.
Definition at line 525 of file place_profile_utils.cpp.
vector< Point32 > lama_common::simplifyPath | ( | const vector< Point32 > & | points, |
size_t | begin, | ||
size_t | end, | ||
double | min_relevance | ||
) |
Reduce the number of points in a series of points
A relevance filter is used.
points[in] list of points. begin[in] index of first point to consider. end[in] index of point after the last point to consider. min_relevance[in] relevance threshold, points with relevance smaller than this are removed.
Definition at line 362 of file place_profile_utils.cpp.
void lama_common::simplifyPlaceProfile | ( | PlaceProfile & | profile, |
double | min_relevance | ||
) |
Reduce the number of points in a PlaceProfile message
A relevance filter is used for points which are not at borders of excluded segments. All points with relevance smaller than the threshold are removed.
profile[in] PlaceProfile min_relevance[in] relevance threshold, points with relevance smaller than this are removed.
Definition at line 477 of file place_profile_utils.cpp.
lama_common::TEST | ( | TestSuite | , |
testCenterPolygon | |||
) |
lama_common::TEST | ( | TestSuite | , |
testNormalizePlaceProfile | |||
) |
lama_common::TEST | ( | TestSuite | , |
testNormalizablePolygon | |||
) |
lama_common::TEST | ( | TestSuite | , |
testNormalizePolygon | |||
) |
lama_common::TEST | ( | TestSuite | , |
testClosePlaceProfile | |||
) |
lama_common::TEST | ( | TestSuite | , |
testClosedPlaceProfile | |||
) |
lama_common::TEST | ( | TestSuite | , |
testSimplifyPath | |||
) |
lama_common::TEST | ( | TestSuite | , |
testSimplifyPlaceProfile | |||
) |
lama_common::TEST | ( | TestSuite | , |
testRealDataSimplify | |||
) |
lama_common::TEST | ( | TestSuite | , |
testCurtailPlaceProfile | |||
) |
lama_common::TEST | ( | TestSuite | , |
testLaserScanToPlaceProfile | |||
) |
std::vector<AngularPoint> lama_common::toAngularPoints | ( | const geometry_msgs::Polygon & | polygon | ) | [inline] |
Definition at line 31 of file angular_point.h.
std::vector<AngularPoint> lama_common::toAngularPoints | ( | const lama_msgs::PlaceProfile & | profile | ) | [inline] |
Definition at line 43 of file angular_point.h.
void lama_common::translate_profile | ( | PlaceProfile & | profile, |
double | dx, | ||
double | dy | ||
) |