Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

chore: sync upstream #295

Merged
merged 24 commits into from
Mar 4, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
24 commits
Select commit Hold shift + click to select a range
7d15fdf
perf(lane_change): only compute interpolate ego once (#2839)
zulfaqar-azmi-t4 Feb 28, 2023
ea58e59
docs(behavior_path_planner): align document title (#2956)
satoshi-ota Feb 28, 2023
102d974
feat(motion_velocity_smoother): improve velocity planning around stop…
purewater0901 Feb 28, 2023
f057084
feat(behavior_path_planner): add new manager independent from behavio…
satoshi-ota Feb 28, 2023
91653ad
refactor(obstacle_avoidance_planner): clean up the code (#2796)
takayuki5168 Mar 1, 2023
e7089d7
docs(behavior_path_planner): fix dead link (#2965)
satoshi-ota Mar 1, 2023
5f75c7d
fix(motion_velocity_smoother): fix external velocity limit if ego veh…
rej55 Mar 1, 2023
a2c62dc
fix(fault injection): add find package to cmake (#2973)
tkimura4 Mar 1, 2023
f76977a
feat(tier4_planning_rviz_plugin): move footprint plugin to path (#2971)
purewater0901 Mar 1, 2023
78388cb
feat(behavior_path_planner): be able to switch framework by macro (#2…
satoshi-ota Mar 1, 2023
f9075cb
feat(avoidance): enable to avoid vehicles that stopped for a long tim…
satoshi-ota Mar 2, 2023
1a8099a
fix(behavior_path_planner): rename macro (#2978)
satoshi-ota Mar 2, 2023
ede21f3
feat(pose_initilizer): support gnss/imu pose estimator (#2904)
rsasaki0109 Mar 2, 2023
261db5c
fix(tier4_state_rviz_plugin): fix typo (#2988)
satoshi-ota Mar 2, 2023
91d8013
fix(motion_velocity_smoother): fix terminal velocity optimization (#2…
purewater0901 Mar 2, 2023
8ce056f
chore(typo): eliminate typos (#2216)
HansRobo Mar 3, 2023
a9faeda
fix(crop_box_filter): fix bug (#2992)
sykwer Mar 3, 2023
9929414
fix(avoidance): avoidable right side parked-vehicle (#2932)
satoshi-ota Mar 3, 2023
844d210
feat(tier4_planning_rviz_plugin): add maintainer (#2996)
purewater0901 Mar 3, 2023
36fcf3f
fix(tier4_rviz_planning_plugin): clear objects before return (#2995)
purewater0901 Mar 3, 2023
7f51957
refactor(behavior_path_planner): remove unnecessary macros (#2998)
satoshi-ota Mar 3, 2023
9dd2100
refactor(motion_utils): delete calcSignedArcLength with threshold (#2…
kyoichi-sugahara Mar 3, 2023
5f8f448
feat(behavior_path_planner): pull over freespace parking (#2879)
kosuke55 Mar 3, 2023
806db52
docs(behavior_path_planner): add pull over out separated docs and upd…
kosuke55 Mar 3, 2023
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ namespace message_field_adapters
/// Using alias for Time message
using TimeStamp = builtin_interfaces::msg::Time;

/// \brief Helper class to check existance of header file in compile time:
/// \brief Helper class to check existence of header file in compile time:
/// https://stackoverflow.com/a/16000226/2325407
template <typename T, typename = nullptr_t>
struct HasHeader : std::false_type
Expand Down
1 change: 1 addition & 0 deletions common/autoware_auto_common/test/test_template_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ template <typename FooT, typename In1, typename In2>
using false_bar_expression2 =
decltype(std::declval<FooT>().bar(std::declval<In1>(), std::declval<const In2 &>()));

// cspell: ignore asdasd
// Signature mismatch:
template <typename FooT, typename In1, typename In2, typename In3>
using false_bar_expression3 = decltype(std::declval<FooT>().asdasd(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ template <typename PointT>
using Point4 = std::array<PointT, 4U>;

/// \brief Helper struct for compile time introspection of array size from
/// stackoverflow.com/questions/16866033/getting-the-number-of-elements-in-stdarray-at-compile-time
/// Ref: https://stackoverflow.com/questions/16866033
template <typename>
struct array_size;
template <typename T, std::size_t N>
Expand All @@ -132,7 +132,7 @@ finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box);

/// \brief given support points and two orthogonal directions, compute corners of bounding box
/// \tparam PointT Type of a point, must have float members x and y`
/// \tparam IT An iterator dereferencable into PointT
/// \tparam IT An iterator dereferenceable into PointT
/// \param[out] corners Gets filled with corner points of bounding box
/// \param[in] supports Iterators referring to support points of current bounding box
/// e.g. bounding box is touching these points
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
/// \brief This file implements 2D pca on a linked list of points to estimate an oriented
/// bounding box

// cspell: ignore eigenbox, EIGENBOX
#ifndef GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_
#define GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_

Expand Down Expand Up @@ -50,11 +51,12 @@ struct Covariance2d
std::size_t num_points;
}; // struct Covariance2d

// cspell: ignore Welford
/// \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm
/// \param[in] begin An iterator pointing to the first point in a point list
/// \param[in] end An iterator pointing to one past the last point in the point list
/// \tparam IT An iterator type dereferencable into a point with float members x and y
/// \return A 2d covariance matrix for all points inthe list
/// \tparam IT An iterator type dereferenceable into a point with float members x and y
/// \return A 2d covariance matrix for all points in the list
template <typename IT>
Covariance2d covariance_2d(const IT begin, const IT end)
{
Expand Down Expand Up @@ -93,13 +95,14 @@ Covariance2d covariance_2d(const IT begin, const IT end)

/// \brief Compute eigenvectors and eigenvalues
/// \param[in] cov 2d Covariance matrix
/// \param[out] eigvec1 First eigenvector
/// \param[out] eigvec2 Second eigenvector
/// \param[out] eig_vec1 First eigenvector
/// \param[out] eig_vec2 Second eigenvector
/// \tparam PointT Point type that has at least float members x and y
/// \return A pairt of eigenvalues: The first is the larger eigenvalue
/// \throw std::runtime error if you would get degenerate covariance
template <typename PointT>
std::pair<float32_t, float32_t> eig_2d(const Covariance2d & cov, PointT & eigvec1, PointT & eigvec2)
std::pair<float32_t, float32_t> eig_2d(
const Covariance2d & cov, PointT & eig_vec1, PointT & eig_vec2)
{
const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F;
const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy);
Expand All @@ -120,28 +123,28 @@ std::pair<float32_t, float32_t> eig_2d(const Covariance2d & cov, PointT & eigvec
// are persistent against further calculations.
// (e.g. taking cross product of two eigen vectors)
if (fabsf(cov.xy * cov.xy) > std::numeric_limits<float32_t>::epsilon()) {
xr_(eigvec1) = cov.xy;
yr_(eigvec1) = ret.first - cov.xx;
xr_(eigvec2) = cov.xy;
yr_(eigvec2) = ret.second - cov.xx;
xr_(eig_vec1) = cov.xy;
yr_(eig_vec1) = ret.first - cov.xx;
xr_(eig_vec2) = cov.xy;
yr_(eig_vec2) = ret.second - cov.xx;
} else {
if (cov.xx > cov.yy) {
xr_(eigvec1) = 1.0F;
yr_(eigvec1) = 0.0F;
xr_(eigvec2) = 0.0F;
yr_(eigvec2) = 1.0F;
xr_(eig_vec1) = 1.0F;
yr_(eig_vec1) = 0.0F;
xr_(eig_vec2) = 0.0F;
yr_(eig_vec2) = 1.0F;
} else {
xr_(eigvec1) = 0.0F;
yr_(eigvec1) = 1.0F;
xr_(eigvec2) = 1.0F;
yr_(eigvec2) = 0.0F;
xr_(eig_vec1) = 0.0F;
yr_(eig_vec1) = 1.0F;
xr_(eig_vec2) = 1.0F;
yr_(eig_vec2) = 0.0F;
}
}
return ret;
}

/// \brief Given eigenvectors, compute support (furthest) point in each direction
/// \tparam IT An iterator type dereferencable into a point with float members x and y
/// \tparam IT An iterator type dereferenceable into a point with float members x and y
/// \tparam PointT type of a point with float members x and y
/// \param[in] begin An iterator pointing to the first point in a point list
/// \param[in] end An iterator pointing to one past the last point in the point list
Expand Down Expand Up @@ -183,7 +186,7 @@ bool8_t compute_supports(
}

/// \brief Compute bounding box given a pair of basis directions
/// \tparam IT An iterator type dereferencable into a point with float members x and y
/// \tparam IT An iterator type dereferenceable into a point with float members x and y
/// \tparam PointT Point type of the lists, must have float members x and y
/// \param[in] ax1 First basis direction, assumed to be normal to ax2
/// \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1
Expand All @@ -210,7 +213,7 @@ BoundingBox compute_bounding_box(
/// modify the list. The resulting bounding box is not necessarily minimum in any way
/// \param[in] begin An iterator pointing to the first point in a point list
/// \param[in] end An iterator pointing to one past the last point in the point list
/// \tparam IT An iterator type dereferencable into a point with float members x and y
/// \tparam IT An iterator type dereferenceable into a point with float members x and y
/// \return An oriented bounding box in x-y. This bounding box has no height information
template <typename IT>
BoundingBox eigenbox_2d(const IT begin, const IT end)
Expand All @@ -222,7 +225,7 @@ BoundingBox eigenbox_2d(const IT begin, const IT end)
using PointT = details::base_type<decltype(*begin)>;
PointT eig1;
PointT eig2;
const auto eigv = details::eig_2d(cov, eig1, eig2);
const auto eig_v = details::eig_2d(cov, eig1, eig2);

// find extreme points
details::Point4<IT> supports;
Expand All @@ -232,7 +235,7 @@ BoundingBox eigenbox_2d(const IT begin, const IT end)
std::swap(eig1, eig2);
}
BoundingBox bbox = details::compute_bounding_box(eig1, eig2, supports);
bbox.value = eigv.first;
bbox.value = eig_v.first;

return bbox;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@
/// \brief This file implements 2D pca on a linked list of points to estimate an oriented
/// bounding box

// cspell: ignore LFIT, lfit
// LFIT means "L-Shape Fitting"
#ifndef GEOMETRY__BOUNDING_BOX__LFIT_HPP_
#define GEOMETRY__BOUNDING_BOX__LFIT_HPP_

Expand Down Expand Up @@ -117,8 +119,8 @@ float32_t solve_lfit(const LFitWs & ws, PointT & dir)
ws.m22d - (((ws.m12b * ws.m12b) * pi) + ((ws.m12d * ws.m12d) * qi)),
ws.m22b - (((ws.m12a * ws.m12b) * pi) + ((ws.m12c * ws.m12d) * qi)), 0UL};
PointT eig1;
const auto eigv = eig_2d(M, eig1, dir);
return eigv.second;
const auto eig_v = eig_2d(M, eig1, dir);
return eig_v.second;
}

/// \brief Increments L fit M matrix with information in the point
Expand Down Expand Up @@ -176,7 +178,7 @@ class LFitCompare
/// \param[in] end An iterator pointing to one past the last point in the point list
/// \param[in] size The number of points in the point list
/// \return A bounding box that minimizes the LFit residual
/// \tparam IT An iterator type dereferencable into a point with float members x and y
/// \tparam IT An iterator type dereferenceable into a point with float members x and y
template <typename IT>
BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size)
{
Expand Down Expand Up @@ -207,11 +209,11 @@ BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::s
}
}
// can recover best corner point, but don't care, need to cover all points
const auto inorm = 1.0F / norm_2d(best_normal);
if (!std::isnormal(inorm)) {
const auto inv_norm = 1.0F / norm_2d(best_normal);
if (!std::isnormal(inv_norm)) {
throw std::runtime_error{"LFit: Abnormal norm"};
}
best_normal = times_2d(best_normal, inorm);
best_normal = times_2d(best_normal, inv_norm);
auto best_tangent = get_normal(best_normal);
// find extreme points
Point4<IT> supports;
Expand All @@ -235,7 +237,7 @@ BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::s
/// \param[in] hint An iterator pointing to the point whose normal will be used to sort the list
/// \return A pair where the first element is an iterator pointing to the nearest point, and the
/// second element is the size of the list
/// \tparam IT An iterator type dereferencable into a point with float members x and y
/// \tparam IT An iterator type dereferenceable into a point with float members x and y
/// \throw std::domain_error If the number of points is too few
template <typename IT, typename PointT>
BoundingBox lfit_bounding_box_2d(
Expand All @@ -258,7 +260,7 @@ BoundingBox lfit_bounding_box_2d(
/// \return An oriented bounding box in x-y. This bounding box has no height information
/// \param[in] begin An iterator pointing to the first point in a point list
/// \param[in] end An iterator pointing to one past the last point in the point list
/// \tparam IT An iterator type dereferencable into a point with float members x and y
/// \tparam IT An iterator type dereferenceable into a point with float members x and y
template <typename IT>
BoundingBox lfit_bounding_box_2d(const IT begin, const IT end)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ void init_bbox(const IT begin, const IT end, Point4<IT> & support)
/// \param[in] end An iterator to one past the last point on a convex hull
/// \param[in] metric_fn A functor determining what measure the bounding box is minimum with respect
/// to
/// \tparam IT An iterator type dereferencable into a point type with float members x and y
/// \tparam IT An iterator type dereferenceable into a point type with float members x and y
/// \tparam MetricF A functor that computes a float measure from the x and y size (width and length)
/// of a bounding box, assumed to be packed in a Point32 message.
/// \throw std::domain_error if the list of points is too small to reasonable generate a bounding
Expand Down Expand Up @@ -223,7 +223,7 @@ BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF m
/// \param[in] begin An iterator to the first point on a convex hull
/// \param[in] end An iterator to one past the last point on a convex hull
/// \return A minimum area bounding box, value field is the area
/// \tparam IT An iterator type dereferencable into a point type with float members x and y
/// \tparam IT An iterator type dereferenceable into a point type with float members x and y
template <typename IT>
BoundingBox minimum_area_bounding_box(const IT begin, const IT end)
{
Expand All @@ -238,7 +238,7 @@ BoundingBox minimum_area_bounding_box(const IT begin, const IT end)
/// \param[in] begin An iterator to the first point on a convex hull
/// \param[in] end An iterator to one past the last point on a convex hull
/// \return A minimum perimeter bounding box, value field is half the perimeter
/// \tparam IT An iterator type dereferencable into a point type with float members x and y
/// \tparam IT An iterator type dereferenceable into a point type with float members x and y
template <typename IT>
BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end)
{
Expand Down
9 changes: 5 additions & 4 deletions common/autoware_auto_geometry/include/geometry/common_2d.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -262,7 +262,6 @@ T times_2d(const T & p, const float32_t a)
/// \tparam T point type. Must have point adapters defined or have float members x and y
/// \brief solve p + t * u = q + s * v
/// Ref: https://stackoverflow.com/questions/563198/
/// whats-the-most-efficent-way-to-calculate-where-two-line-segments-intersect
/// \param[in] pt anchor point of first line
/// \param[in] u direction of first line
/// \param[in] q anchor point of second line
Expand All @@ -274,6 +273,8 @@ inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v)
{
const float32_t num = cross_2d(minus_2d(pt, q), u);
float32_t den = cross_2d(v, u);
// cspell: ignore FEPS
// FEPS means "Float EPSilon"
constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
if (fabsf(den) < FEPS) {
if (fabsf(num) < FEPS) {
Expand All @@ -292,7 +293,7 @@ inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v)
/// \brief rotate point given precomputed sin and cos
/// \param[inout] pt point to rotate
/// \param[in] cos_th precomputed cosine value
/// \param[in] sin_th precompined sine value
/// \param[in] sin_th precomputed sine value
template <typename T>
inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th)
{
Expand Down Expand Up @@ -321,7 +322,7 @@ inline T rotate_2d(const T & pt, const float32_t th_rad)
/// \brief compute q s.t. p T q, or p * q = 0
/// This is the equivalent of a 90 degree ccw rotation
/// \param[in] pt point to get normal point of
/// \return point normal to p (unnormalized)
/// \return point normal to p (un-normalized)
template <typename T>
inline T get_normal(const T & pt)
{
Expand All @@ -334,7 +335,7 @@ inline T get_normal(const T & pt)
/// \tparam T point type. Must have point adapters defined or have float members x and y
/// \brief get magnitude of x and y components:
/// \param[in] pt point to get magnitude of
/// \return magitude of x and y components together
/// \return magnitude of x and y components together
template <typename T>
inline auto norm_2d(const T & pt)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -136,6 +136,8 @@ typename std::list<PointT>::const_iterator convex_hull_impl(std::list<PointT> &
const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t {
using point_adapter::x_;
using point_adapter::y_;
// cspell: ignore FEPS
// FEPS means "Float EPSilon"
constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
return (fabsf(x_(a) - x_(b)) > FEPS) ? (x_(a) < x_(b)) : (y_(a) < y_(b));
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ std::vector<Line> get_sorted_face_list(const Iter start, const Iter end)
return face_list;
}

/// \brief Append points of the polygon `internal` that are contained in the polygon `exernal`.
/// \brief Append points of the polygon `internal` that are contained in the polygon `external`.
template <
template <typename...> class Iterable1T, template <typename...> class Iterable2T, typename PointT>
void append_contained_points(
Expand Down Expand Up @@ -147,6 +147,9 @@ void append_intersection_points(
std::min(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second)),
std::max(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second))};

// cspell: ignore feps
// feps means "Float EPSilon"

// The accumulated floating point error depends on the magnitudes of each end of the
// intervals. Hence the upper bound of the absolute magnitude should be taken into account
// while computing the epsilon.
Expand Down Expand Up @@ -274,7 +277,7 @@ std::list<PointT> convex_polygon_intersection2d(
/// \param polygon1 A convex polygon
/// \param polygon2 A convex polygon
/// \return (Intersection / Union) between two given polygons.
/// \throws std::domain_error If there is any inconsistency on the undderlying geometrical
/// \throws std::domain_error If there is any inconsistency on the underlying geometrical
/// computation.
template <
template <typename...> class Iterable1T, template <typename...> class Iterable2T, typename PointT>
Expand Down
6 changes: 3 additions & 3 deletions common/autoware_auto_geometry/include/geometry/interval.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -256,8 +256,8 @@ bool Interval<T>::abs_eq(const Interval & i1, const Interval & i2, const T & eps
const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2);

const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps);
const auto maxs_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps);
const auto both_non_empty_equal = both_non_empty && mins_equal && maxs_equal;
const auto maxes_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps);
const auto both_non_empty_equal = both_non_empty && mins_equal && maxes_equal;

return both_empty || both_non_empty_equal;
}
Expand Down Expand Up @@ -286,7 +286,7 @@ bool Interval<T>::bounds_valid(const Interval & i)
{
const auto is_ordered = (Interval::min(i) <= Interval::max(i));

// Check for emptiness expicitly because it requires both bounds to be NaN
// Check for emptiness explicitly because it requires both bounds to be NaN
return Interval::empty(i) || is_ordered;
}

Expand Down
Loading