diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 733a842865802..f8273cebdd578 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -94,4 +94,4 @@ repos:
args: [--quiet]
exclude: .cu
-exclude: .svg|control/trajectory_follower|control/trajectory_follower_nodes|common/autoware_auto_cmake|common/autoware_auto_common|common/autoware_auto_geometry|common/autoware_auto_tf2|common/autoware_testing|common/fake_test_node|common/had_map_utils|common/motion_common|common/motion_testing|common/time_utils|common/vehicle_constants_manager|common/autoware_auto_perception_rviz_plugin|common/osqp_interface|simulator/simple_planning_simulator|planning/freespace_planner|planning/astar_search|planning/costmap_generator
+exclude: .svg
diff --git a/common/autoware_auto_cmake/design/autoware_auto_cmake-design.md b/common/autoware_auto_cmake/design/autoware_auto_cmake-design.md
index 62c82d6dd7db2..8946a02f8d951 100644
--- a/common/autoware_auto_cmake/design/autoware_auto_cmake-design.md
+++ b/common/autoware_auto_cmake/design/autoware_auto_cmake-design.md
@@ -1,10 +1,8 @@
-autoware_auto_cmake {#autoware-auto-cmake-design}
-===========
+# autoware_auto_cmake {#autoware-auto-cmake-design}
This is the design document for the `autoware_auto_cmake` package.
-
-# Purpose
+## Purpose
Provide common CMake variables and functions to Autoware packages.
@@ -13,17 +11,17 @@ Those include:
- Setting the language standard
- Getting user-provided variables
- Providing functions to:
- + set compiler flags
- + turn off optimizations
+ - set compiler flags
+ - turn off optimizations
-# Design
+## Design
-## Usage
+### Usage
Add `autoware_auto_cmake` as a "build_depend" in the dependent packages.
-### CMake variables {#cmake-config-variables}
+#### CMake variables {#cmake-config-variables}
-|Name|Type|Descritpion|Default|
-|----|----|-----------|-------|
-|`DOWNLOAD_ARTIFACTS`|*BOOL*|Allow downloading artifacts at build time.|`OFF`|
+| Name | Type | Descritpion | Default |
+| -------------------- | ------ | ------------------------------------------ | ------- |
+| `DOWNLOAD_ARTIFACTS` | _BOOL_ | Allow downloading artifacts at build time. | `OFF` |
diff --git a/common/autoware_auto_cmake/package.xml b/common/autoware_auto_cmake/package.xml
index 6d8163eae6018..6c3df541c4095 100644
--- a/common/autoware_auto_cmake/package.xml
+++ b/common/autoware_auto_cmake/package.xml
@@ -12,11 +12,11 @@
<build_depend>ros_environment</build_depend>
- <buildtool_export_depend>ament_cmake_core</buildtool_export_depend>
- <buildtool_export_depend>ament_cmake_lint_cmake</buildtool_export_depend>
<buildtool_export_depend>ament_cmake_copyright</buildtool_export_depend>
+ <buildtool_export_depend>ament_cmake_core</buildtool_export_depend>
<buildtool_export_depend>ament_cmake_cppcheck</buildtool_export_depend>
<buildtool_export_depend>ament_cmake_cpplint</buildtool_export_depend>
+ <buildtool_export_depend>ament_cmake_lint_cmake</buildtool_export_depend>
<buildtool_export_depend>ament_cmake_uncrustify</buildtool_export_depend>
<test_depend>ament_cmake_lint_cmake</test_depend>
diff --git a/common/autoware_auto_common/CMakeLists.txt b/common/autoware_auto_common/CMakeLists.txt
index 45284843f5644..54407c7a0facf 100644
--- a/common/autoware_auto_common/CMakeLists.txt
+++ b/common/autoware_auto_common/CMakeLists.txt
@@ -32,7 +32,6 @@ if(BUILD_TESTING)
# Temporarily disable cpplint and uncrustify
list(APPEND AMENT_LINT_AUTO_EXCLUDE
ament_cmake_cpplint
- ament_cmake_uncrustify
)
ament_lint_auto_find_test_dependencies()
@@ -46,10 +45,6 @@ if(BUILD_TESTING)
find_package(ament_cmake_cpplint)
ament_cpplint(${FILES_MINUS_SOME})
- # Re-enable uncrustify
- find_package(ament_cmake_uncrustify)
- ament_uncrustify(${FILES_MINUS_SOME})
-
# Unit tests
set(TEST_COMMON test_common_gtest)
ament_add_gtest(${TEST_COMMON}
diff --git a/common/autoware_auto_common/design/comparisons.md b/common/autoware_auto_common/design/comparisons.md
index 6e1d2eb9db35e..d8c1ef6de17e8 100644
--- a/common/autoware_auto_common/design/comparisons.md
+++ b/common/autoware_auto_common/design/comparisons.md
@@ -1,5 +1,4 @@
-Comparisons {#helper-comparisons}
-===========
+# Comparisons {#helper-comparisons}
The `float_comparisons.hpp` library is a simple set of functions for performing approximate numerical comparisons.
There are separate functions for performing comparisons using absolute bounds and relative bounds. Absolute comparison checks are prefixed with `abs_` and relative checks are prefixed with `rel_`.
@@ -8,19 +7,19 @@ The `bool_comparisons.hpp` library additionally contains an XOR operator.
The intent of the library is to improve readability of code and reduce likelihood of typographical errors when using numerical and boolean comparisons.
-# Target use cases
+## Target use cases
The approximate comparisons are intended to be used to check whether two numbers lie within some absolute or relative interval.
The `exclusive_or` function will test whether two values cast to different boolean values.
-# Assumptions
+## Assumptions
-* The approximate comparisons all take an `epsilon` parameter.
-The value of this parameter must be >= 0.
-* The library is only intended to be used with floating point types.
-A static assertion will be thrown if the library is used with a non-floating point type.
+- The approximate comparisons all take an `epsilon` parameter.
+ The value of this parameter must be >= 0.
+- The library is only intended to be used with floating point types.
+ A static assertion will be thrown if the library is used with a non-floating point type.
-# Example Usage
+## Example Usage
```c++
#include "common/bool_comparisons.hpp"
diff --git a/common/autoware_auto_common/include/common/type_traits.hpp b/common/autoware_auto_common/include/common/type_traits.hpp
index ed0ab1709d92c..44dfe92681017 100644
--- a/common/autoware_auto_common/include/common/type_traits.hpp
+++ b/common/autoware_auto_common/include/common/type_traits.hpp
@@ -41,28 +41,32 @@ namespace type_traits
///
/// @return A boolean that should be false for any type passed into this function.
///
-template<typename T>
+template <typename T>
constexpr inline autoware::common::types::bool8_t COMMON_PUBLIC impossible_branch() noexcept
{
return sizeof(T) == 0;
}
/// Find an index of a type in a tuple
-template<class QueryT, class TupleT>
+template <class QueryT, class TupleT>
struct COMMON_PUBLIC index
{
static_assert(!std::is_same<TupleT, std::tuple<>>::value, "Could not find QueryT in given tuple");
};
/// Specialization for a tuple that starts with the HeadT type. End of recursion.
-template<class HeadT, class ... Tail>
+template <class HeadT, class... Tail>
struct COMMON_PUBLIC index<HeadT, std::tuple<HeadT, Tail...>>
- : std::integral_constant<std::int32_t, 0> {};
+: std::integral_constant<std::int32_t, 0>
+{
+};
/// Specialization for a tuple with a type different to QueryT that calls the recursive step.
-template<class QueryT, class HeadT, class ... Tail>
+template <class QueryT, class HeadT, class... Tail>
struct COMMON_PUBLIC index<QueryT, std::tuple<HeadT, Tail...>>
- : std::integral_constant<std::int32_t, 1 + index<QueryT, std::tuple<Tail...>>::value> {};
+: std::integral_constant<std::int32_t, 1 + index<QueryT, std::tuple<Tail...>>::value>
+{
+};
///
/// @brief Visit every element in a tuple.
@@ -75,13 +79,17 @@ struct COMMON_PUBLIC index<QueryT, std::tuple<HeadT, Tail...>>
///
/// @return Does not return anything. Capture variables in a lambda to return any values.
///
-template<std::size_t I = 0UL, typename Callable, typename ... TypesT>
-COMMON_PUBLIC inline constexpr typename std::enable_if_t<I == sizeof...(TypesT)>
-visit(std::tuple<TypesT...> &, Callable) noexcept {}
+template <std::size_t I = 0UL, typename Callable, typename... TypesT>
+COMMON_PUBLIC inline constexpr typename std::enable_if_t<I == sizeof...(TypesT)> visit(
+ std::tuple<TypesT...> &, Callable) noexcept
+{
+}
/// @brief Same as the previous specialization but for const tuple.
-template<std::size_t I = 0UL, typename Callable, typename ... TypesT>
-COMMON_PUBLIC inline constexpr typename std::enable_if_t<I == sizeof...(TypesT)>
-visit(const std::tuple<TypesT...> &, Callable) noexcept {}
+template <std::size_t I = 0UL, typename Callable, typename... TypesT>
+COMMON_PUBLIC inline constexpr typename std::enable_if_t<I == sizeof...(TypesT)> visit(
+ const std::tuple<TypesT...> &, Callable) noexcept
+{
+}
///
/// @brief Visit every element in a tuple.
@@ -98,32 +106,37 @@ visit(const std::tuple<TypesT...> &, Callable) noexcept {}
///
/// @return Does not return anything. Capture variables in a lambda to return any values.
///
-template<std::size_t I = 0UL, typename Callable, typename ... TypesT>
-COMMON_PUBLIC inline constexpr typename std::enable_if_t<I != sizeof...(TypesT)>
-visit(std::tuple<TypesT...> & tuple, Callable callable) noexcept
+template <std::size_t I = 0UL, typename Callable, typename... TypesT>
+COMMON_PUBLIC inline constexpr typename std::enable_if_t<I != sizeof...(TypesT)> visit(
+ std::tuple<TypesT...> & tuple, Callable callable) noexcept
{
callable(std::get<I>(tuple));
visit<I + 1UL, Callable, TypesT...>(tuple, callable);
}
/// @brief Same as the previous specialization but for const tuple.
-template<std::size_t I = 0UL, typename Callable, typename ... TypesT>
-COMMON_PUBLIC inline constexpr typename std::enable_if_t<I != sizeof...(TypesT)>
-visit(const std::tuple<TypesT...> & tuple, Callable callable) noexcept
+template <std::size_t I = 0UL, typename Callable, typename... TypesT>
+COMMON_PUBLIC inline constexpr typename std::enable_if_t<I != sizeof...(TypesT)> visit(
+ const std::tuple<TypesT...> & tuple, Callable callable) noexcept
{
callable(std::get<I>(tuple));
visit<I + 1UL, Callable, TypesT...>(tuple, callable);
}
/// @brief A class to compute a conjunction over given traits.
-template<class ...>
-struct COMMON_PUBLIC conjunction : std::true_type {};
+template <class...>
+struct COMMON_PUBLIC conjunction : std::true_type
+{
+};
/// @brief A conjunction of another type shall derive from that type.
-template<class TraitT>
-struct COMMON_PUBLIC conjunction<TraitT>: TraitT {};
-template<class TraitT, class ... TraitsTs>
+template <class TraitT>
+struct COMMON_PUBLIC conjunction<TraitT> : TraitT
+{
+};
+template <class TraitT, class... TraitsTs>
struct COMMON_PUBLIC conjunction<TraitT, TraitsTs...>
- : std::conditional_t<static_cast<bool>(TraitT::value), conjunction<TraitsTs...>, TraitT> {};
-
+: std::conditional_t<static_cast<bool>(TraitT::value), conjunction<TraitsTs...>, TraitT>
+{
+};
///
/// @brief A trait to check if a tuple has a type.
@@ -133,7 +146,7 @@ struct COMMON_PUBLIC conjunction<TraitT, TraitsTs...>
/// @tparam QueryT A query type.
/// @tparam TupleT A tuple to search the type in.
///
-template<typename QueryT, typename TupleT>
+template <typename QueryT, typename TupleT>
struct has_type;
///
@@ -142,8 +155,10 @@ struct has_type;
///
/// @tparam QueryT Any type.
///
-template<typename QueryT>
-struct has_type<QueryT, std::tuple<>>: std::false_type {};
+template <typename QueryT>
+struct has_type<QueryT, std::tuple<>> : std::false_type
+{
+};
///
/// @brief Recursive override of the main trait.
@@ -152,8 +167,10 @@ struct has_type<QueryT, std::tuple<>>: std::false_type {};
/// @tparam HeadT Head type in the tuple.
/// @tparam TailTs Rest of the tuple types.
///
-template<typename QueryT, typename HeadT, typename ... TailTs>
-struct has_type<QueryT, std::tuple<HeadT, TailTs...>>: has_type<QueryT, std::tuple<TailTs...>> {};
+template <typename QueryT, typename HeadT, typename... TailTs>
+struct has_type<QueryT, std::tuple<HeadT, TailTs...>> : has_type<QueryT, std::tuple<TailTs...>>
+{
+};
///
/// @brief End of recursion for the main `has_type` trait. Becomes a `true_type` when the first
@@ -162,9 +179,10 @@ struct has_type<QueryT, std::tuple<HeadT, TailTs...>>: has_type<QueryT, std::tup
/// @tparam QueryT Query type.
/// @tparam TailTs Other types in the tuple.
///
-template<typename QueryT, typename ... TailTs>
-struct has_type<QueryT, std::tuple<QueryT, TailTs...>>: std::true_type {};
-
+template <typename QueryT, typename... TailTs>
+struct has_type<QueryT, std::tuple<QueryT, TailTs...>> : std::true_type
+{
+};
///
/// @brief A trait used to intersect types stored in tuples at compile time. The resulting
@@ -176,7 +194,7 @@ struct has_type<QueryT, std::tuple<QueryT, TailTs...>>: std::true_type {};
/// @tparam TupleT1 Tuple 1
/// @tparam TupleT2 Tuple 2
///
-template<typename TupleT1, typename TupleT2>
+template <typename TupleT1, typename TupleT2>
struct intersect
{
///
@@ -185,18 +203,16 @@ struct intersect
/// @details This function "iterates" over the types in TupleT1 and checks if those are in
/// TupleT2. If this is true, these types are concatenated into a new tuple.
///
- template<std::size_t... Indices>
+ template <std::size_t... Indices>
static constexpr auto make_intersection(std::index_sequence<Indices...>)
{
- return std::tuple_cat(
- std::conditional_t<
- has_type<std::tuple_element_t<Indices, TupleT1>, TupleT2>::value,
- std::tuple<std::tuple_element_t<Indices, TupleT1>>,
- std::tuple<>>{} ...);
+ return std::tuple_cat(std::conditional_t<
+ has_type<std::tuple_element_t<Indices, TupleT1>, TupleT2>::value,
+ std::tuple<std::tuple_element_t<Indices, TupleT1>>, std::tuple<>>{}...);
}
/// The resulting tuple type.
using type =
- decltype(make_intersection(std::make_index_sequence<std::tuple_size<TupleT1>::value> {}));
+ decltype(make_intersection(std::make_index_sequence<std::tuple_size<TupleT1>::value>{}));
};
} // namespace type_traits
diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/common/types.hpp
index f3d49fcea04ff..5c77449e30ff5 100644
--- a/common/autoware_auto_common/include/common/types.hpp
+++ b/common/autoware_auto_common/include/common/types.hpp
@@ -19,12 +19,12 @@
#ifndef COMMON__TYPES_HPP_
#define COMMON__TYPES_HPP_
+#include "common/visibility_control.hpp"
+#include "helper_functions/float_comparisons.hpp"
+
#include <cstdint>
-#include <vector>
#include <limits>
-
-#include "helper_functions/float_comparisons.hpp"
-#include "common/visibility_control.hpp"
+#include <vector>
namespace autoware
{
@@ -61,16 +61,12 @@ struct COMMON_PUBLIC PointXYZIF
float32_t intensity{0};
uint16_t id{0};
static constexpr uint16_t END_OF_SCAN_ID = 65535u;
- friend bool operator==(
- const PointXYZIF & p1,
- const PointXYZIF & p2) noexcept
+ friend bool operator==(const PointXYZIF & p1, const PointXYZIF & p2) noexcept
{
using autoware::common::helper_functions::comparisons::rel_eq;
const auto epsilon = std::numeric_limits<float32_t>::epsilon();
- return rel_eq(p1.x, p2.x, epsilon) &&
- rel_eq(p1.y, p2.y, epsilon) &&
- rel_eq(p1.z, p2.z, epsilon) &&
- rel_eq(p1.intensity, p2.intensity, epsilon) &&
+ return rel_eq(p1.x, p2.x, epsilon) && rel_eq(p1.y, p2.y, epsilon) &&
+ rel_eq(p1.z, p2.z, epsilon) && rel_eq(p1.intensity, p2.intensity, epsilon) &&
(p1.id == p2.id);
}
};
@@ -82,16 +78,12 @@ struct COMMON_PUBLIC PointXYZF
float32_t z{0};
uint16_t id{0};
static constexpr uint16_t END_OF_SCAN_ID = 65535u;
- friend bool operator==(
- const PointXYZF & p1,
- const PointXYZF & p2) noexcept
+ friend bool operator==(const PointXYZF & p1, const PointXYZF & p2) noexcept
{
using autoware::common::helper_functions::comparisons::rel_eq;
const auto epsilon = std::numeric_limits<float32_t>::epsilon();
- return rel_eq(p1.x, p2.x, epsilon) &&
- rel_eq(p1.y, p2.y, epsilon) &&
- rel_eq(p1.z, p2.z, epsilon) &&
- (p1.id == p2.id);
+ return rel_eq(p1.x, p2.x, epsilon) && rel_eq(p1.y, p2.y, epsilon) &&
+ rel_eq(p1.z, p2.z, epsilon) && (p1.id == p2.id);
}
};
@@ -101,22 +93,19 @@ struct COMMON_PUBLIC PointXYZI
float32_t y{0.0F};
float32_t z{0.0F};
float32_t intensity{0.0F};
- friend bool operator==(
- const PointXYZI & p1,
- const PointXYZI & p2) noexcept
+ friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept
{
- return
- helper_functions::comparisons::rel_eq(
- p1.x, p2.x, std::numeric_limits<float32_t>::epsilon()) &&
+ return helper_functions::comparisons::rel_eq(
+ p1.x, p2.x, std::numeric_limits<float32_t>::epsilon()) &&
- helper_functions::comparisons::rel_eq(
- p1.y, p2.y, std::numeric_limits<float32_t>::epsilon()) &&
+ helper_functions::comparisons::rel_eq(
+ p1.y, p2.y, std::numeric_limits<float32_t>::epsilon()) &&
- helper_functions::comparisons::rel_eq(
- p1.z, p2.z, std::numeric_limits<float32_t>::epsilon()) &&
+ helper_functions::comparisons::rel_eq(
+ p1.z, p2.z, std::numeric_limits<float32_t>::epsilon()) &&
- helper_functions::comparisons::rel_eq(
- p1.intensity, p2.intensity, std::numeric_limits<float32_t>::epsilon());
+ helper_functions::comparisons::rel_eq(
+ p1.intensity, p2.intensity, std::numeric_limits<float32_t>::epsilon());
}
};
@@ -127,7 +116,7 @@ static constexpr uint16_t POINT_BLOCK_CAPACITY = 512U;
// TODO(yunus.caliskan): switch to std::void_t when C++17 is available
/// \brief `std::void_t<> implementation
-template<typename ... Ts>
+template <typename... Ts>
using void_t = void;
} // namespace types
} // namespace common
diff --git a/common/autoware_auto_common/include/common/visibility_control.hpp b/common/autoware_auto_common/include/common/visibility_control.hpp
index d2d3d89148d03..0a988d6407dfa 100644
--- a/common/autoware_auto_common/include/common/visibility_control.hpp
+++ b/common/autoware_auto_common/include/common/visibility_control.hpp
@@ -18,21 +18,21 @@
#define COMMON__VISIBILITY_CONTROL_HPP_
#if defined(_MSC_VER) && defined(_WIN64)
- #if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS)
- #define COMMON_PUBLIC __declspec(dllexport)
- #define COMMON_LOCAL
- #else // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS)
- #define COMMON_PUBLIC __declspec(dllimport)
- #define COMMON_LOCAL
- #endif // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS)
+#if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS)
+#define COMMON_PUBLIC __declspec(dllexport)
+#define COMMON_LOCAL
+#else // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS)
+#define COMMON_PUBLIC __declspec(dllimport)
+#define COMMON_LOCAL
+#endif // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS)
#elif defined(__GNUC__) && defined(__linux__)
- #define COMMON_PUBLIC __attribute__((visibility("default")))
- #define COMMON_LOCAL __attribute__((visibility("hidden")))
+#define COMMON_PUBLIC __attribute__((visibility("default")))
+#define COMMON_LOCAL __attribute__((visibility("hidden")))
#elif defined(__GNUC__) && defined(__APPLE__)
- #define COMMON_PUBLIC __attribute__((visibility("default")))
- #define COMMON_LOCAL __attribute__((visibility("hidden")))
+#define COMMON_PUBLIC __attribute__((visibility("default")))
+#define COMMON_LOCAL __attribute__((visibility("hidden")))
#else // !(defined(__GNUC__) && defined(__APPLE__))
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif // _MSC_VER
#endif // COMMON__VISIBILITY_CONTROL_HPP_
diff --git a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp b/common/autoware_auto_common/include/helper_functions/angle_utils.hpp
index 39ee0bf657748..31a33ef2dab47 100644
--- a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp
+++ b/common/autoware_auto_common/include/helper_functions/angle_utils.hpp
@@ -46,12 +46,16 @@ constexpr auto kDoublePi = 2.0 * M_PI;
///
/// @return Angle wrapped to the chosen range.
///
-template<typename T>
+template <typename T>
constexpr T wrap_angle(T angle) noexcept
{
auto help_angle = angle + T(M_PI);
- while (help_angle < T{}) {help_angle += T(detail::kDoublePi);}
- while (help_angle >= T(detail::kDoublePi)) {help_angle -= T(detail::kDoublePi);}
+ while (help_angle < T{}) {
+ help_angle += T(detail::kDoublePi);
+ }
+ while (help_angle >= T(detail::kDoublePi)) {
+ help_angle -= T(detail::kDoublePi);
+ }
return help_angle - T(M_PI);
}
diff --git a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp b/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp
index dda752bf2f2b5..c6bf09365af4f 100644
--- a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp
+++ b/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp
@@ -36,7 +36,7 @@ namespace comparisons
* @brief Convenience method for performing logical exclusive or ops.
* @return True iff exactly one of 'a' and 'b' is true.
*/
-template<typename T>
+template <typename T>
types::bool8_t exclusive_or(const T & a, const T & b)
{
return static_cast<types::bool8_t>(a) != static_cast<types::bool8_t>(b);
diff --git a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp b/common/autoware_auto_common/include/helper_functions/byte_reader.hpp
index aa05e5c6a7be4..bef27ea310e26 100644
--- a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp
+++ b/common/autoware_auto_common/include/helper_functions/byte_reader.hpp
@@ -40,14 +40,13 @@ class ByteReader
/// \brief Default constructor, byte reader class
/// \param[in] byte_vector A vector to read bytes from
explicit ByteReader(const std::vector<uint8_t> & byte_vector)
- : byte_vector_(byte_vector),
- index_(0U)
+ : byte_vector_(byte_vector), index_(0U)
{
}
// brief Read bytes and store it in the argument passed in big-endian order
/// \param[inout] value Read and store the bytes from the vector matching the size of the argument
- template<typename T>
+ template <typename T>
void read(T & value)
{
constexpr std::size_t kTypeSize = sizeof(T);
@@ -65,10 +64,7 @@ class ByteReader
index_ += kTypeSize;
}
- void skip(std::size_t count)
- {
- index_ += count;
- }
+ void skip(std::size_t count) { index_ += count; }
};
} // namespace helper_functions
} // namespace common
diff --git a/common/autoware_auto_common/include/helper_functions/crtp.hpp b/common/autoware_auto_common/include/helper_functions/crtp.hpp
index 7cba8d0350829..707d522251dc2 100644
--- a/common/autoware_auto_common/include/helper_functions/crtp.hpp
+++ b/common/autoware_auto_common/include/helper_functions/crtp.hpp
@@ -25,7 +25,7 @@ namespace common
{
namespace helper_functions
{
-template<typename Derived>
+template <typename Derived>
class crtp
{
protected:
@@ -33,7 +33,7 @@ class crtp
{
// This is the CRTP pattern for static polymorphism: this is related, static_cast is the only
// way to do this
- //lint -e{9005, 9176, 1939} NOLINT
+ // lint -e{9005, 9176, 1939} NOLINT
return *static_cast<const Derived *>(this);
}
@@ -41,7 +41,7 @@ class crtp
{
// This is the CRTP pattern for static polymorphism: this is related, static_cast is the only
// way to do this
- //lint -e{9005, 9176, 1939} NOLINT
+ // lint -e{9005, 9176, 1939} NOLINT
return *static_cast<Derived *>(this);
}
};
diff --git a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp b/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp
index fcf1b90c59cf2..de1f459f21d0a 100644
--- a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp
+++ b/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp
@@ -39,12 +39,11 @@ namespace comparisons
* @pre eps >= 0
* @return True iff 'a' and 'b' are within 'eps' of each other.
*/
-template<typename T>
+template <typename T>
bool abs_eq(const T & a, const T & b, const T & eps)
{
static_assert(
- std::is_floating_point<T>::value,
- "Float comparisons only support floating point types.");
+ std::is_floating_point<T>::value, "Float comparisons only support floating point types.");
return std::abs(a - b) <= eps;
}
@@ -54,7 +53,7 @@ bool abs_eq(const T & a, const T & b, const T & eps)
* @pre eps >= 0
* @return True iff 'a' is less than 'b' minus 'eps'.
*/
-template<typename T>
+template <typename T>
bool abs_lt(const T & a, const T & b, const T & eps)
{
return !abs_eq(a, b, eps) && (a < b);
@@ -65,7 +64,7 @@ bool abs_lt(const T & a, const T & b, const T & eps)
* @pre eps >= 0
* @return True iff 'a' is less than or equal to 'b' plus 'eps'.
*/
-template<typename T>
+template <typename T>
bool abs_lte(const T & a, const T & b, const T & eps)
{
return abs_eq(a, b, eps) || (a < b);
@@ -76,7 +75,7 @@ bool abs_lte(const T & a, const T & b, const T & eps)
* @pre eps >= 0
* @return True iff 'a' is greater than or equal to 'b' minus 'eps'.
*/
-template<typename T>
+template <typename T>
bool abs_gte(const T & a, const T & b, const T & eps)
{
return !abs_lt(a, b, eps);
@@ -87,7 +86,7 @@ bool abs_gte(const T & a, const T & b, const T & eps)
* @pre eps >= 0
* @return True iff 'a' is greater than 'b' minus 'eps'.
*/
-template<typename T>
+template <typename T>
bool abs_gt(const T & a, const T & b, const T & eps)
{
return !abs_lte(a, b, eps);
@@ -98,7 +97,7 @@ bool abs_gt(const T & a, const T & b, const T & eps)
* @pre eps >= 0
* @return True iff 'a' is within 'eps' of zero.
*/
-template<typename T>
+template <typename T>
bool abs_eq_zero(const T & a, const T & eps)
{
return abs_eq(a, static_cast<T>(0), eps);
@@ -110,12 +109,11 @@ bool abs_eq_zero(const T & a, const T & eps)
* @pre rel_eps >= 0
* @return True iff 'a' and 'b' are within relative 'rel_eps' of each other.
*/
-template<typename T>
+template <typename T>
bool rel_eq(const T & a, const T & b, const T & rel_eps)
{
static_assert(
- std::is_floating_point<T>::value,
- "Float comparisons only support floating point types.");
+ std::is_floating_point<T>::value, "Float comparisons only support floating point types.");
const auto delta = std::abs(a - b);
const auto larger = std::max(std::abs(a), std::abs(b));
@@ -135,7 +133,7 @@ bool rel_eq(const T & a, const T & b, const T & rel_eps)
* @pre rel_eps >= 0
* @return True iff 'a' and 'b' are within 'eps' or 'rel_eps' of each other
*/
-template<typename T>
+template <typename T>
bool approx_eq(const T & a, const T & b, const T & abs_eps, const T & rel_eps)
{
const auto are_absolute_eq = abs_eq(a, b, abs_eps);
diff --git a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp b/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp
index fde5efca90a23..dcfa65880fdd7 100644
--- a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp
+++ b/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp
@@ -32,10 +32,9 @@ namespace helper_functions
/// \param mean Single column matrix containing mean of samples received so far
/// \param covariance_factor Covariance matrix
/// \return Square of mahalanobis distance
-template<typename T, std::int32_t kNumOfStates>
+template <typename T, std::int32_t kNumOfStates>
types::float32_t calculate_squared_mahalanobis_distance(
- const Eigen::Matrix<T, kNumOfStates, 1> & sample,
- const Eigen::Matrix<T, kNumOfStates, 1> & mean,
+ const Eigen::Matrix<T, kNumOfStates, 1> & sample, const Eigen::Matrix<T, kNumOfStates, 1> & mean,
const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor)
{
using Vector = Eigen::Matrix<T, kNumOfStates, 1>;
@@ -59,12 +58,10 @@ types::float32_t calculate_squared_mahalanobis_distance(
/// \param mean Single column matrix containing mean of samples received so far
/// \param covariance_factor Covariance matrix
/// \return Mahalanobis distance
-template<typename T, std::int32_t kNumOfStates>
+template <typename T, std::int32_t kNumOfStates>
types::float32_t calculate_mahalanobis_distance(
- const Eigen::Matrix<T, kNumOfStates, 1> & sample,
- const Eigen::Matrix<T, kNumOfStates, 1> & mean,
- const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor
-)
+ const Eigen::Matrix<T, kNumOfStates, 1> & sample, const Eigen::Matrix<T, kNumOfStates, 1> & mean,
+ const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor)
{
return sqrtf(calculate_squared_mahalanobis_distance(sample, mean, covariance_factor));
}
diff --git a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/helper_functions/message_adapters.hpp
index 90156b152fdd1..fc813f7d5107b 100644
--- a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp
+++ b/common/autoware_auto_common/include/helper_functions/message_adapters.hpp
@@ -20,6 +20,7 @@
#define HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_
#include <builtin_interfaces/msg/time.hpp>
+
#include <string>
namespace autoware
@@ -35,11 +36,15 @@ using TimeStamp = builtin_interfaces::msg::Time;
/// \brief Helper class to check existance of header file in compile time:
/// https://stackoverflow.com/a/16000226/2325407
-template<typename T, typename = nullptr_t>
-struct HasHeader : std::false_type {};
+template <typename T, typename = nullptr_t>
+struct HasHeader : std::false_type
+{
+};
-template<typename T>
-struct HasHeader<T, decltype((void) T::header, nullptr)>: std::true_type {};
+template <typename T>
+struct HasHeader<T, decltype((void)T::header, nullptr)> : std::true_type
+{
+};
/////////// Template declarations
@@ -48,7 +53,7 @@ struct HasHeader<T, decltype((void) T::header, nullptr)>: std::true_type {};
/// \tparam T Message type.
/// \param msg Message.
/// \return Frame id of the message.
-template<typename T, nullptr_t>
+template <typename T, nullptr_t>
const std::string & get_frame_id(const T & msg) noexcept;
/// Get a reference to the frame id from message. nullptr_t is used to prevent
@@ -57,7 +62,7 @@ const std::string & get_frame_id(const T & msg) noexcept;
/// \tparam T Message type.
/// \param msg Message.
/// \return Frame id of the message.
-template<typename T, nullptr_t>
+template <typename T, nullptr_t>
std::string & get_frame_id(T & msg) noexcept;
/// Get stamp from message. nullptr_t is used to prevent template ambiguity on
@@ -65,7 +70,7 @@ std::string & get_frame_id(T & msg) noexcept;
/// \tparam T Message type.
/// \param msg Message.
/// \return Frame id of the message.
-template<typename T, nullptr_t>
+template <typename T, nullptr_t>
const TimeStamp & get_stamp(const T & msg) noexcept;
/// Get a reference to the stamp from message. nullptr_t is used to prevent
@@ -74,30 +79,29 @@ const TimeStamp & get_stamp(const T & msg) noexcept;
/// \tparam T Message type.
/// \param msg Message.
/// \return Frame id of the message.
-template<typename T, nullptr_t>
+template <typename T, nullptr_t>
TimeStamp & get_stamp(T & msg) noexcept;
-
/////////////// Default specializations for message types that contain a header.
-template<class T, typename std::enable_if<HasHeader<T>::value, nullptr_t>::type = nullptr>
+template <class T, typename std::enable_if<HasHeader<T>::value, nullptr_t>::type = nullptr>
const std::string & get_frame_id(const T & msg) noexcept
{
return msg.header.frame_id;
}
-template<class T, typename std::enable_if<HasHeader<T>::value, nullptr_t>::type = nullptr>
+template <class T, typename std::enable_if<HasHeader<T>::value, nullptr_t>::type = nullptr>
std::string & get_frame_id(T & msg) noexcept
{
return msg.header.frame_id;
}
-template<class T, typename std::enable_if<HasHeader<T>::value, nullptr_t>::type = nullptr>
+template <class T, typename std::enable_if<HasHeader<T>::value, nullptr_t>::type = nullptr>
TimeStamp & get_stamp(T & msg) noexcept
{
return msg.header.stamp;
}
-template<class T, typename std::enable_if<HasHeader<T>::value, nullptr_t>::type = nullptr>
+template <class T, typename std::enable_if<HasHeader<T>::value, nullptr_t>::type = nullptr>
TimeStamp get_stamp(const T & msg) noexcept
{
return msg.header.stamp;
diff --git a/common/autoware_auto_common/include/helper_functions/template_utils.hpp b/common/autoware_auto_common/include/helper_functions/template_utils.hpp
index b52be9eae0c59..25a1da4134660 100644
--- a/common/autoware_auto_common/include/helper_functions/template_utils.hpp
+++ b/common/autoware_auto_common/include/helper_functions/template_utils.hpp
@@ -18,6 +18,7 @@
#define HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_
#include <common/types.hpp>
+
#include <type_traits>
namespace autoware
@@ -30,36 +31,43 @@ namespace helper_functions
/// `std::false_type` otherwise.
/// \tparam ExpressionTemplate Expression to be checked in compile time
/// \tparam T Template parameter to instantiate the expression.
-template<template<typename ...> class ExpressionTemplate, typename T, typename = void>
-struct expression_valid : std::false_type {};
+template <template <typename...> class ExpressionTemplate, typename T, typename = void>
+struct expression_valid : std::false_type
+{
+};
/// This struct is `std::true_type` if the expression is valid for a given template and
/// `std::false_type` otherwise.
/// \tparam ExpressionTemplate Expression to be checked in compile time
/// \tparam T Template parameter to instantiate the expression.
-template<template<typename ...> class ExpressionTemplate, typename T>
-struct expression_valid<ExpressionTemplate, T,
- types::void_t<ExpressionTemplate<T>>>: std::true_type {};
+template <template <typename...> class ExpressionTemplate, typename T>
+struct expression_valid<ExpressionTemplate, T, types::void_t<ExpressionTemplate<T>>>
+: std::true_type
+{
+};
/// This struct is `std::true_type` if the expression is valid for a given template
/// type with the specified return type and `std::false_type` otherwise.
/// \tparam ExpressionTemplate Expression to be checked in compile time
/// \tparam T Template parameter to instantiate the expression.
/// \tparam ReturnT Return type of the expression.
-template<template<typename ...> class ExpressionTemplate, typename T, typename ReturnT,
- typename = void>
-struct expression_valid_with_return : std::false_type {};
+template <
+ template <typename...> class ExpressionTemplate, typename T, typename ReturnT, typename = void>
+struct expression_valid_with_return : std::false_type
+{
+};
/// This struct is `std::true_type` if the expression is valid for a given template
/// type with the specified return type and `std::false_type` otherwise.
/// \tparam ExpressionTemplate Expression to be checked in compile time
/// \tparam T Template parameter to instantiate the expression.
/// \tparam ReturnT Return type of the expression.
-template<template<typename ...> class ExpressionTemplate, typename T, typename ReturnT>
-struct expression_valid_with_return<ExpressionTemplate, T, ReturnT,
- std::enable_if_t<std::is_same<ReturnT,
- ExpressionTemplate<T>>::value>>: std::true_type {};
-
+template <template <typename...> class ExpressionTemplate, typename T, typename ReturnT>
+struct expression_valid_with_return<
+ ExpressionTemplate, T, ReturnT,
+ std::enable_if_t<std::is_same<ReturnT, ExpressionTemplate<T>>::value>> : std::true_type
+{
+};
} // namespace helper_functions
} // namespace common
diff --git a/common/autoware_auto_common/include/helper_functions/type_name.hpp b/common/autoware_auto_common/include/helper_functions/type_name.hpp
index 1ca75ab86bd6e..a226f7c8fd23a 100644
--- a/common/autoware_auto_common/include/helper_functions/type_name.hpp
+++ b/common/autoware_auto_common/include/helper_functions/type_name.hpp
@@ -32,19 +32,19 @@ namespace helper_functions
{
/// @brief Get a demangled name of a type.
-template<typename T>
+template <typename T>
COMMON_PUBLIC std::string get_type_name()
{
- #if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)
+#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)
return abi::__cxa_demangle(typeid(T).name(), NULL, NULL, 0);
- #else
+#else
// For unsupported compilers return a mangled name.
return typeid(T).name();
- #endif
+#endif
}
/// @brief Get a demangled name of a type given its instance.
-template<typename T>
+template <typename T>
COMMON_PUBLIC std::string get_type_name(const T &)
{
return get_type_name<T>();
diff --git a/common/autoware_auto_common/package.xml b/common/autoware_auto_common/package.xml
index b79ac8cbe2291..eb8f4af55c2fa 100644
--- a/common/autoware_auto_common/package.xml
+++ b/common/autoware_auto_common/package.xml
@@ -1,21 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
- <name>autoware_auto_common</name>
- <version>1.0.0</version>
- <description>Miscelaneous helper functions</description>
- <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
- <license>Apache 2</license>
+ <name>autoware_auto_common</name>
+ <version>1.0.0</version>
+ <description>Miscelaneous helper functions</description>
+ <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
+ <license>Apache 2</license>
- <buildtool_depend>ament_cmake_auto</buildtool_depend>
- <buildtool_depend>autoware_auto_cmake</buildtool_depend>
+ <buildtool_depend>ament_cmake_auto</buildtool_depend>
+ <buildtool_depend>autoware_auto_cmake</buildtool_depend>
- <build_depend>builtin_interfaces</build_depend>
- <build_depend>eigen</build_depend>
+ <build_depend>builtin_interfaces</build_depend>
+ <build_depend>eigen</build_depend>
- <test_depend>ament_cmake_gtest</test_depend>
- <test_depend>ament_lint_auto</test_depend>
- <test_depend>ament_lint_common</test_depend>
+ <test_depend>ament_cmake_gtest</test_depend>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>autoware_lint_common</test_depend>
- <export><build_type>ament_cmake</build_type></export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/common/autoware_auto_common/test/test_angle_utils.cpp b/common/autoware_auto_common/test/test_angle_utils.cpp
index dda28d7d62588..1aa0f3c0a8057 100644
--- a/common/autoware_auto_common/test/test_angle_utils.cpp
+++ b/common/autoware_auto_common/test/test_angle_utils.cpp
@@ -21,14 +21,15 @@
namespace
{
+using autoware::common::helper_functions::wrap_angle;
using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
-using autoware::common::helper_functions::wrap_angle;
} // namespace
/// @test Wrap an angle.
-TEST(TestAngleUtils, WrapAngle) {
+TEST(TestAngleUtils, WrapAngle)
+{
EXPECT_DOUBLE_EQ(wrap_angle(-5.0 * M_PI_2), -M_PI_2);
EXPECT_DOUBLE_EQ(wrap_angle(5.0 * M_PI_2), M_PI_2);
EXPECT_DOUBLE_EQ(wrap_angle(M_PI), -M_PI);
diff --git a/common/autoware_auto_common/test/test_bool_comparisons.cpp b/common/autoware_auto_common/test/test_bool_comparisons.cpp
index 6dd15f6dfb597..67c1c8ddbf9aa 100644
--- a/common/autoware_auto_common/test/test_bool_comparisons.cpp
+++ b/common/autoware_auto_common/test/test_bool_comparisons.cpp
@@ -18,10 +18,10 @@
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
-#include <gtest/gtest.h>
-
#include "helper_functions/bool_comparisons.hpp"
+#include <gtest/gtest.h>
+
// cppcheck does not like gtest macros inside of namespaces:
// https://sourceforge.net/p/cppcheck/discussion/general/thread/e68df47b/
// use a namespace alias instead of putting macros into the namespace
@@ -29,7 +29,8 @@ namespace comp = autoware::common::helper_functions::comparisons;
//------------------------------------------------------------------------------
-TEST(HelperFunctionsComparisons, ExclusiveOr) {
+TEST(HelperFunctionsComparisons, ExclusiveOr)
+{
EXPECT_TRUE(comp::exclusive_or(0, 1));
EXPECT_TRUE(comp::exclusive_or(1, 0));
EXPECT_FALSE(comp::exclusive_or(0, 0));
diff --git a/common/autoware_auto_common/test/test_byte_reader.cpp b/common/autoware_auto_common/test/test_byte_reader.cpp
index 8425cda73a1e6..0d9daec1e92f7 100644
--- a/common/autoware_auto_common/test/test_byte_reader.cpp
+++ b/common/autoware_auto_common/test/test_byte_reader.cpp
@@ -14,10 +14,11 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <vector>
+#include "common/types.hpp"
#include "gtest/gtest.h"
#include "helper_functions/byte_reader.hpp"
-#include "common/types.hpp"
+
+#include <vector>
using autoware::common::types::float64_t;
@@ -28,13 +29,11 @@ class ByteReader : public ::testing::Test
};
} // namespace
-
// tests serial_driver_node's get_packet function which receives serial packages
TEST_F(ByteReader, Basic)
{
- std::vector<uint8_t> data = {
- 0x00, 0x00, 0x00, 0x17, 0x40, 0x28, 0xAE, 0x14, 0x7A, 0xE1, 0x47, 0xAE, 0x00, 0x00, 0x08
- };
+ std::vector<uint8_t> data = {0x00, 0x00, 0x00, 0x17, 0x40, 0x28, 0xAE, 0x14,
+ 0x7A, 0xE1, 0x47, 0xAE, 0x00, 0x00, 0x08};
autoware::common::helper_functions::ByteReader byte_reader(data);
diff --git a/common/autoware_auto_common/test/test_float_comparisons.cpp b/common/autoware_auto_common/test/test_float_comparisons.cpp
index cb44c1bf4d038..d292dc0a0cb20 100644
--- a/common/autoware_auto_common/test/test_float_comparisons.cpp
+++ b/common/autoware_auto_common/test/test_float_comparisons.cpp
@@ -18,10 +18,10 @@
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
-#include <gtest/gtest.h>
-
#include "helper_functions/float_comparisons.hpp"
+#include <gtest/gtest.h>
+
// cppcheck does not like gtest macros inside of namespaces:
// https://sourceforge.net/p/cppcheck/discussion/general/thread/e68df47b/
// use a namespace alias instead of putting macros into the namespace
@@ -40,7 +40,8 @@ const auto epsilon = 0.0001;
//------------------------------------------------------------------------------
-TEST(HelperFunctionsComparisons, AbsEqZero) {
+TEST(HelperFunctionsComparisons, AbsEqZero)
+{
EXPECT_TRUE(comp::abs_eq_zero(d, epsilon));
EXPECT_TRUE(comp::abs_eq_zero(d + epsilon * epsilon, epsilon));
EXPECT_FALSE(comp::abs_eq_zero(d + 2.0 * epsilon, epsilon));
@@ -50,7 +51,8 @@ TEST(HelperFunctionsComparisons, AbsEqZero) {
//------------------------------------------------------------------------------
-TEST(HelperFunctionsComparisons, AbsEq) {
+TEST(HelperFunctionsComparisons, AbsEq)
+{
EXPECT_TRUE(comp::abs_eq(c, e, epsilon));
EXPECT_TRUE(comp::abs_eq(e, c, epsilon));
EXPECT_FALSE(comp::abs_eq(c, e, 0.0));
@@ -63,7 +65,8 @@ TEST(HelperFunctionsComparisons, AbsEq) {
//------------------------------------------------------------------------------
-TEST(HelperFunctionsComparisons, AbsLt) {
+TEST(HelperFunctionsComparisons, AbsLt)
+{
EXPECT_TRUE(comp::abs_lt(f, c, 0.0));
EXPECT_TRUE(comp::abs_lt(f, c, epsilon));
EXPECT_FALSE(comp::abs_lt(c, f, epsilon));
@@ -73,7 +76,8 @@ TEST(HelperFunctionsComparisons, AbsLt) {
//------------------------------------------------------------------------------
-TEST(HelperFunctionsComparisons, AbsLte) {
+TEST(HelperFunctionsComparisons, AbsLte)
+{
EXPECT_TRUE(comp::abs_lte(c, e, epsilon));
EXPECT_TRUE(comp::abs_lte(e, c, epsilon));
EXPECT_FALSE(comp::abs_lte(c, e, 0.0));
@@ -88,7 +92,8 @@ TEST(HelperFunctionsComparisons, AbsLte) {
//------------------------------------------------------------------------------
-TEST(HelperFunctionsComparisons, AbsGt) {
+TEST(HelperFunctionsComparisons, AbsGt)
+{
EXPECT_TRUE(comp::abs_gt(c, e, 0.0));
EXPECT_FALSE(comp::abs_gt(c, e, epsilon));
EXPECT_FALSE(comp::abs_gt(f, c, epsilon));
@@ -99,7 +104,8 @@ TEST(HelperFunctionsComparisons, AbsGt) {
//------------------------------------------------------------------------------
-TEST(HelperFunctionsComparisons, AbsGte) {
+TEST(HelperFunctionsComparisons, AbsGte)
+{
EXPECT_TRUE(comp::abs_gte(c, e, 0.0));
EXPECT_FALSE(comp::abs_gte(e, c, 0.0));
EXPECT_TRUE(comp::abs_gte(c, e, epsilon));
@@ -112,7 +118,8 @@ TEST(HelperFunctionsComparisons, AbsGte) {
//------------------------------------------------------------------------------
-TEST(HelperFunctionsComparisons, RelEq) {
+TEST(HelperFunctionsComparisons, RelEq)
+{
EXPECT_FALSE(comp::rel_eq(c, e, 0.0));
EXPECT_FALSE(comp::rel_eq(e, c, 0.0));
EXPECT_TRUE(comp::rel_eq(a, a, 0.0));
@@ -128,7 +135,8 @@ TEST(HelperFunctionsComparisons, RelEq) {
//------------------------------------------------------------------------------
-TEST(HelperFunctionsComparisons, ApproxEq) {
+TEST(HelperFunctionsComparisons, ApproxEq)
+{
EXPECT_TRUE(comp::approx_eq(c, e, epsilon, 0.0));
EXPECT_TRUE(comp::approx_eq(e, c, epsilon, 0.0));
EXPECT_TRUE(comp::approx_eq(a, a, epsilon, 0.0));
diff --git a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp
index 878893fda161e..d3f2448c52f92 100644
--- a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp
+++ b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp
@@ -13,11 +13,11 @@
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <gtest/gtest.h>
-
#include <common/types.hpp>
#include <helper_functions/mahalanobis_distance.hpp>
+#include <gtest/gtest.h>
+
TEST(MahalanobisDistanceTest, BasicTest)
{
Eigen::Matrix<autoware::common::types::float32_t, 2, 1> mean;
@@ -31,11 +31,10 @@ TEST(MahalanobisDistanceTest, BasicTest)
// equidistant from mean but on two different axes will have vastly different
// mahalanobis distance values
EXPECT_FLOAT_EQ(
- autoware::common::helper_functions::calculate_mahalanobis_distance(
- sample, mean, cov), 1.666666666F);
+ autoware::common::helper_functions::calculate_mahalanobis_distance(sample, mean, cov),
+ 1.666666666F);
sample << 3.F, 2.F;
EXPECT_FLOAT_EQ(
- autoware::common::helper_functions::calculate_mahalanobis_distance(
- sample, mean, cov), 10.0F);
+ autoware::common::helper_functions::calculate_mahalanobis_distance(sample, mean, cov), 10.0F);
}
diff --git a/common/autoware_auto_common/test/test_message_field_adapters.cpp b/common/autoware_auto_common/test/test_message_field_adapters.cpp
index aafddd8f320de..30b9148eaf5f3 100644
--- a/common/autoware_auto_common/test/test_message_field_adapters.cpp
+++ b/common/autoware_auto_common/test/test_message_field_adapters.cpp
@@ -14,15 +14,17 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <gtest/gtest.h>
-
#include <helper_functions/message_adapters.hpp>
+
#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <gtest/gtest.h>
+
#include <memory>
#include <vector>
-using autoware::common::helper_functions::message_field_adapters::get_stamp;
using autoware::common::helper_functions::message_field_adapters::get_frame_id;
+using autoware::common::helper_functions::message_field_adapters::get_stamp;
namespace
{
@@ -35,7 +37,8 @@ builtin_interfaces::msg::Time get_stamp_msg(int t)
}
} // namespace
-TEST(MessageFieldAdapterTest, ConstHeaderTests) {
+TEST(MessageFieldAdapterTest, ConstHeaderTests)
+{
using Message = geometry_msgs::msg::TransformStamped;
const auto stamp = get_stamp_msg(0);
@@ -50,7 +53,8 @@ TEST(MessageFieldAdapterTest, ConstHeaderTests) {
EXPECT_EQ(frame_id, get_frame_id(msg));
}
-TEST(MessageFieldAdapterTest, NonconstHeaderTests) {
+TEST(MessageFieldAdapterTest, NonconstHeaderTests)
+{
using Message = geometry_msgs::msg::TransformStamped;
const auto stamp = get_stamp_msg(0);
diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp
index 876119d1e6776..c20704536e424 100644
--- a/common/autoware_auto_common/test/test_template_utils.cpp
+++ b/common/autoware_auto_common/test/test_template_utils.cpp
@@ -14,97 +14,85 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <gtest/gtest.h>
#include <helper_functions/template_utils.hpp>
-struct CorrectType {};
-struct FalseType {};
+#include <gtest/gtest.h>
+
+struct CorrectType
+{
+};
+struct FalseType
+{
+};
struct Foo
{
- CorrectType bar(CorrectType, const CorrectType &, CorrectType *)
- {
- return CorrectType{};
- }
+ CorrectType bar(CorrectType, const CorrectType &, CorrectType *) { return CorrectType{}; }
};
-template<template<typename> class Expression, typename ... Ts>
+template <template <typename> class Expression, typename... Ts>
using expression_valid_with_return =
- ::autoware::common::helper_functions::expression_valid_with_return<Expression, Ts ...>;
-template<template<typename> class Expression, typename ... Ts>
-using expression_valid = ::autoware::common::helper_functions::expression_valid<Expression, Ts ...>;
+ ::autoware::common::helper_functions::expression_valid_with_return<Expression, Ts...>;
+template <template <typename> class Expression, typename... Ts>
+using expression_valid = ::autoware::common::helper_functions::expression_valid<Expression, Ts...>;
// Types are defined here and not in the header because these definitions are basically the test
// code themselves.
// Correct way to call Foo::bar(...)
-template<typename FooT, typename In1, typename In2, typename In3>
+template <typename FooT, typename In1, typename In2, typename In3>
using call_bar_expression = decltype(std::declval<FooT>().bar(
- std::declval<In1>(),
- std::declval<const In2 &>(),
- std::declval<In3 *>()
-));
+ std::declval<In1>(), std::declval<const In2 &>(), std::declval<In3 *>()));
// Another correct way to call Foo::bar(...) since a temporary can bind to the const lvalue
// reference
-template<typename FooT, typename In1, typename In2, typename In3>
+template <typename FooT, typename In1, typename In2, typename In3>
using call_bar_expression2 = decltype(std::declval<FooT>().bar(
- std::declval<In1>(),
- std::declval<In2>(),
- std::declval<In3 *>()
-));
+ std::declval<In1>(), std::declval<In2>(), std::declval<In3 *>()));
// Signature mismatch:
-template<typename FooT, typename In1, typename In2, typename In3>
-using false_bar_expression1 = decltype(std::declval<FooT>().bar(
- std::declval<In1>(),
- std::declval<In2>(),
- std::declval<In3>()
-));
+template <typename FooT, typename In1, typename In2, typename In3>
+using false_bar_expression1 =
+ decltype(std::declval<FooT>().bar(std::declval<In1>(), std::declval<In2>(), std::declval<In3>()));
// Signature mismatch:
-template<typename FooT, typename In1, typename In2>
-using false_bar_expression2 = decltype(std::declval<FooT>().bar(
- std::declval<In1>(),
- std::declval<const In2 &>()
-));
+template <typename FooT, typename In1, typename In2>
+using false_bar_expression2 =
+ decltype(std::declval<FooT>().bar(std::declval<In1>(), std::declval<const In2 &>()));
// Signature mismatch:
-template<typename FooT, typename In1, typename In2, typename In3>
+template <typename FooT, typename In1, typename In2, typename In3>
using false_bar_expression3 = decltype(std::declval<FooT>().asdasd(
- std::declval<In1>(),
- std::declval<const In2 &>(),
- std::declval<In3 *>()
-));
+ std::declval<In1>(), std::declval<const In2 &>(), std::declval<In3 *>()));
// Correct signature, correct types:
-template<typename FooT>
+template <typename FooT>
using correct_expression1 = call_bar_expression<FooT, CorrectType, CorrectType, CorrectType>;
-template<typename FooT>
+template <typename FooT>
using correct_expression2 = call_bar_expression2<FooT, CorrectType, CorrectType, CorrectType>;
// Correct signature, false types:
-template<typename FooT>
+template <typename FooT>
using false_expression1 = call_bar_expression<FooT, FalseType, CorrectType, CorrectType>;
-template<typename FooT>
+template <typename FooT>
using false_expression2 = call_bar_expression<FooT, CorrectType, FalseType, CorrectType>;
-template<typename FooT>
+template <typename FooT>
using false_expression3 = call_bar_expression<FooT, FalseType, FalseType, FalseType>;
// False signature, correct types:
-template<typename FooT>
+template <typename FooT>
using false_expression4 = false_bar_expression1<FooT, CorrectType, CorrectType, CorrectType>;
-template<typename FooT>
+template <typename FooT>
using false_expression5 = false_bar_expression3<FooT, CorrectType, CorrectType, CorrectType>;
// False signature, false types:
-template<typename FooT>
+template <typename FooT>
using false_expression6 = false_bar_expression1<FooT, CorrectType, CorrectType, CorrectType>;
-template<typename FooT>
+template <typename FooT>
using false_expression7 = false_bar_expression2<FooT, CorrectType, CorrectType>;
-
-TEST(TestTemplateUtils, ExpressionValid) {
+TEST(TestTemplateUtils, ExpressionValid)
+{
EXPECT_TRUE((expression_valid<correct_expression1, Foo>::value));
EXPECT_TRUE((expression_valid<correct_expression2, Foo>::value));
EXPECT_FALSE((expression_valid<false_expression1, Foo>::value));
@@ -116,7 +104,8 @@ TEST(TestTemplateUtils, ExpressionValid) {
EXPECT_FALSE((expression_valid<false_expression7, Foo>::value));
}
-TEST(TestTemplateUtils, ExpressionReturnValid) {
+TEST(TestTemplateUtils, ExpressionReturnValid)
+{
EXPECT_TRUE((expression_valid_with_return<correct_expression1, Foo, CorrectType>::value));
EXPECT_FALSE((expression_valid_with_return<correct_expression1, Foo, FalseType>::value));
diff --git a/common/autoware_auto_common/test/test_type_name.cpp b/common/autoware_auto_common/test/test_type_name.cpp
index 9a93c2625daeb..856a220b024f9 100644
--- a/common/autoware_auto_common/test/test_type_name.cpp
+++ b/common/autoware_auto_common/test/test_type_name.cpp
@@ -25,11 +25,14 @@ using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
using autoware::helper_functions::get_type_name;
-struct SomeStruct {};
+struct SomeStruct
+{
+};
} // namespace
/// @test Test that type names can be demangled.
-TEST(TestTypeDemangling, Demangle) {
+TEST(TestTypeDemangling, Demangle)
+{
EXPECT_EQ(get_type_name<float32_t>(), "float");
const float64_t val{42.0};
EXPECT_EQ(get_type_name(val), "double");
diff --git a/common/autoware_auto_common/test/test_type_traits.cpp b/common/autoware_auto_common/test/test_type_traits.cpp
index a4fbdf53652f1..822058e74c9a8 100644
--- a/common/autoware_auto_common/test/test_type_traits.cpp
+++ b/common/autoware_auto_common/test/test_type_traits.cpp
@@ -27,7 +27,7 @@ namespace
///
/// Trait "is_arithmetic" is picked at random and any other trait could have been used
/// instead.
-template<typename ... Ts>
+template <typename... Ts>
bool all_are_arithmetic()
{
// This is just a random function that we use with conjunction.
@@ -40,56 +40,66 @@ using autoware::common::types::float64_t;
} // namespace
/// @test Test that index of a type can be computed.
-TEST(TestCommonTypeTraits, Index) {
+TEST(TestCommonTypeTraits, Index)
+{
using T = std::tuple<std::int32_t, float64_t>;
EXPECT_EQ(0, (autoware::common::type_traits::index<std::int32_t, T>::value));
EXPECT_EQ(1, (autoware::common::type_traits::index<float64_t, T>::value));
}
-TEST(TestCommonTypeTraits, Conjunction) {
+TEST(TestCommonTypeTraits, Conjunction)
+{
EXPECT_TRUE((all_are_arithmetic<std::int32_t, float32_t>()));
EXPECT_FALSE(
(all_are_arithmetic<std::int32_t, float32_t, std::tuple<std::int32_t, float32_t>>()));
}
-TEST(TestCommonTypeTraits, Visit) {
+TEST(TestCommonTypeTraits, Visit)
+{
const std::tuple<std::int32_t, float64_t> t;
std::int32_t counter{};
- autoware::common::type_traits::visit(t, [&counter](const auto &) {counter++;});
+ autoware::common::type_traits::visit(t, [&counter](const auto &) { counter++; });
EXPECT_EQ(2, counter);
float64_t sum{};
autoware::common::type_traits::visit(
- std::make_tuple(2, 42.0F, 23.0), [&sum](const auto & element) {
- sum += static_cast<float64_t>(element);
- });
+ std::make_tuple(2, 42.0F, 23.0),
+ [&sum](const auto & element) { sum += static_cast<float64_t>(element); });
EXPECT_DOUBLE_EQ(67.0, sum);
}
-TEST(TestCommonTypeTraits, HasType) {
- struct T1 {};
- struct T2 {};
- struct T3 {};
- EXPECT_TRUE(
- (autoware::common::type_traits::has_type<T1, std::tuple<T1, T2>>::value));
- EXPECT_FALSE(
- (autoware::common::type_traits::has_type<T3, std::tuple<T1, T2>>::value));
- EXPECT_FALSE(
- (autoware::common::type_traits::has_type<T1, std::tuple<>>::value));
+TEST(TestCommonTypeTraits, HasType)
+{
+ struct T1
+ {
+ };
+ struct T2
+ {
+ };
+ struct T3
+ {
+ };
+ EXPECT_TRUE((autoware::common::type_traits::has_type<T1, std::tuple<T1, T2>>::value));
+ EXPECT_FALSE((autoware::common::type_traits::has_type<T3, std::tuple<T1, T2>>::value));
+ EXPECT_FALSE((autoware::common::type_traits::has_type<T1, std::tuple<>>::value));
}
-TEST(TestCommonTypeTraits, TypeIntersection) {
- struct T1 {};
- struct T2 {};
- struct T3 {};
+TEST(TestCommonTypeTraits, TypeIntersection)
+{
+ struct T1
+ {
+ };
+ struct T2
+ {
+ };
+ struct T3
+ {
+ };
using A = std::tuple<T1, T2>;
using B = std::tuple<T2, T3>;
using C = std::tuple<T3>;
EXPECT_TRUE(
- (std::is_same<std::tuple<T2>,
- autoware::common::type_traits::intersect<A, B>::type>::value));
- EXPECT_TRUE(
- (std::is_same<A, autoware::common::type_traits::intersect<A, A>::type>::value));
+ (std::is_same<std::tuple<T2>, autoware::common::type_traits::intersect<A, B>::type>::value));
+ EXPECT_TRUE((std::is_same<A, autoware::common::type_traits::intersect<A, A>::type>::value));
EXPECT_TRUE(
- (std::is_same<std::tuple<>,
- autoware::common::type_traits::intersect<A, C>::type>::value));
+ (std::is_same<std::tuple<>, autoware::common::type_traits::intersect<A, C>::type>::value));
}
diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md
index d41e2846715c0..f586d4c19607e 100644
--- a/common/autoware_auto_geometry/design/interval.md
+++ b/common/autoware_auto_geometry/design/interval.md
@@ -1,43 +1,42 @@
-Interval {#geometry-interval}
-========
+# Interval {#geometry-interval}
The interval is a standard 1D real-valued interval.
The class implements a representation and operations on the interval type and guarantees interval validity on construction.
-Basic operations and accessors are implemented, as well as other common operations.
+Basic operations and accessors are implemented, as well as other common operations.
See 'Example Usage' below.
-# Target use cases
+## Target use cases
-* Range or containment checks.
-The interval class simplifies code that involves checking membership of a value to a range, or intersecting two ranges.
-It also provides consistent behavior and consistent handling of edge cases.
+- Range or containment checks.
+ The interval class simplifies code that involves checking membership of a value to a range, or intersecting two ranges.
+ It also provides consistent behavior and consistent handling of edge cases.
-# Properties
+## Properties
-* **empty**: An empty interval is equivalent to an empty set.
-It contains no elements.
-It is a valid interval, but because it is empty, the notion of measure (length) is undefined; the measure of an empty interval is *not* zero.
-The implementation represents the measure of an empty interval with `NaN`.
-* **zero measure**: An interval with zero measure is an interval whose bounds are exactly equal.
-The measure is zero because the interval contains only a single point, and points have zero measure.
-However, because it does contain a single element, the interval is *not* empty.
-* **valid**: A valid interval is either empty or has min/max bounds such that (min <= max). On construction, interval objects are guaranteed to be valid.
-An attempt to construct an invalid interval results in a runtime_error exception being thrown.
-* **pseudo-immutable**: Once constructed the only way to change the value of an interval is to overwrite it with a new one; an existing object cannot be modified.
+- **empty**: An empty interval is equivalent to an empty set.
+ It contains no elements.
+ It is a valid interval, but because it is empty, the notion of measure (length) is undefined; the measure of an empty interval is _not_ zero.
+ The implementation represents the measure of an empty interval with `NaN`.
+- **zero measure**: An interval with zero measure is an interval whose bounds are exactly equal.
+ The measure is zero because the interval contains only a single point, and points have zero measure.
+ However, because it does contain a single element, the interval is _not_ empty.
+- **valid**: A valid interval is either empty or has min/max bounds such that (min <= max). On construction, interval objects are guaranteed to be valid.
+ An attempt to construct an invalid interval results in a runtime_error exception being thrown.
+- **pseudo-immutable**: Once constructed the only way to change the value of an interval is to overwrite it with a new one; an existing object cannot be modified.
-# Conventions
+## Conventions
-* All operations on interval objects are defined as static class methods on the interval class.
-This is a functional-style of programming that basically turns the class into a namespace that grants functions access to private member variables of the object they operate on.
+- All operations on interval objects are defined as static class methods on the interval class.
+ This is a functional-style of programming that basically turns the class into a namespace that grants functions access to private member variables of the object they operate on.
-# Assumptions
+## Assumptions
-* The interval is only intended for floating point types.
-This is enforced via static assertion.
-* The constructor for non-empty intervals takes two arguments 'min' and 'max', and they must be ordered (i.e., min <= max).
-If this assumption is violated, an exception is emitted and construction fails.
+- The interval is only intended for floating point types.
+ This is enforced via static assertion.
+- The constructor for non-empty intervals takes two arguments 'min' and 'max', and they must be ordered (i.e., min <= max).
+ If this assumption is violated, an exception is emitted and construction fails.
-# Example Usage
+## Example Usage
```c++
#include "geometry/interval.hpp"
@@ -72,7 +71,7 @@ const auto i = Interval_d(MIN, MAX);
// Test accessors and properties
//
-std::cout << Interval_d::min(i) << " " << Interval_d::max(i) << "\n";
+std::cout << Interval_d::min(i) << " " << Interval_d::max(i) << "\n";
// Prints: 0.0 1.0
std::cout << Interval_d::empty(i) << " " << Interval_d::length(i) << "\n";
diff --git a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md
index d2a943b0e6e30..a771facbf002a 100644
--- a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md
+++ b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md
@@ -1,45 +1,46 @@
-2D Convex Polygon Intersection {#convex-polygon-intersection-2d}
-============
+# 2D Convex Polygon Intersection {#convex-polygon-intersection-2d}
Two convex polygon's intersection can be visualized on the image below as the blue area:
<img src="convex_polygon_intersection.png">
-# Purpose / Use cases
+## Purpose / Use cases
Computing the intersection between two polygons can be useful in many applications of scene
-understanding. It can be used to estimate collision detection, shape alignment, shape
+understanding. It can be used to estimate collision detection, shape alignment, shape
association and in any application that deals with the objects around the perceiving agent.
-# Design
+## Design
-[\(Livermore, Calif, 1977\)](https://www.osti.gov/servlets/purl/7309916/) mention the following
+[\(Livermore, Calif, 1977\)](https://www.osti.gov/servlets/purl/7309916/) mention the following
observations about convex polygon intersection:
-* Intersection of two convex polygons is a convex polygon
-* A vertex from a polygon that is contained in the other polygon is a vertex of the intersection
-shape. (Vertices A, C, D in the shape above)
-* An edge from a polygon that is contained in the other polygon is an edge in the intersection
-shape. (edge C-D in the shape above)
-* Edge intersections between two polygons are vertices in the intersection shape. (Vertices B,
-E in the shape above.)
+- Intersection of two convex polygons is a convex polygon
+- A vertex from a polygon that is contained in the other polygon is a vertex of the intersection
+ shape. (Vertices A, C, D in the shape above)
+- An edge from a polygon that is contained in the other polygon is an edge in the intersection
+ shape. (edge C-D in the shape above)
+- Edge intersections between two polygons are vertices in the intersection shape. (Vertices B,
+ E in the shape above.)
-## Inner-workings / Algorithms
+### Inner-workings / Algorithms
With the observation mentioned above, the current algorithm operates in the following way:
-* Compute and find the vertices from each polygon that is contained in the other polygon
-(Vertices A, C, D)
-* Compute and find the intersection points between each polygon (Verties B, E)
-* Compute the convex hull shaped by these vertices by ordering them CCW.
+- Compute and find the vertices from each polygon that is contained in the other polygon
+ (Vertices A, C, D)
+- Compute and find the intersection points between each polygon (Verties B, E)
+- Compute the convex hull shaped by these vertices by ordering them CCW.
-## Inputs / Outputs / API
+### Inputs / Outputs / API
Inputs:
-* Two iterables that contain vertices of the convex polygons ordered in the CCW direction.
+
+- Two iterables that contain vertices of the convex polygons ordered in the CCW direction.
Outputs:
-* A list of vertices of the intersection shape ordered in the CCW direction.
+
+- A list of vertices of the intersection shape ordered in the CCW direction.
## Future Work
@@ -47,4 +48,4 @@ Outputs:
## Related issues
-- #983: Integrate vision detections in object tracker
+- #983: Integrate vision detections in object tracker
diff --git a/common/autoware_auto_geometry/design/spatial-hash-design.md b/common/autoware_auto_geometry/design/spatial-hash-design.md
index 2b8f5449a0da4..469da747329fb 100644
--- a/common/autoware_auto_geometry/design/spatial-hash-design.md
+++ b/common/autoware_auto_geometry/design/spatial-hash-design.md
@@ -1,5 +1,4 @@
-Spatial Hash {#geometry-spatial-hash}
-============
+# Spatial Hash {#geometry-spatial-hash}
The spatial hash is a data structure designed for efficient fixed-radius near-neighbor queries in
low dimensions.
@@ -13,7 +12,7 @@ radius.
For `n` points with an average of `k` neighbors each, this data structure can
perform `m` near-neighbor queries (to generate lists of near-neighbors for `m`
- different points) in `O(mk)` time.
+different points) in `O(mk)` time.
By contrast, using a k-d tree for successive nearest-neighbor queries results in
a running time of `O(m log n)`.
@@ -21,17 +20,17 @@ a running time of `O(m log n)`.
The spatial hash works as follows:
- Each point is assigned to a bin in the predefined bounding area defined by
-`x_min/x_max` and `y_min/y_max`
+ `x_min/x_max` and `y_min/y_max`
- This can be done by converting x and y position into x and y index
-respectively
- - For example with the bin containing `x_min` and `y_min` as index `(0, 0)`
- - The two (or more) indices can then be converted into a single index
+ respectively
+ - For example with the bin containing `x_min` and `y_min` as index `(0, 0)`
+ - The two (or more) indices can then be converted into a single index
- Once every point of interest has been inserted into the hash, near-neighbor
-queries can begin:
- - The bin of the reference point is first computed
- - For each point in each adjacent bin, perform an explicit distance computation
- between said point and the reference point. If the distance is below the given
- radius, said point is considered to be a near-neighbor
+ queries can begin:
+ - The bin of the reference point is first computed
+ - For each point in each adjacent bin, perform an explicit distance computation
+ between said point and the reference point. If the distance is below the given
+ radius, said point is considered to be a near-neighbor
Under the hood, an `std::unordered_multimap` is used, where the key is a bin/voxel index.
The bin size was computed to be the same as the lookup distance.
@@ -42,9 +41,9 @@ this was to avoid if statements in tight loops. The configuration class speciali
use CRTP (Curiously Recurring Template Patterns) to do "static polymorphism", and avoid
a dispatching call.
-# Performance characterization
+## Performance characterization
-## Time
+### Time
Insertion is `O(n)` because lookup time for the underlying hashmap is `O(n)` for
hashmaps. In practice, lookup time for hashmaps and thus insertion time should
@@ -56,33 +55,30 @@ direct reference to a node.
Finding `k` near-neighbors is worst case `O(n)` in the case of an adversarial
example, but in practice `O(k)`.
-
-## Space
+### Space
The module consists of the following components:
- The internal hashmap is `O(n + n + A * n)`, where `A` is an arbitrary
-constant (load factor)
+ constant (load factor)
- The other components of the spatial hash are `O(n + n)`
This results in `O(n)` space complexity.
-
-# States
+## States
The spatial hash's state is dictated by the status of the underlying unordered_multimap.
-
The data structure is wholly configured by a
[config](@ref autoware::common::geometry::spatial_hash::Config) class. The constructor
of the class determines in the data structure accepts strictly 2D or strictly 3D queries.
-# Inputs
+## Inputs
The primary method of introducing data into the data structure is via the
[insert](@ref autoware::common::geometry::spatial_hash::SpatialHashBase::insert) method.
-# Outputs
+## Outputs
The primary method of retrieving data from the data structure is via the
[near](@ref autoware::common::geometry::spatial_hash::SpatialHash<PointT, Config2d>::near)\(2D
@@ -92,7 +88,6 @@ or [near](@ref autoware::common::geometry::spatial_hash::SpatialHash<PointT, Con
The whole data structure can also be traversed using standard constant iterators.
-
## Future Work
- Performance tuning and optimization
diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp
index 9425c451cf3f9..b12a95b5ad7c9 100644
--- a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp
+++ b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp
@@ -22,6 +22,7 @@
#define GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_
#include <geometry/bounding_box/bounding_box_common.hpp>
+
#include <limits>
#include <utility>
@@ -54,7 +55,7 @@ struct Covariance2d
/// \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
-template<typename IT>
+template <typename IT>
Covariance2d covariance_2d(const IT begin, const IT end)
{
Covariance2d ret{0.0F, 0.0F, 0.0F, 0U};
@@ -97,9 +98,8 @@ Covariance2d covariance_2d(const IT begin, const IT end)
/// \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)
+template <typename PointT>
+std::pair<float32_t, float32_t> eig_2d(const Covariance2d & cov, PointT & eigvec1, PointT & eigvec2)
{
const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F;
const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy);
@@ -107,8 +107,8 @@ std::pair<float32_t, float32_t> eig_2d(
float32_t disc = ((tr_2 * tr_2) - det) + std::numeric_limits<float32_t>::epsilon();
if (disc < 0.0F) {
throw std::runtime_error(
- "pca_2d: negative discriminant! Should never happen for well formed "
- "covariance matrix");
+ "pca_2d: negative discriminant! Should never happen for well formed "
+ "covariance matrix");
}
disc = sqrtf(disc);
// compute eigenvalues
@@ -140,7 +140,6 @@ std::pair<float32_t, float32_t> eig_2d(
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 PointT type of a point with float members x and y
@@ -151,21 +150,14 @@ std::pair<float32_t, float32_t> eig_2d(
/// \param[out] support Array to get filled with extreme points in each of the principal
/// components
/// \return whether or not eig2 is ccw wrt eig1
-template<typename IT, typename PointT>
+template <typename IT, typename PointT>
bool8_t compute_supports(
- const IT begin,
- const IT end,
- const PointT & eig1,
- const PointT & eig2,
- Point4<IT> & support)
+ const IT begin, const IT end, const PointT & eig1, const PointT & eig2, Point4<IT> & support)
{
const bool8_t ret = cross_2d(eig1, eig2) >= 0.0F;
- std::array<float32_t, 4U> metrics{{
- -std::numeric_limits<float32_t>::max(),
- -std::numeric_limits<float32_t>::max(),
- std::numeric_limits<float32_t>::max(),
- std::numeric_limits<float32_t>::max()
- }};
+ std::array<float32_t, 4U> metrics{
+ {-std::numeric_limits<float32_t>::max(), -std::numeric_limits<float32_t>::max(),
+ std::numeric_limits<float32_t>::max(), std::numeric_limits<float32_t>::max()}};
for (auto it = begin; it != end; ++it) {
const PointT & pt = *it;
float32_t val = dot_2d(ret ? eig1 : eig2, pt);
@@ -198,11 +190,9 @@ bool8_t compute_supports(
/// \param[in] supports Array of iterators referring to points that are most extreme in each basis
/// direction. Should be result of compute_supports function
/// \return A bounding box based on the basis axes and the support points
-template<typename IT, typename PointT>
+template <typename IT, typename PointT>
BoundingBox compute_bounding_box(
- const PointT & ax1,
- const PointT & ax2,
- const Point4<IT> & supports)
+ const PointT & ax1, const PointT & ax2, const Point4<IT> & supports)
{
decltype(BoundingBox::corners) corners;
const Point4<PointT> directions{{ax1, ax2, minus_2d(ax1), minus_2d(ax2)}};
@@ -222,7 +212,7 @@ BoundingBox compute_bounding_box(
/// \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 An oriented bounding box in x-y. This bounding box has no height information
-template<typename IT>
+template <typename IT>
BoundingBox eigenbox_2d(const IT begin, const IT end)
{
// compute covariance
diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp
index ac2417204664c..0b70153935f5c 100644
--- a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp
+++ b/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp
@@ -22,6 +22,7 @@
#define GEOMETRY__BOUNDING_BOX__LFIT_HPP_
#include <geometry/bounding_box/eigenbox_2d.hpp>
+
#include <limits>
#include <utility>
@@ -67,7 +68,7 @@ struct LFitWs
/// \param[in] size The number of points in the point list
/// \param[out] ws A representation of the M matrix to get initialized
/// \tparam IT The iterator type, should dereference to a point type with float members x and y
-template<typename IT>
+template <typename IT>
void init_lfit_ws(const IT begin, const IT end, const std::size_t size, LFitWs & ws)
{
ws.p = 1UL;
@@ -106,17 +107,15 @@ void init_lfit_ws(const IT begin, const IT end, const std::size_t size, LFitWs &
/// \param[in] ws A representation of the M Matrix
/// \param[out] dir The best fit direction for this partition/M matrix
/// \return The L2 residual for this fit (the score, lower is better)
-template<typename PointT>
+template <typename PointT>
float32_t solve_lfit(const LFitWs & ws, PointT & dir)
{
const float32_t pi = 1.0F / static_cast<float32_t>(ws.p);
const float32_t qi = 1.0F / static_cast<float32_t>(ws.q);
- const Covariance2d M{ // matrix of form [x, z; z y]
- ws.m22a - (((ws.m12a * ws.m12a) * pi) + ((ws.m12c * ws.m12c) * qi)),
- 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
- };
+ const Covariance2d M{// matrix of form [x, z; z y]
+ ws.m22a - (((ws.m12a * ws.m12a) * pi) + ((ws.m12c * ws.m12c) * qi)),
+ 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;
@@ -126,7 +125,7 @@ float32_t solve_lfit(const LFitWs & ws, PointT & dir)
/// \tparam PointT The point type
/// \param[in] pt The point to increment the M matrix with
/// \param[inout] ws A representation of the M matrix
-template<typename PointT>
+template <typename PointT>
void increment_lfit_ws(const PointT & pt, LFitWs & ws)
{
const float32_t px = point_adapter::x_(pt);
@@ -142,7 +141,7 @@ void increment_lfit_ws(const PointT & pt, LFitWs & ws)
}
/// \tparam IT An iterator type that should dereference into a point type with float members x and y
-template<typename PointT>
+template <typename PointT>
class LFitCompare
{
public:
@@ -150,8 +149,7 @@ class LFitCompare
/// \param[in] hint A 2d vector with the direction that will be used to order the
/// point list
explicit LFitCompare(const PointT & hint)
- : m_nx(point_adapter::x_(hint)),
- m_ny(point_adapter::y_(hint))
+ : m_nx(point_adapter::x_(hint)), m_ny(point_adapter::y_(hint))
{
}
@@ -179,7 +177,7 @@ class LFitCompare
/// \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
-template<typename IT>
+template <typename IT>
BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size)
{
// initialize M
@@ -239,19 +237,16 @@ BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::s
/// second element is the size of the list
/// \tparam IT An iterator type dereferencable 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>
+template <typename IT, typename PointT>
BoundingBox lfit_bounding_box_2d(
- const IT begin,
- const IT end,
- const PointT hint,
- const std::size_t size)
+ const IT begin, const IT end, const PointT hint, const std::size_t size)
{
if (size <= 1U) {
throw std::domain_error("LFit requires >= 2 points!");
}
// NOTE: assumes points are in base_link/sensor frame!
// sort along tangent wrt sensor origin
- //lint -e522 NOLINT Not a pure function: data structure iterators are pointing to is modified
+ // lint -e522 NOLINT Not a pure function: data structure iterators are pointing to is modified
std::partial_sort(begin, end, end, details::LFitCompare<PointT>{hint});
return details::lfit_bounding_box_2d_impl(begin, end, size);
@@ -264,7 +259,7 @@ BoundingBox lfit_bounding_box_2d(
/// \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
-template<typename IT>
+template <typename IT>
BoundingBox lfit_bounding_box_2d(const IT begin, const IT end)
{
// use the principal component as the sorting direction
diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp
index f1a2123e25c9b..550bfd82f5e29 100644
--- a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp
+++ b/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp
@@ -19,9 +19,9 @@
#ifndef GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_
#define GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_
-#include <geometry/convex_hull.hpp>
-#include <geometry/common_2d.hpp>
#include <geometry/bounding_box/bounding_box_common.hpp>
+#include <geometry/common_2d.hpp>
+#include <geometry/convex_hull.hpp>
#include <algorithm>
#include <cstring>
@@ -44,7 +44,7 @@ namespace details
/// \param[inout] directions Array of directions of current bounding box (in ccw order)
/// \tparam PointT Point type of the lists, must have float members x and y
/// \return index of array to advance, e.g. the one with the smallest angle between edge and dir
-template<typename PointT>
+template <typename PointT>
uint32_t update_angles(const Point4<PointT> & edges, Point4<PointT> & directions)
{
// find smallest angle to next
@@ -52,10 +52,8 @@ uint32_t update_angles(const Point4<PointT> & edges, Point4<PointT> & directions
float32_t best_edge_dir_mag = 0.0F;
uint32_t advance_idx = 0U;
for (uint32_t idx = 0U; idx < edges.size(); ++idx) {
- const float32_t edge_dir_mag =
- std::max(
- norm_2d(edges[idx]) * norm_2d(
- directions[idx]), std::numeric_limits<float32_t>::epsilon());
+ const float32_t edge_dir_mag = std::max(
+ norm_2d(edges[idx]) * norm_2d(directions[idx]), std::numeric_limits<float32_t>::epsilon());
const float32_t cos_th = dot_2d(edges[idx], directions[idx]) / edge_dir_mag;
if (cos_th > best_cos_th) {
best_cos_th = cos_th;
@@ -81,12 +79,8 @@ uint32_t update_angles(const Point4<PointT> & edges, Point4<PointT> & directions
/// \param[in] end An iterator pointing to one past the last point in the point list
/// \param[in] support Array of points that are most extreme in 4 directions for current OBB
/// \param[out] edges An array to be filled with the polygon edges next from support points
-template<typename IT, typename PointT>
-void init_edges(
- const IT begin,
- const IT end,
- const Point4<IT> & support,
- Point4<PointT> & edges)
+template <typename IT, typename PointT>
+void init_edges(const IT begin, const IT end, const Point4<IT> & support, Point4<PointT> & edges)
{
for (uint32_t idx = 0U; idx < support.size(); ++idx) {
auto tmp_it = support[idx];
@@ -104,7 +98,7 @@ void init_edges(
/// \param[out] support Array that gets filled with pointers to points that are most extreme in
/// each direction aligned with and orthogonal to the first polygon edge.
/// Most extreme = max/min wrt u = P[1]-P[0] (in the dot/cross product sense)
-template<typename IT>
+template <typename IT>
void init_bbox(const IT begin, const IT end, Point4<IT> & support)
{
// compute initial orientation using first two points
@@ -112,11 +106,9 @@ void init_bbox(const IT begin, const IT end, Point4<IT> & support)
++pt_it;
const auto u = minus_2d(*pt_it, *begin);
support[0U] = begin;
- std::array<float32_t, 3U> metric{{
- -std::numeric_limits<float32_t>::max(),
- -std::numeric_limits<float32_t>::max(),
- std::numeric_limits<float32_t>::max()
- }};
+ std::array<float32_t, 3U> metric{
+ {-std::numeric_limits<float32_t>::max(), -std::numeric_limits<float32_t>::max(),
+ std::numeric_limits<float32_t>::max()}};
// track u * p, fabsf(u x p), and -u * p
for (pt_it = begin; pt_it != end; ++pt_it) {
// check points against orientation for run_ptr
@@ -152,7 +144,7 @@ void init_bbox(const IT begin, const IT end, Point4<IT> & support)
/// 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
/// box
-template<typename IT, typename MetricF>
+template <typename IT, typename MetricF>
BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF metric_fn)
{
using PointT = base_type<decltype(*begin)>;
@@ -232,13 +224,12 @@ BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF m
/// \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
-template<typename IT>
+template <typename IT>
BoundingBox minimum_area_bounding_box(const IT begin, const IT end)
{
- const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t
- {
- return pt.x * pt.y;
- };
+ const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t {
+ return pt.x * pt.y;
+ };
return details::rotating_calipers_impl(begin, end, metric_fn);
}
@@ -248,13 +239,12 @@ BoundingBox minimum_area_bounding_box(const IT begin, const IT end)
/// \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
-template<typename IT>
+template <typename IT>
BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end)
{
- const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t
- {
- return pt.x + pt.y;
- };
+ const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t {
+ return pt.x + pt.y;
+ };
return details::rotating_calipers_impl(begin, end, metric_fn);
}
@@ -264,7 +254,7 @@ BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end)
/// \param[inout] list A list of points to form a hull around, gets reordered
/// \return A minimum area bounding box, value field is the area
/// \tparam PointT Point type of the lists, must have float members x and y
-template<typename PointT>
+template <typename PointT>
BoundingBox minimum_area_bounding_box(std::list<PointT> & list)
{
const auto last = convex_hull(list);
@@ -277,7 +267,7 @@ BoundingBox minimum_area_bounding_box(std::list<PointT> & list)
/// \param[inout] list A list of points to form a hull around, gets reordered
/// \return A minimum perimeter bounding box, value field is half the perimeter
/// \tparam PointT Point type of the lists, must have float members x and y
-template<typename PointT>
+template <typename PointT>
BoundingBox minimum_perimeter_bounding_box(std::list<PointT> & list)
{
const auto last = convex_hull(list);
diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp
index c072ebb2b7a99..32ddb48633ee6 100644
--- a/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp
+++ b/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp
@@ -18,9 +18,9 @@
#ifndef GEOMETRY__BOUNDING_BOX_2D_HPP_
#define GEOMETRY__BOUNDING_BOX_2D_HPP_
-#include <geometry/bounding_box/rotating_calipers.hpp>
#include <geometry/bounding_box/eigenbox_2d.hpp>
#include <geometry/bounding_box/lfit.hpp>
+#include <geometry/bounding_box/rotating_calipers.hpp>
namespace autoware
{
diff --git a/common/autoware_auto_geometry/include/geometry/common_2d.hpp b/common/autoware_auto_geometry/include/geometry/common_2d.hpp
index 9b2a4e6ad9058..bd00c05328ec2 100644
--- a/common/autoware_auto_geometry/include/geometry/common_2d.hpp
+++ b/common/autoware_auto_geometry/include/geometry/common_2d.hpp
@@ -19,16 +19,18 @@
#ifndef GEOMETRY__COMMON_2D_HPP_
#define GEOMETRY__COMMON_2D_HPP_
+#include "geometry/interval.hpp"
+
#include <common/types.hpp>
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
#include <cmath>
#include <limits>
#include <stdexcept>
-#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
-#include "geometry/interval.hpp"
-
-using autoware::common::types::float32_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float32_t;
namespace comparison = autoware::common::helper_functions::comparisons;
@@ -46,7 +48,7 @@ namespace point_adapter
/// \return The x value of the point
/// \param[in] pt The point
/// \tparam PointT The point type
-template<typename PointT>
+template <typename PointT>
inline auto x_(const PointT & pt)
{
return pt.x;
@@ -62,7 +64,7 @@ inline auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
/// \return The y value of the point
/// \param[in] pt The point
/// \tparam PointT The point type
-template<typename PointT>
+template <typename PointT>
inline auto y_(const PointT & pt)
{
return pt.y;
@@ -78,7 +80,7 @@ inline auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
/// \return The z value of the point
/// \param[in] pt The point
/// \tparam PointT The point type
-template<typename PointT>
+template <typename PointT>
inline auto z_(const PointT & pt)
{
return pt.z;
@@ -94,7 +96,7 @@ inline auto z_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
/// \return A reference to the x value of the point
/// \param[in] pt The point
/// \tparam PointT The point type
-template<typename PointT>
+template <typename PointT>
inline auto & xr_(PointT & pt)
{
return pt.x;
@@ -110,7 +112,7 @@ inline auto & xr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
/// \return A reference to The y value of the point
/// \param[in] pt The point
/// \tparam PointT The point type
-template<typename PointT>
+template <typename PointT>
inline auto & yr_(PointT & pt)
{
return pt.y;
@@ -126,7 +128,7 @@ inline auto & yr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
/// \return A reference to the z value of the point
/// \param[in] pt The point
/// \tparam PointT The point type
-template<typename PointT>
+template <typename PointT>
inline auto & zr_(PointT & pt)
{
return pt.z;
@@ -143,7 +145,7 @@ inline auto & zr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
namespace details
{
// Return the next iterator, cycling back to the beginning of the list if you hit the end
-template<typename IT>
+template <typename IT>
IT circular_next(const IT begin, const IT end, const IT current) noexcept
{
auto next = std::next(current);
@@ -155,14 +157,13 @@ IT circular_next(const IT begin, const IT end, const IT current) noexcept
} // namespace details
-
/// \tparam T1, T2, T3 point type. Must have point adapters defined or have float members x and y
/// \brief compute whether line segment rp is counter clockwise relative to line segment qp
/// \param[in] pt shared point for both line segments
/// \param[in] r point to check if it forms a ccw angle
/// \param[in] q reference point
/// \return whether angle formed is ccw. Three collinear points is considered ccw
-template<typename T1, typename T2, typename T3>
+template <typename T1, typename T2, typename T3>
inline auto ccw(const T1 & pt, const T2 & q, const T3 & r)
{
using point_adapter::x_;
@@ -175,7 +176,7 @@ inline auto ccw(const T1 & pt, const T2 & q, const T3 & r)
/// \param[in] pt first point
/// \param[in] q second point
/// \return 2d cross product
-template<typename T1, typename T2>
+template <typename T1, typename T2>
inline auto cross_2d(const T1 & pt, const T2 & q)
{
using point_adapter::x_;
@@ -188,7 +189,7 @@ inline auto cross_2d(const T1 & pt, const T2 & q)
/// \param[in] pt first point
/// \param[in] q second point
/// \return 2d scalar dot product
-template<typename T1, typename T2>
+template <typename T1, typename T2>
inline auto dot_2d(const T1 & pt, const T2 & q)
{
using point_adapter::x_;
@@ -202,7 +203,7 @@ inline auto dot_2d(const T1 & pt, const T2 & q)
/// \param[in] q The right hand side
/// \return A point with the difference in the x and y fields, all other fields are default
/// initialized
-template<typename T>
+template <typename T>
T minus_2d(const T & p, const T & q)
{
T r;
@@ -218,7 +219,7 @@ T minus_2d(const T & p, const T & q)
/// \param[in] p The left hand side
/// \return A point with the negation in the x and y fields, all other fields are default
/// initialized
-template<typename T>
+template <typename T>
T minus_2d(const T & p)
{
T r;
@@ -232,7 +233,7 @@ T minus_2d(const T & p)
/// \param[in] q The right hand side
/// \return A point with the sum in the x and y fields, all other fields are default
/// initialized
-template<typename T>
+template <typename T>
T plus_2d(const T & p, const T & q)
{
T r;
@@ -249,7 +250,7 @@ T plus_2d(const T & p, const T & q)
/// \param[in] a The scalar value
/// \return A point with the scaled x and y fields, all other fields are default
/// initialized
-template<typename T>
+template <typename T>
T times_2d(const T & p, const float32_t a)
{
T r;
@@ -268,7 +269,7 @@ T times_2d(const T & p, const float32_t a)
/// \param[in] v direction of second line
/// \return intersection point
/// \throw std::runtime_error if lines are (nearly) collinear or parallel
-template<typename T>
+template <typename T>
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);
@@ -281,19 +282,18 @@ inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v)
} else {
// parallel case, no valid output
throw std::runtime_error(
- "intersection_2d: no unique solution (either collinear or parallel)");
+ "intersection_2d: no unique solution (either collinear or parallel)");
}
}
return plus_2d(q, times_2d(v, num / den));
}
-
/// \tparam T point type. Must have point adapters defined or have float members x and y
/// \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
-template<typename T>
+template <typename T>
inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th)
{
const float32_t x = point_adapter::x_(pt);
@@ -307,7 +307,7 @@ inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th)
/// \param[in] pt reference point to rotate
/// \param[in] th_rad angle by which to rotate point
/// \return rotated point
-template<typename T>
+template <typename T>
inline T rotate_2d(const T & pt, const float32_t th_rad)
{
T q(pt); // It's reasonable to expect a copy constructor
@@ -322,7 +322,7 @@ inline T rotate_2d(const T & pt, const float32_t th_rad)
/// 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)
-template<typename T>
+template <typename T>
inline T get_normal(const T & pt)
{
T q(pt);
@@ -335,7 +335,7 @@ inline T get_normal(const T & pt)
/// \brief get magnitude of x and y components:
/// \param[in] pt point to get magnitude of
/// \return magitude of x and y components together
-template<typename T>
+template <typename T>
inline auto norm_2d(const T & pt)
{
return sqrtf(static_cast<float32_t>(dot_2d(pt, pt)));
@@ -349,7 +349,7 @@ inline auto norm_2d(const T & pt)
/// \param[in] q Second point defining the line segment
/// \param[in] r Reference point to find the closest point to
/// \return Closest point on line segment p-q to point r
-template<typename T>
+template <typename T>
inline T closest_segment_point_2d(const T & p, const T & q, const T & r)
{
const T qp = minus_2d(q, p);
@@ -373,7 +373,7 @@ inline T closest_segment_point_2d(const T & p, const T & q, const T & r)
/// \return Closest point on line p-q to point r
/// \throw std::runtime_error if the two points coincide and hence don't uniquely
// define a line
-template<typename T>
+template <typename T>
inline T closest_line_point_2d(const T & p, const T & q, const T & r)
{
const T qp = minus_2d(q, p);
@@ -384,7 +384,7 @@ inline T closest_line_point_2d(const T & p, const T & q, const T & r)
ret = plus_2d(p, times_2d(qp, t));
} else {
throw std::runtime_error(
- "closet_line_point_2d: line ill-defined because given points coincide");
+ "closet_line_point_2d: line ill-defined because given points coincide");
}
return ret;
}
@@ -395,7 +395,7 @@ inline T closest_line_point_2d(const T & p, const T & q, const T & r)
/// \param[in] q Second point defining the line segment
/// \param[in] r Reference point to find the distance from the line segment to
/// \return Distance from point r to line segment p-q
-template<typename T>
+template <typename T>
inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r)
{
const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r);
@@ -406,7 +406,7 @@ inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r
/// \tparam T Point type. Must have point adapters defined or have float members x and y
/// \param th Angle in radians
/// \return Unit vector in the direction of the given angle.
-template<typename T>
+template <typename T>
inline T make_unit_vector2d(float th)
{
T r;
@@ -422,7 +422,7 @@ inline T make_unit_vector2d(float th)
/// \param a point 1
/// \param b point 2
/// \return squared euclidean distance
-template<typename OUT = float32_t, typename T1, typename T2>
+template <typename OUT = float32_t, typename T1, typename T2>
inline OUT squared_distance_2d(const T1 & a, const T2 & b)
{
const auto x = static_cast<OUT>(point_adapter::x_(a)) - static_cast<OUT>(point_adapter::x_(b));
@@ -437,7 +437,7 @@ inline OUT squared_distance_2d(const T1 & a, const T2 & b)
/// \param a point 1
/// \param b point 2
/// \return euclidean distance
-template<typename OUT = float32_t, typename T1, typename T2>
+template <typename OUT = float32_t, typename T1, typename T2>
inline OUT distance_2d(const T1 & a, const T2 & b)
{
return std::sqrt(squared_distance_2d<OUT>(a, b));
@@ -452,7 +452,7 @@ inline OUT distance_2d(const T1 & a, const T2 & b)
/// \return > 0 for point q left of the line through p1 to p2
/// = 0 for point q on the line through p1 to p2
/// < 0 for point q right of the line through p1 to p2
-template<typename T>
+template <typename T>
inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q)
{
return cross_2d(minus_2d(p2, p1), minus_2d(q, p1));
@@ -465,7 +465,7 @@ inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T
/// \param[in] end One past the last of the point sequence
/// \return Whether or not all point triples p_i, p_{i+1}, p_{i+2} are in a particular order.
/// Returns true for collinear points as well
-template<typename IT>
+template <typename IT>
bool all_ordered(const IT begin, const IT end) noexcept
{
// Short circuit: a line or point is always CCW or otherwise ill-defined
@@ -479,9 +479,7 @@ bool all_ordered(const IT begin, const IT end) noexcept
const auto query_point = details::circular_next(begin, end, line_end);
// Check if 3 points starting from current point are in clockwise direction
const bool is_cw = comparison::abs_lte(
- check_point_position_to_line_2d(*line_start, *line_end, *query_point),
- 0.0F, 1e-3F
- );
+ check_point_position_to_line_2d(*line_start, *line_end, *query_point), 0.0F, 1e-3F);
if (is_cw) {
if (line_start == begin) {
is_first_point_cw = true;
@@ -502,7 +500,7 @@ bool all_ordered(const IT begin, const IT end) noexcept
/// \param[in] begin Iterator pointing to the beginning of the polygon points
/// \param[in] end Iterator pointing to one past the last of the polygon points
/// \return The area of the polygon, in squared of whatever units your points are in
-template<typename IT>
+template <typename IT>
auto area_2d(const IT begin, const IT end) noexcept
{
using point_adapter::x_;
@@ -515,7 +513,7 @@ auto area_2d(const IT begin, const IT end) noexcept
area += x_(*it) * y_(*next);
area -= x_(*next) * y_(*it);
}
- return std::abs(T{0.5} *area);
+ return std::abs(T{0.5} * area);
}
/// Compute area of convex hull, throw if points are not ordered (convexity check is not
@@ -525,7 +523,7 @@ auto area_2d(const IT begin, const IT end) noexcept
/// \param[in] begin Iterator pointing to the beginning of the polygon points
/// \param[in] end Iterator pointing to one past the last of the polygon points
/// \return The area of the polygon, in squared of whatever units your points are in
-template<typename IT>
+template <typename IT>
auto area_checked_2d(const IT begin, const IT end)
{
if (!all_ordered(begin, end)) {
@@ -543,7 +541,7 @@ auto area_checked_2d(const IT begin, const IT end)
/// order.
/// \param p point to be searched
/// \return True if the point is inside or on the edge of the polygon. False otherwise
-template<typename IteratorType, typename PointType>
+template <typename IteratorType, typename PointType>
bool is_point_inside_polygon_2d(
const IteratorType & start_it, const IteratorType & end_it, const PointType & p)
{
@@ -560,10 +558,7 @@ bool is_point_inside_polygon_2d(
if (comparison::abs_gt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) {
++winding_num;
} else {
- if (comparison::abs_eq_zero(
- check_point_position_to_line_2d(*it, *next_it, p),
- 1e-3F))
- {
+ if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) {
return true;
}
}
@@ -574,10 +569,7 @@ bool is_point_inside_polygon_2d(
if (comparison::abs_lt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) {
--winding_num;
} else {
- if (comparison::abs_eq_zero(
- check_point_position_to_line_2d(*it, *next_it, p),
- 1e-3F))
- {
+ if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) {
return true;
}
}
@@ -587,7 +579,6 @@ bool is_point_inside_polygon_2d(
return winding_num != 0;
}
-
} // namespace geometry
} // namespace common
} // namespace autoware
diff --git a/common/autoware_auto_geometry/include/geometry/common_3d.hpp b/common/autoware_auto_geometry/include/geometry/common_3d.hpp
index 9ba839f11d352..d38e5ca8f5daf 100644
--- a/common/autoware_auto_geometry/include/geometry/common_3d.hpp
+++ b/common/autoware_auto_geometry/include/geometry/common_3d.hpp
@@ -33,7 +33,7 @@ namespace geometry
/// \param[in] pt first point
/// \param[in] q second point
/// \return 3d scalar dot product
-template<typename T1, typename T2>
+template <typename T1, typename T2>
inline auto dot_3d(const T1 & pt, const T2 & q)
{
using point_adapter::x_;
@@ -49,7 +49,7 @@ inline auto dot_3d(const T1 & pt, const T2 & q)
/// \param a point 1
/// \param b point 2
/// \return squared 3D euclidean distance
-template<typename OUT = float32_t, typename T1, typename T2>
+template <typename OUT = float32_t, typename T1, typename T2>
inline OUT squared_distance_3d(const T1 & a, const T2 & b)
{
const auto x = static_cast<OUT>(point_adapter::x_(a)) - static_cast<OUT>(point_adapter::x_(b));
@@ -64,7 +64,7 @@ inline OUT squared_distance_3d(const T1 & a, const T2 & b)
/// \param a point 1
/// \param b point 2
/// \return 3D euclidean distance
-template<typename OUT = float32_t, typename T1, typename T2>
+template <typename OUT = float32_t, typename T1, typename T2>
inline OUT distance_3d(const T1 & a, const T2 & b)
{
return std::sqrt(squared_distance_3d<OUT>(a, b));
diff --git a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/geometry/convex_hull.hpp
index e780c2a23ee49..97e3871c82d9c 100644
--- a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp
+++ b/common/autoware_auto_geometry/include/geometry/convex_hull.hpp
@@ -24,11 +24,11 @@
#include <common/types.hpp>
#include <geometry/common_2d.hpp>
-//lint -e537 NOLINT pclint vs cpplint
+// lint -e537 NOLINT pclint vs cpplint
#include <algorithm>
-//lint -e537 NOLINT pclint vs cpplint
-#include <list>
+// lint -e537 NOLINT pclint vs cpplint
#include <limits>
+#include <list>
#include <utility>
using autoware::common::types::float32_t;
@@ -49,14 +49,14 @@ namespace details
/// (for splice)
/// \tparam PointT The point type for the points list
/// \tparam HullT the point type for the hull list
-template<typename PointT, typename HullT>
+template <typename PointT, typename HullT>
void form_lower_hull(std::list<PointT> & points, std::list<HullT> & hull)
{
auto hull_it = hull.cbegin();
auto point_it = points.cbegin();
// This ensures that the points we splice to back to the end of list are not processed
const auto iters = points.size();
- for (auto idx = decltype(iters) {0}; idx < iters; ++idx) {
+ for (auto idx = decltype(iters){0}; idx < iters; ++idx) {
// splice points from tail of hull to tail of list until point from head of list satisfies ccw
bool8_t is_ccw = true;
while ((hull.cbegin() != hull_it) && is_ccw) {
@@ -87,7 +87,7 @@ void form_lower_hull(std::list<PointT> & points, std::list<HullT> & hull)
/// and to contain the lower hull (minus the left-most point)
/// \tparam PointT The point type for the points list
/// \tparam HullT the point type for the hull list
-template<typename PointT, typename HullT>
+template <typename PointT, typename HullT>
void form_upper_hull(std::list<PointT> & points, std::list<HullT> & hull)
{
// TODO(c.ho) consider reverse iterators, not sure if they work with splice()
@@ -98,7 +98,7 @@ void form_upper_hull(std::list<PointT> & points, std::list<HullT> & hull)
--point_it;
// This ensures that the points we splice to back to the head of list are not processed
const auto iters = points.size();
- for (auto idx = decltype(iters) {0}; idx < iters; ++idx) {
+ for (auto idx = decltype(iters){0}; idx < iters; ++idx) {
// splice points from tail of hull to head of list until ccw is satisfied with tail of list
bool8_t is_ccw = true;
while ((lower_hull_end != hull_it) && is_ccw) {
@@ -129,18 +129,16 @@ void form_upper_hull(std::list<PointT> & points, std::list<HullT> & hull)
/// \param[inout] list A list of nodes that will be pruned down and reordered into a ccw convex hull
/// \return An iterator pointing to one after the last point contained in the hull
/// \tparam PointT Type of a point, must have x and y float members
-template<typename PointT>
+template <typename PointT>
typename std::list<PointT>::const_iterator convex_hull_impl(std::list<PointT> & list)
{
// Functor that return whether a <= b in the lexical sense (a.x < b.x), sort by y if tied
- const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t
- {
- using point_adapter::x_;
- using point_adapter::y_;
- constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
- return (fabsf(x_(a) - x_(b)) > FEPS) ?
- (x_(a) < x_(b)) : (y_(a) < y_(b));
- };
+ const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t {
+ using point_adapter::x_;
+ using point_adapter::y_;
+ constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
+ return (fabsf(x_(a) - x_(b)) > FEPS) ? (x_(a) < x_(b)) : (y_(a) < y_(b));
+ };
list.sort(lexical_comparator);
// Temporary list to store points
@@ -181,7 +179,7 @@ typename std::list<PointT>::const_iterator convex_hull_impl(std::list<PointT> &
/// \param[inout] list A list of nodes that will be reordered into a ccw convex hull
/// \return An iterator pointing to one after the last point contained in the hull
/// \tparam PointT Type of a point, must have x and y float members
-template<typename PointT>
+template <typename PointT>
typename std::list<PointT>::const_iterator convex_hull(std::list<PointT> & list)
{
return (list.size() <= 3U) ? list.end() : details::convex_hull_impl(list);
diff --git a/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp
index 39aad00798229..edd2e5ed2e517 100644
--- a/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp
+++ b/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp
@@ -23,11 +23,12 @@
#include <common/types.hpp>
#include <geometry/common_2d.hpp>
-#include <vector>
-#include <utility>
-#include <limits>
+
#include <algorithm>
#include <iterator>
+#include <limits>
+#include <utility>
+#include <vector>
using autoware::common::types::float32_t;
@@ -38,7 +39,6 @@ namespace common
namespace geometry
{
-
/// \brief Compute a list of "pockets" of a simple polygon
/// (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make
/// up the difference between the polygon and its convex hull.
@@ -59,13 +59,10 @@ namespace geometry
///
/// \tparam Iter1 Iterator to a point type
/// \tparam Iter2 Iterator to a point type (can be the same as Iter1, but does not need to be)
-template<typename Iter1, typename Iter2>
+template <typename Iter1, typename Iter2>
typename std::vector<std::vector<typename std::iterator_traits<Iter1>::value_type>> hull_pockets(
- const Iter1 polygon_start,
- const Iter1 polygon_end,
- const Iter2 convex_hull_start,
- const Iter2 convex_hull_end
-)
+ const Iter1 polygon_start, const Iter1 polygon_end, const Iter2 convex_hull_start,
+ const Iter2 convex_hull_end)
{
auto pockets = std::vector<std::vector<typename std::iterator_traits<Iter1>::value_type>>{};
if (std::distance(polygon_start, polygon_end) <= 3) {
@@ -74,15 +71,10 @@ typename std::vector<std::vector<typename std::iterator_traits<Iter1>::value_typ
// Function to check whether a point is in the convex hull.
const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) {
- return std::any_of(
- convex_hull_start, convex_hull_end, [p](auto hull_entry)
- {
- return norm_2d(
- minus_2d(
- hull_entry,
- *p)) < std::numeric_limits<float32_t>::epsilon();
- });
- };
+ return std::any_of(convex_hull_start, convex_hull_end, [p](auto hull_entry) {
+ return norm_2d(minus_2d(hull_entry, *p)) < std::numeric_limits<float32_t>::epsilon();
+ });
+ };
// We go through the points of the polygon only once, adding pockets to the list of pockets
// as we detect them.
diff --git a/common/autoware_auto_geometry/include/geometry/intersection.hpp b/common/autoware_auto_geometry/include/geometry/intersection.hpp
index 5c172a08ccfdb..174d18cd2b62c 100644
--- a/common/autoware_auto_geometry/include/geometry/intersection.hpp
+++ b/common/autoware_auto_geometry/include/geometry/intersection.hpp
@@ -17,20 +17,20 @@
#ifndef GEOMETRY__INTERSECTION_HPP_
#define GEOMETRY__INTERSECTION_HPP_
+#include <geometry/common_2d.hpp>
+#include <geometry/convex_hull.hpp>
-#include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
#include <autoware_auto_perception_msgs/msg/bounding_box.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
-#include <geometry/convex_hull.hpp>
-#include <geometry/common_2d.hpp>
+#include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
-#include <limits>
-#include <vector>
+#include <algorithm>
#include <iostream>
+#include <limits>
#include <list>
-#include <utility>
#include <type_traits>
-#include <algorithm>
+#include <utility>
+#include <vector>
namespace autoware
{
@@ -38,15 +38,15 @@ namespace common
{
namespace geometry
{
-using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-using autoware_auto_perception_msgs::msg::BoundingBox;
+using autoware::common::geometry::closest_line_point_2d;
using autoware::common::geometry::convex_hull;
-using autoware::common::geometry::get_normal;
using autoware::common::geometry::dot_2d;
+using autoware::common::geometry::get_normal;
using autoware::common::geometry::minus_2d;
-using autoware::common::geometry::times_2d;
using autoware::common::geometry::norm_2d;
-using autoware::common::geometry::closest_line_point_2d;
+using autoware::common::geometry::times_2d;
+using autoware_auto_perception_msgs::msg::BoundingBox;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using Point = geometry_msgs::msg::Point32;
@@ -62,7 +62,7 @@ using Line = std::pair<Point, Point>;
/// \param[in] start Start iterator of the list of points
/// \param[in] end End iterator of the list of points
/// \return The list of faces
-template<typename Iter>
+template <typename Iter>
std::vector<Line> get_sorted_face_list(const Iter start, const Iter end)
{
// First get a sorted list of points - convex_hull does that by modifying its argument
@@ -84,44 +84,41 @@ std::vector<Line> get_sorted_face_list(const Iter start, const Iter end)
}
/// \brief Append points of the polygon `internal` that are contained in the polygon `exernal`.
-template<template<typename ...> class Iterable1T, template<typename ...> class Iterable2T,
- typename PointT>
+template <
+ template <typename...> class Iterable1T, template <typename...> class Iterable2T, typename PointT>
void append_contained_points(
- const Iterable1T<PointT> & external,
- const Iterable2T<PointT> & internal,
+ const Iterable1T<PointT> & external, const Iterable2T<PointT> & internal,
std::list<PointT> & result)
{
std::copy_if(
- internal.begin(), internal.end(), std::back_inserter(result),
- [&external](const auto & pt) {
+ internal.begin(), internal.end(), std::back_inserter(result), [&external](const auto & pt) {
return common::geometry::is_point_inside_polygon_2d(external.begin(), external.end(), pt);
});
}
/// \brief Append the intersecting points between two polygons into the output list.
-template<template<typename ...> class Iterable1T, template<typename ...> class Iterable2T,
- typename PointT>
+template <
+ template <typename...> class Iterable1T, template <typename...> class Iterable2T, typename PointT>
void append_intersection_points(
- const Iterable1T<PointT> & polygon1,
- const Iterable2T<PointT> & polygon2,
+ const Iterable1T<PointT> & polygon1, const Iterable2T<PointT> & polygon2,
std::list<PointT> & result)
{
using FloatT = decltype(point_adapter::x_(std::declval<PointT>()));
using Interval = common::geometry::Interval<float32_t>;
auto get_edge = [](const auto & list, const auto & iterator) {
- const auto next_it = std::next(iterator);
- const auto & next_pt = (next_it != list.end()) ? *next_it : list.front();
- return std::make_pair(*iterator, next_pt);
- };
+ const auto next_it = std::next(iterator);
+ const auto & next_pt = (next_it != list.end()) ? *next_it : list.front();
+ return std::make_pair(*iterator, next_pt);
+ };
// Get the max absolute value out of two intervals and a scalar.
auto compute_eps_scale = [](const auto & i1, const auto & i2, const auto val) {
- auto get_abs_max = [](const auto & interval) {
- return std::max(std::fabs(Interval::min(interval)), std::fabs(Interval::max(interval)));
- };
- return std::max(std::fabs(val), std::max(get_abs_max(i1), get_abs_max(i2)));
+ auto get_abs_max = [](const auto & interval) {
+ return std::max(std::fabs(Interval::min(interval)), std::fabs(Interval::max(interval)));
};
+ return std::max(std::fabs(val), std::max(get_abs_max(i1), get_abs_max(i2)));
+ };
// Compare each edge from polygon1 to each edge from polygon2
for (auto corner1_it = polygon1.begin(); corner1_it != polygon1.end(); ++corner1_it) {
@@ -138,10 +135,9 @@ void append_intersection_points(
for (auto corner2_it = polygon2.begin(); corner2_it != polygon2.end(); ++corner2_it) {
try {
const auto & edge2 = get_edge(polygon2, corner2_it);
- const auto & intersection =
- common::geometry::intersection_2d(
- edge1.first, minus_2d(edge1.second, edge1.first),
- edge2.first, minus_2d(edge2.second, edge2.first));
+ const auto & intersection = common::geometry::intersection_2d(
+ edge1.first, minus_2d(edge1.second, edge1.first), edge2.first,
+ minus_2d(edge2.second, edge2.first));
Interval edge2_x_interval{
std::min(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second)),
@@ -156,15 +152,14 @@ void append_intersection_points(
// while computing the epsilon.
const auto max_feps_scale = std::max(
compute_eps_scale(edge1_x_interval, edge2_x_interval, point_adapter::x_(intersection)),
- compute_eps_scale(edge1_y_interval, edge2_y_interval, point_adapter::y_(intersection))
- );
+ compute_eps_scale(edge1_y_interval, edge2_y_interval, point_adapter::y_(intersection)));
const auto feps = max_feps_scale * std::numeric_limits<FloatT>::epsilon();
// Only accept intersections that lie on both of the line segments (edges)
- if (Interval::contains(edge1_x_interval, point_adapter::x_(intersection), feps) &&
+ if (
+ Interval::contains(edge1_x_interval, point_adapter::x_(intersection), feps) &&
Interval::contains(edge2_x_interval, point_adapter::x_(intersection), feps) &&
Interval::contains(edge1_y_interval, point_adapter::y_(intersection), feps) &&
- Interval::contains(edge2_y_interval, point_adapter::y_(intersection), feps))
- {
+ Interval::contains(edge2_y_interval, point_adapter::y_(intersection), feps)) {
result.push_back(intersection);
}
} catch (const std::runtime_error &) {
@@ -175,7 +170,6 @@ void append_intersection_points(
}
}
-
} // namespace details
// TODO(s.me) implement GJK(+EPA) algorithm as well as per Chris Ho's suggestion
@@ -189,41 +183,38 @@ void append_intersection_points(
/// \param[in] begin2 Start iterator to first list of point types
/// \param[in] end2 End iterator to first list of point types
/// \return true if the boxes collide, false otherwise.
-template<typename Iter>
+template <typename Iter>
bool intersect(const Iter begin1, const Iter end1, const Iter begin2, const Iter end2)
{
// Obtain sorted lists of faces of both boxes, merge them into one big list of faces
auto faces = details::get_sorted_face_list(begin1, end1);
const auto faces_2 = details::get_sorted_face_list(begin2, end2);
faces.reserve(faces.size() + faces_2.size());
- faces.insert(faces.end(), faces_2.begin(), faces_2.end() );
+ faces.insert(faces.end(), faces_2.begin(), faces_2.end());
// Also look at last line
for (const auto & face : faces) {
// Compute normal vector to the face and define a closure to get progress along it
const auto normal = get_normal(minus_2d(face.second, face.first));
- auto get_position_along_line = [&normal](auto point)
- {
- return dot_2d(normal, minus_2d(point, Point{}) );
- };
+ auto get_position_along_line = [&normal](auto point) {
+ return dot_2d(normal, minus_2d(point, Point{}));
+ };
// Define a function to get the minimum and maximum projected position of the corners
// of a given bounding box along the normal line of the face
- auto get_projected_min_max = [&get_position_along_line, &normal](Iter begin, Iter end)
- {
- const auto zero_point = Point{};
- auto min_corners =
- get_position_along_line(closest_line_point_2d(normal, zero_point, *begin));
- auto max_corners = min_corners;
-
- for (auto & point = begin; point != end; ++point) {
- const auto point_projected = closest_line_point_2d(normal, zero_point, *point);
- const auto position_along_line = get_position_along_line(point_projected);
- min_corners = std::min(min_corners, position_along_line);
- max_corners = std::max(max_corners, position_along_line);
- }
- return std::pair<float, float>{min_corners, max_corners};
- };
+ auto get_projected_min_max = [&get_position_along_line, &normal](Iter begin, Iter end) {
+ const auto zero_point = Point{};
+ auto min_corners = get_position_along_line(closest_line_point_2d(normal, zero_point, *begin));
+ auto max_corners = min_corners;
+
+ for (auto & point = begin; point != end; ++point) {
+ const auto point_projected = closest_line_point_2d(normal, zero_point, *point);
+ const auto position_along_line = get_position_along_line(point_projected);
+ min_corners = std::min(min_corners, position_along_line);
+ max_corners = std::max(max_corners, position_along_line);
+ }
+ return std::pair<float, float>{min_corners, max_corners};
+ };
// Perform the actual computations for the extent computation
auto minmax_1 = get_projected_min_max(begin1, end1);
@@ -260,11 +251,10 @@ bool intersect(const Iter begin1, const Iter end1, const Iter begin2, const Iter
/// \param polygon1 A convex polygon
/// \param polygon2 A convex polygon
/// \return The resulting conv
-template<template<typename ...> class Iterable1T, template<typename ...> class Iterable2T,
- typename PointT>
+template <
+ template <typename...> class Iterable1T, template <typename...> class Iterable2T, typename PointT>
std::list<PointT> convex_polygon_intersection2d(
- const Iterable1T<PointT> & polygon1,
- const Iterable2T<PointT> & polygon2)
+ const Iterable1T<PointT> & polygon1, const Iterable2T<PointT> & polygon2)
{
std::list<PointT> result;
details::append_contained_points(polygon1, polygon2, result);
@@ -275,7 +265,6 @@ std::list<PointT> convex_polygon_intersection2d(
return result;
}
-
/// \brief Compute the intersection over union of two 2d convex polygons. If any of the polygons
/// span a zero area, the result is 0.0.
/// \tparam Iterable1T A container class that has stl style iterators defined.
@@ -287,12 +276,10 @@ std::list<PointT> convex_polygon_intersection2d(
/// \return (Intersection / Union) between two given polygons.
/// \throws std::domain_error If there is any inconsistency on the undderlying geometrical
/// computation.
-template<template<typename ...> class Iterable1T, template<typename ...> class Iterable2T,
- typename PointT>
+template <
+ template <typename...> class Iterable1T, template <typename...> class Iterable2T, typename PointT>
common::types::float32_t convex_intersection_over_union_2d(
- const Iterable1T<PointT> & polygon1,
- const Iterable2T<PointT> & polygon2
-)
+ const Iterable1T<PointT> & polygon1, const Iterable2T<PointT> & polygon2)
{
constexpr auto eps = std::numeric_limits<float32_t>::epsilon();
const auto intersection = convex_polygon_intersection2d(polygon1, polygon2);
@@ -304,10 +291,8 @@ common::types::float32_t convex_intersection_over_union_2d(
return 0.0F; // There's either no intersection or the points are collinear
}
- const auto polygon1_area =
- common::geometry::area_2d(polygon1.begin(), polygon1.end());
- const auto polygon2_area =
- common::geometry::area_2d(polygon2.begin(), polygon2.end());
+ const auto polygon1_area = common::geometry::area_2d(polygon1.begin(), polygon1.end());
+ const auto polygon2_area = common::geometry::area_2d(polygon2.begin(), polygon2.end());
const auto union_area = polygon1_area + polygon2_area - intersection_area;
if (union_area < eps) {
diff --git a/common/autoware_auto_geometry/include/geometry/interval.hpp b/common/autoware_auto_geometry/include/geometry/interval.hpp
index a15fb52b42612..10cab1a1d7b79 100644
--- a/common/autoware_auto_geometry/include/geometry/interval.hpp
+++ b/common/autoware_auto_geometry/include/geometry/interval.hpp
@@ -21,6 +21,9 @@
#ifndef GEOMETRY__INTERVAL_HPP_
#define GEOMETRY__INTERVAL_HPP_
+#include "common/types.hpp"
+#include "helper_functions/float_comparisons.hpp"
+
#include <algorithm>
#include <cmath>
#include <iostream>
@@ -31,9 +34,6 @@
#include <type_traits>
#include <utility>
-#include "common/types.hpp"
-#include "helper_functions/float_comparisons.hpp"
-
namespace autoware
{
namespace common
@@ -48,12 +48,10 @@ namespace geometry
* interval [min, max] is "valid" if it is empty (min/max = NaN) or its bounds
* are ordered (min <= max).
*/
-template<typename T>
+template <typename T>
class Interval
{
- static_assert(
- std::is_floating_point<T>::value,
- "Intervals only support floating point types.");
+ static_assert(std::is_floating_point<T>::value, "Intervals only support floating point types.");
public:
/**
@@ -79,10 +77,7 @@ class Interval
* @brief Compute inequality and the logical negation of equality.
* @note This is defined inline because the class is templated.
*/
- friend bool operator!=(const Interval & i1, const Interval & i2)
- {
- return !(i1 == i2);
- }
+ friend bool operator!=(const Interval & i1, const Interval & i2) { return !(i1 == i2); }
/**
* @brief Stream overload for Interval types.
@@ -99,26 +94,26 @@ class Interval
// Internal helper to format the output.
const auto readable_extremum = [](const T & val) {
- if (val == std::numeric_limits<T>::lowest()) {
- return std::string("REAL_MIN");
- }
- if (val == std::numeric_limits<T>::max()) {
- return std::string("REAL_MAX");
- }
-
- std::stringstream ss;
- ss.setf(std::ios::fixed, std::ios::floatfield);
- ss.precision(PRECISION);
- ss << val;
- return ss.str();
- };
+ if (val == std::numeric_limits<T>::lowest()) {
+ return std::string("REAL_MIN");
+ }
+ if (val == std::numeric_limits<T>::max()) {
+ return std::string("REAL_MAX");
+ }
+
+ std::stringstream ss;
+ ss.setf(std::ios::fixed, std::ios::floatfield);
+ ss.precision(PRECISION);
+ ss << val;
+ return ss.str();
+ };
// Do stream output.
if (Interval::empty(i)) {
return os << "{}";
}
- return os << "{\"min\": " << readable_extremum(Interval::min(i)) <<
- ", \"max\": " << readable_extremum(Interval::max(i)) << "}";
+ return os << "{\"min\": " << readable_extremum(Interval::min(i))
+ << ", \"max\": " << readable_extremum(Interval::max(i)) << "}";
}
/**
@@ -131,10 +126,10 @@ class Interval
static bool abs_eq(const Interval & i1, const Interval & i2, const T & eps);
/** @brief The minimum bound of the interval. */
- static T min(const Interval & i) {return i.min_;}
+ static T min(const Interval & i) { return i.min_; }
/** @brief The maximum bound of the interval. */
- static T max(const Interval & i) {return i.max_;}
+ static T max(const Interval & i) { return i.max_; }
/**
* @brief Return the measure (length) of the interval.
@@ -171,8 +166,7 @@ class Interval
* @return True iff 'interval' contains 'value'.
*/
static bool contains(
- const Interval & i, const T & value,
- const T & eps = std::numeric_limits<T>::epsilon());
+ const Interval & i, const T & value, const T & eps = std::numeric_limits<T>::epsilon());
/**
* @brief Test for whether 'i1' subseteq 'i2'
@@ -229,20 +223,20 @@ typedef Interval<autoware::common::types::float32_t> Interval_f;
//------------------------------------------------------------------------------
-template<typename T>
+template <typename T>
constexpr T Interval<T>::NaN;
//------------------------------------------------------------------------------
-template<typename T>
-Interval<T>::Interval()
-: min_(Interval::NaN), max_(Interval::NaN) {}
+template <typename T>
+Interval<T>::Interval() : min_(Interval::NaN), max_(Interval::NaN)
+{
+}
//------------------------------------------------------------------------------
-template<typename T>
-Interval<T>::Interval(const T & min, const T & max)
-: min_(min), max_(max)
+template <typename T>
+Interval<T>::Interval(const T & min, const T & max) : min_(min), max_(max)
{
if (!Interval::bounds_valid(*this)) {
std::stringstream ss;
@@ -253,9 +247,8 @@ Interval<T>::Interval(const T & min, const T & max)
//------------------------------------------------------------------------------
-template<typename T>
-bool Interval<T>::abs_eq(
- const Interval & i1, const Interval & i2, const T & eps)
+template <typename T>
+bool Interval<T>::abs_eq(const Interval & i1, const Interval & i2, const T & eps)
{
namespace comp = helper_functions::comparisons;
@@ -271,7 +264,7 @@ bool Interval<T>::abs_eq(
//------------------------------------------------------------------------------
-template<typename T>
+template <typename T>
bool Interval<T>::zero_measure(const Interval & i)
{
// An empty interval will return false because (NaN == NaN) is false.
@@ -280,7 +273,7 @@ bool Interval<T>::zero_measure(const Interval & i)
//------------------------------------------------------------------------------
-template<typename T>
+template <typename T>
bool Interval<T>::empty(const Interval & i)
{
return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i));
@@ -288,7 +281,7 @@ bool Interval<T>::empty(const Interval & i)
//------------------------------------------------------------------------------
-template<typename T>
+template <typename T>
bool Interval<T>::bounds_valid(const Interval & i)
{
const auto is_ordered = (Interval::min(i) <= Interval::max(i));
@@ -299,7 +292,7 @@ bool Interval<T>::bounds_valid(const Interval & i)
//------------------------------------------------------------------------------
-template<typename T>
+template <typename T>
bool Interval<T>::is_subset_eq(const Interval & i1, const Interval & i2)
{
const auto lower_contained = (Interval::min(i1) >= Interval::min(i2));
@@ -309,7 +302,7 @@ bool Interval<T>::is_subset_eq(const Interval & i1, const Interval & i2)
//------------------------------------------------------------------------------
-template<typename T>
+template <typename T>
bool Interval<T>::contains(const Interval & i, const T & value, const T & eps)
{
return common::helper_functions::comparisons::abs_gte(value, Interval::min(i), eps) &&
@@ -318,7 +311,7 @@ bool Interval<T>::contains(const Interval & i, const T & value, const T & eps)
//------------------------------------------------------------------------------
-template<typename T>
+template <typename T>
T Interval<T>::measure(const Interval & i)
{
return Interval::max(i) - Interval::min(i);
@@ -326,7 +319,7 @@ T Interval<T>::measure(const Interval & i)
//------------------------------------------------------------------------------
-template<typename T>
+template <typename T>
Interval<T> Interval<T>::intersect(const Interval & i1, const Interval & i2)
{
// Construct intersection assuming an intersection exists.
@@ -344,7 +337,7 @@ Interval<T> Interval<T>::intersect(const Interval & i1, const Interval & i2)
//------------------------------------------------------------------------------
-template<typename T>
+template <typename T>
T Interval<T>::clamp_to(const Interval & i, T val)
{
// clamp val to min
diff --git a/common/autoware_auto_geometry/include/geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/geometry/lookup_table.hpp
index 6d97f392a14c9..0038623802f5a 100644
--- a/common/autoware_auto_geometry/include/geometry/lookup_table.hpp
+++ b/common/autoware_auto_geometry/include/geometry/lookup_table.hpp
@@ -20,13 +20,13 @@
#ifndef GEOMETRY__LOOKUP_TABLE_HPP_
#define GEOMETRY__LOOKUP_TABLE_HPP_
+#include "common/types.hpp"
+#include "geometry/interval.hpp"
+
#include <cmath>
#include <stdexcept>
#include <vector>
-#include "common/types.hpp"
-#include "geometry/interval.hpp"
-
namespace autoware
{
namespace common
@@ -41,7 +41,7 @@ namespace
* @brief Compute a scaling between 'a' and 'b'
* @note scaling is clamped to be in the interval [0, 1]
*/
-template<typename T, typename U>
+template <typename T, typename U>
T interpolate(const T & a, const T & b, U scaling)
{
static const geometry::Interval<U> UNIT_INTERVAL(static_cast<U>(0), static_cast<U>(1));
@@ -55,7 +55,7 @@ T interpolate(const T & a, const T & b, U scaling)
// TODO(c.ho) support more forms of interpolation as template functor
// Actual lookup logic, assuming all invariants hold:
// Throw if value is not finite
-template<typename T>
+template <typename T>
T lookup_impl_1d(const std::vector<T> & domain, const std::vector<T> & range, const T value)
{
if (!std::isfinite(value)) {
@@ -85,7 +85,7 @@ T lookup_impl_1d(const std::vector<T> & domain, const std::vector<T> & range, co
}
// Check invariants for table lookup:
-template<typename T>
+template <typename T>
void check_table_lookup_invariants(const std::vector<T> & domain, const std::vector<T> & range)
{
if (domain.size() != range.size()) {
@@ -115,7 +115,7 @@ void check_table_lookup_invariants(const std::vector<T> & domain, const std::vec
/// \throw std::domain_error If domain is not sorted
/// \throw std::domain_error If value is not finite (NAN or INF)
/// \tparam T The type of the function, must be interpolatable
-template<typename T>
+template <typename T>
T lookup_1d(const std::vector<T> & domain, const std::vector<T> & range, const T value)
{
check_table_lookup_invariants(domain, range);
@@ -126,7 +126,7 @@ T lookup_1d(const std::vector<T> & domain, const std::vector<T> & range, const T
/// A class wrapping a 1D lookup table. Intended for more frequent lookups. Error checking is pushed
/// into the constructor and not done in the lookup function call
/// \tparam T The type of the function, must be interpolatable
-template<typename T>
+template <typename T>
class LookupTable1D
{
public:
@@ -137,8 +137,7 @@ class LookupTable1D
/// \throw std::domain_error If range is not the same size as domain
/// \throw std::domain_error If domain is not sorted
LookupTable1D(const std::vector<T> & domain, const std::vector<T> & range)
- : m_domain{domain},
- m_range{range}
+ : m_domain{domain}, m_range{range}
{
check_table_lookup_invariants(m_domain, m_range);
}
@@ -150,8 +149,7 @@ class LookupTable1D
/// \throw std::domain_error If range is not the same size as domain
/// \throw std::domain_error If domain is not sorted
LookupTable1D(std::vector<T> && domain, std::vector<T> && range)
- : m_domain{domain},
- m_range{range}
+ : m_domain{domain}, m_range{range}
{
check_table_lookup_invariants(m_domain, m_range);
}
@@ -162,14 +160,11 @@ class LookupTable1D
/// \param[in] value The point in the domain to query, x
/// \return A linearly interpolated value y, corresponding to the query, x
/// \throw std::domain_error If value is not finite
- T lookup(const T value) const
- {
- return lookup_impl_1d(m_domain, m_range, value);
- }
+ T lookup(const T value) const { return lookup_impl_1d(m_domain, m_range, value); }
/// Get the domain table
- const std::vector<T> & domain() const noexcept {return m_domain;}
+ const std::vector<T> & domain() const noexcept { return m_domain; }
/// Get the range table
- const std::vector<T> & range() const noexcept {return m_range;}
+ const std::vector<T> & range() const noexcept { return m_range; }
private:
std::vector<T> m_domain;
@@ -180,5 +175,4 @@ class LookupTable1D
} // namespace common
} // namespace autoware
-
#endif // GEOMETRY__LOOKUP_TABLE_HPP_
diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp
index 7a77f02738732..deebad84ef019 100644
--- a/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp
+++ b/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp
@@ -23,12 +23,13 @@
#include <common/types.hpp>
#include <geometry/spatial_hash_config.hpp>
#include <geometry/visibility_control.hpp>
-#include <vector>
+
#include <unordered_map>
#include <utility>
+#include <vector>
-using autoware::common::types::float32_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float32_t;
namespace autoware
{
@@ -47,11 +48,11 @@ namespace spatial_hash
/// This implementation can support both 2D and 3D queries
/// (though only one type per data structure), and can support queries of varying radius. This data
/// structure cannot do near neighbor lookups for euclidean distance in arbitrary dimensions.
-template<typename PointT, typename ConfigT>
+template <typename PointT, typename ConfigT>
class GEOMETRY_PUBLIC SpatialHashBase
{
using Index3 = details::Index3;
- //lint -e{9131} NOLINT There's no other way to make this work in a static assert
+ // lint -e{9131} NOLINT There's no other way to make this work in a static assert
static_assert(
std::is_same<ConfigT, Config2d>::value || std::is_same<ConfigT, Config3d>::value,
"SpatialHash only works with Config2d or Config3d");
@@ -62,47 +63,30 @@ class GEOMETRY_PUBLIC SpatialHashBase
/// \brief Wrapper around an iterator and a distance (from some query point)
class Output
{
-public:
+ public:
/// \brief Constructor
/// \param[in] iterator An iterator pointing to some point
/// \param[in] distance The euclidean distance (2d or 3d) to a reference point
- Output(const IT iterator, const float32_t distance)
- : m_iterator(iterator),
- m_distance(distance)
+ Output(const IT iterator, const float32_t distance) : m_iterator(iterator), m_distance(distance)
{
}
/// \brief Get stored point
/// \return A const reference to the stored point
- const PointT & get_point() const
- {
- return m_iterator->second;
- }
+ const PointT & get_point() const { return m_iterator->second; }
/// \brief Get underlying iterator
/// \return A copy of the underlying iterator
- IT get_iterator() const
- {
- return m_iterator;
- }
+ IT get_iterator() const { return m_iterator; }
/// \brief Convert to underlying point
/// \return A reference to the underlying point
- operator const PointT &() const
- {
- return get_point();
- }
+ operator const PointT &() const { return get_point(); }
/// \brief Convert to underlying iterator
/// \return A copy of the iterator
- operator IT() const
- {
- return get_iterator();
- }
+ operator IT() const { return get_iterator(); }
/// \brief Get distance to reference point
/// \return The distance
- float32_t get_distance() const
- {
- return m_distance;
- }
+ float32_t get_distance() const { return m_distance; }
-private:
+ private:
IT m_iterator;
float32_t m_distance;
}; // class Output
@@ -114,7 +98,7 @@ class GEOMETRY_PUBLIC SpatialHashBase
: m_config{cfg},
m_hash(),
m_neighbors{}, // TODO(c.ho) reserve, but there's no default constructor for output
- m_bins_hit{}, // zero initialization (and below)
+ m_bins_hit{}, // zero initialization (and below)
m_neighbors_found{}
{
}
@@ -137,7 +121,7 @@ class GEOMETRY_PUBLIC SpatialHashBase
/// \tparam IteratorT The iterator type
/// \throw std::length_error If the range of points to insert exceeds the data structure's
/// capacity
- template<typename IteratorT>
+ template <typename IteratorT>
void insert(IteratorT begin, IteratorT end)
{
// This check is here for strong exception safety
@@ -166,68 +150,38 @@ class GEOMETRY_PUBLIC SpatialHashBase
}
/// \brief Reset the state of the data structure
- void clear()
- {
- m_hash.clear();
- }
+ void clear() { m_hash.clear(); }
/// \brief Get current number of element stored in this data structure
/// \return Number of stored elements
- Index size() const
- {
- return m_hash.size();
- }
+ Index size() const { return m_hash.size(); }
/// \brief Get the maximum capacity of the data structure
/// \return The capacity of the data structure
- Index capacity() const
- {
- return m_config.get_capacity();
- }
+ Index capacity() const { return m_config.get_capacity(); }
/// \brief Whether the hash is empty
/// \return True if data structure is empty
- bool8_t empty() const
- {
- return m_hash.empty();
- }
+ bool8_t empty() const { return m_hash.empty(); }
/// \brief Get iterator to beginning of data structure
/// \return Iterator
- IT begin() const
- {
- return m_hash.begin();
- }
+ IT begin() const { return m_hash.begin(); }
/// \brief Get iterator to end of data structure
/// \return Iterator
- IT end() const
- {
- return m_hash.end();
- }
+ IT end() const { return m_hash.end(); }
/// \brief Get iterator to beginning of data structure
/// \return Iterator
- IT cbegin() const
- {
- return begin();
- }
+ IT cbegin() const { return begin(); }
/// \brief Get iterator to end of data structure
/// \return Iterator
- IT cend() const
- {
- return end();
- }
+ IT cend() const { return end(); }
/// \brief Get the number of bins touched during the lifetime of this object, for debugging and
/// size tuning
/// \return The total number of bins touched during near() queries
- Index bins_hit() const
- {
- return m_bins_hit;
- }
+ Index bins_hit() const { return m_bins_hit; }
/// \brief Get number of near neighbors found during the lifetime of this object, for debugging
/// and size tuning
/// \return The total number of neighbors found during near() queries
- Index neighbors_found() const
- {
- return m_neighbors_found;
- }
+ Index neighbors_found() const { return m_neighbors_found; }
protected:
/// \brief Finds all points within a fixed radius of a reference point
@@ -239,10 +193,7 @@ class GEOMETRY_PUBLIC SpatialHashBase
/// \return A const reference to a vector containing iterators pointing to
/// all points within the radius, and the actual distance to the reference point
const OutputVector & near_impl(
- const float32_t x,
- const float32_t y,
- const float32_t z,
- const float32_t radius)
+ const float32_t x, const float32_t y, const float32_t z, const float32_t radius)
{
// reset output
m_neighbors.clear();
@@ -298,19 +249,18 @@ class GEOMETRY_PUBLIC SpatialHashBase
/// apex_app::common::geometry::spatial_hash::SpatialHashBase to provide different function
/// signatures on 2D and 3D configurations
/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z
-template<typename PointT, typename ConfigT>
+template <typename PointT, typename ConfigT>
class GEOMETRY_PUBLIC SpatialHash;
/// \brief Explicit specialization of SpatialHash for 2D configuration
/// \tparam PointT The point type stored in this data structure.
-template<typename PointT>
-class GEOMETRY_PUBLIC SpatialHash<PointT, Config2d>: public SpatialHashBase<PointT, Config2d>
+template <typename PointT>
+class GEOMETRY_PUBLIC SpatialHash<PointT, Config2d> : public SpatialHashBase<PointT, Config2d>
{
public:
using OutputVector = typename SpatialHashBase<PointT, Config2d>::OutputVector;
- explicit SpatialHash(const Config2d & cfg)
- : SpatialHashBase<PointT, Config2d>(cfg) {}
+ explicit SpatialHash(const Config2d & cfg) : SpatialHashBase<PointT, Config2d>(cfg) {}
/// \brief Finds all points within a fixed radius of a reference point
/// \param[in] x The x component of the reference point
@@ -318,10 +268,7 @@ class GEOMETRY_PUBLIC SpatialHash<PointT, Config2d>: public SpatialHashBase<Poin
/// \param[in] radius The radius within which to find all near points
/// \return A const reference to a vector containing iterators pointing to
/// all points within the radius, and the actual distance to the reference point
- const OutputVector & near(
- const float32_t x,
- const float32_t y,
- const float32_t radius)
+ const OutputVector & near(const float32_t x, const float32_t y, const float32_t radius)
{
return this->near_impl(x, y, 0.0F, radius);
}
@@ -339,14 +286,13 @@ class GEOMETRY_PUBLIC SpatialHash<PointT, Config2d>: public SpatialHashBase<Poin
/// \brief Explicit specialization of SpatialHash for 3D configuration
/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z
-template<typename PointT>
-class GEOMETRY_PUBLIC SpatialHash<PointT, Config3d>: public SpatialHashBase<PointT, Config3d>
+template <typename PointT>
+class GEOMETRY_PUBLIC SpatialHash<PointT, Config3d> : public SpatialHashBase<PointT, Config3d>
{
public:
using OutputVector = typename SpatialHashBase<PointT, Config3d>::OutputVector;
- explicit SpatialHash(const Config3d & cfg)
- : SpatialHashBase<PointT, Config3d>(cfg) {}
+ explicit SpatialHash(const Config3d & cfg) : SpatialHashBase<PointT, Config3d>(cfg) {}
/// \brief Finds all points within a fixed radius of a reference point
/// \param[in] x The x component of the reference point
@@ -357,10 +303,7 @@ class GEOMETRY_PUBLIC SpatialHash<PointT, Config3d>: public SpatialHashBase<Poin
/// \return A const reference to a vector containing iterators pointing to
/// all points within the radius, and the actual distance to the reference point
const OutputVector & near(
- const float32_t x,
- const float32_t y,
- const float32_t z,
- const float32_t radius)
+ const float32_t x, const float32_t y, const float32_t z, const float32_t radius)
{
return this->near_impl(x, y, z, radius);
}
@@ -372,15 +315,13 @@ class GEOMETRY_PUBLIC SpatialHash<PointT, Config3d>: public SpatialHashBase<Poin
/// all points within the radius, and the actual distance to the reference point
const OutputVector & near(const PointT & pt, const float32_t radius)
{
- return near(
- point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt),
- radius);
+ return near(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt), radius);
}
};
-template<typename T>
+template <typename T>
using SpatialHash2d = SpatialHash<T, Config2d>;
-template<typename T>
+template <typename T>
using SpatialHash3d = SpatialHash<T, Config3d>;
} // namespace spatial_hash
} // namespace geometry
diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp
index 76ebb8e2da79d..bd5e562251c9d 100644
--- a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp
+++ b/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp
@@ -20,9 +20,11 @@
#ifndef GEOMETRY__SPATIAL_HASH_CONFIG_HPP_
#define GEOMETRY__SPATIAL_HASH_CONFIG_HPP_
+#include "helper_functions/crtp.hpp"
+
#include <common/types.hpp>
-#include <geometry/visibility_control.hpp>
#include <geometry/common_2d.hpp>
+#include <geometry/visibility_control.hpp>
#include <algorithm>
#include <cmath>
@@ -30,11 +32,9 @@
#include <stdexcept>
#include <utility>
-#include "helper_functions/crtp.hpp"
-
-using autoware::common::types::float64_t;
-using autoware::common::types::float32_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float32_t;
+using autoware::common::types::float64_t;
namespace autoware
{
@@ -68,7 +68,7 @@ using BinRange = std::pair<Index3, Index3>;
/// \brief The base class for the configuration object for the SpatialHash class
/// \tparam Derived The type of the derived class to support static polymorphism/CRTP
-template<typename Derived>
+template <typename Derived>
class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<Derived>
{
public:
@@ -82,14 +82,8 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<D
/// \param[in] radius The look up radius
/// \param[in] capacity The maximum number of points the spatial hash can store
Config(
- const float32_t min_x,
- const float32_t max_x,
- const float32_t min_y,
- const float32_t max_y,
- const float32_t min_z,
- const float32_t max_z,
- const float32_t radius,
- const Index capacity)
+ const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y,
+ const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity)
: m_min_x{min_x},
m_min_y{min_y},
m_min_z{min_z},
@@ -117,7 +111,7 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<D
// small fudging to prevent weird boundary effects
// (e.g (x=xmax, y) rolls index over to (x=0, y+1)
constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
- //lint -e{1938} read only access is fine NOLINT
+ // lint -e{1938} read only access is fine NOLINT
m_max_x -= FEPS;
m_max_y -= FEPS;
m_max_z -= FEPS;
@@ -177,16 +171,10 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<D
}
/// \brief Get the maximum capacity of the spatial hash
/// \return The capacity
- Index get_capacity() const
- {
- return m_capacity;
- }
+ Index get_capacity() const { return m_capacity; }
/// \brief Getter for the side length, equivalently the lookup radius
- float32_t radius2() const
- {
- return m_side_length2;
- }
+ float32_t radius2() const { return m_side_length2; }
////////////////////////////////////////////////////////////////////////////////////////////
// "Polymorphic" API
@@ -207,9 +195,7 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<D
/// \return True if query and ref could possibly hold points within reference distance to one
/// another
bool is_candidate_bin(
- const details::Index3 & ref,
- const details::Index3 & query,
- const float ref_distance2) const
+ const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const
{
return this->impl().valid(ref, query, ref_distance2);
}
@@ -225,10 +211,7 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<D
/// \brief Compute the composed single index given a decomposed index
/// \param[in] idx A decomposed index triple for a bin
/// \return The composed bin index
- Index index(const details::Index3 & idx) const
- {
- return this->impl().index_(idx);
- }
+ Index index(const details::Index3 & idx) const { return this->impl().index_(idx); }
/// \brief Compute the squared distance between the two points
/// \tparam PointT A point type with float members x, y and z, or point adapters defined
/// \param[in] x The x component of the first point
@@ -236,12 +219,9 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<D
/// \param[in] z The z component of the first point
/// \param[in] pt The other point being compared
/// \return The squared distance between the points (2d or 3d)
- template<typename PointT>
+ template <typename PointT>
float32_t distance_squared(
- const float32_t x,
- const float32_t y,
- const float32_t z,
- const PointT & pt) const
+ const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const
{
return this->impl().distance_squared_(x, y, z, pt);
}
@@ -272,10 +252,7 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<D
std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv));
}
/// \brief Compose the provided index offsets
- Index bin_impl(const Index xdx, const Index ydx) const
- {
- return xdx + (ydx * m_y_stride);
- }
+ Index bin_impl(const Index xdx, const Index ydx) const { return xdx + (ydx * m_y_stride); }
/// \brief Compose the provided index offsets
Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const
{
@@ -310,10 +287,7 @@ class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<D
}
/// \brief Get side length squared
- float side_length2() const
- {
- return m_side_length2;
- }
+ float side_length2() const { return m_side_length2; }
private:
/// \brief Sanity check a range in a basis direction
@@ -363,12 +337,8 @@ class GEOMETRY_PUBLIC Config2d : public Config<Config2d>
/// \param[in] radius The lookup distance
/// \param[in] capacity The maximum number of points the spatial hash can store
Config2d(
- const float32_t min_x,
- const float32_t max_x,
- const float32_t min_y,
- const float32_t max_y,
- const float32_t radius,
- const Index capacity);
+ const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y,
+ const float32_t radius, const Index capacity);
/// \brief The index of a point given it's x, y and z values, 2d implementation
/// \param[in] x The x value of a point
/// \param[in] y the y value of a point
@@ -383,9 +353,7 @@ class GEOMETRY_PUBLIC Config2d : public Config<Config2d>
/// \return True if the reference bin and query bin could possibly hold a point within the
/// reference distance
bool valid(
- const details::Index3 & ref,
- const details::Index3 & query,
- const float ref_distance2) const;
+ const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const;
/// \brief Compute the decomposed index given a point, 2d implementation
/// \param[in] x The x component of the point
/// \param[in] y The y component of the point
@@ -403,12 +371,9 @@ class GEOMETRY_PUBLIC Config2d : public Config<Config2d>
/// \param[in] z The z component of the first point
/// \param[in] pt The other point being compared
/// \return The squared distance between the points (2d)
- template<typename PointT>
+ template <typename PointT>
float32_t distance_squared_(
- const float32_t x,
- const float32_t y,
- const float32_t z,
- const PointT & pt) const
+ const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const
{
(void)z;
const float32_t dx = x - point_adapter::x_(pt);
@@ -431,14 +396,8 @@ class GEOMETRY_PUBLIC Config3d : public Config<Config3d>
/// \param[in] radius The lookup distance
/// \param[in] capacity The maximum number of points the spatial hash can store
Config3d(
- const float32_t min_x,
- const float32_t max_x,
- const float32_t min_y,
- const float32_t max_y,
- const float32_t min_z,
- const float32_t max_z,
- const float32_t radius,
- const Index capacity);
+ const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y,
+ const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity);
/// \brief The index of a point given it's x, y and z values, 3d implementation
/// \param[in] x The x value of a point
/// \param[in] y the y value of a point
@@ -453,9 +412,7 @@ class GEOMETRY_PUBLIC Config3d : public Config<Config3d>
/// \return True if the reference bin and query bin could possibly hold a point within the
/// reference distance
bool valid(
- const details::Index3 & ref,
- const details::Index3 & query,
- const float ref_distance2) const;
+ const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const;
/// \brief Compute the decomposed index given a point, 3d implementation
/// \param[in] x The x component of the point
/// \param[in] y The y component of the point
@@ -473,12 +430,9 @@ class GEOMETRY_PUBLIC Config3d : public Config<Config3d>
/// \param[in] z The z component of the first point
/// \param[in] pt The other point being compared
/// \return The squared distance between the points (3d)
- template<typename PointT>
+ template <typename PointT>
float32_t distance_squared_(
- const float32_t x,
- const float32_t y,
- const float32_t z,
- const PointT & pt) const
+ const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const
{
const float32_t dx = x - point_adapter::x_(pt);
const float32_t dy = y - point_adapter::y_(pt);
diff --git a/common/autoware_auto_geometry/include/geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/geometry/visibility_control.hpp
index d7da65dd7bec8..571a7fb1588b7 100644
--- a/common/autoware_auto_geometry/include/geometry/visibility_control.hpp
+++ b/common/autoware_auto_geometry/include/geometry/visibility_control.hpp
@@ -17,26 +17,25 @@
#ifndef GEOMETRY__VISIBILITY_CONTROL_HPP_
#define GEOMETRY__VISIBILITY_CONTROL_HPP_
-
////////////////////////////////////////////////////////////////////////////////
#if defined(__WIN32)
- #if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS)
- #define GEOMETRY_PUBLIC __declspec(dllexport)
- #define GEOMETRY_LOCAL
- #else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS)
- #define GEOMETRY_PUBLIC __declspec(dllimport)
- #define GEOMETRY_LOCAL
- #endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS)
+#if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS)
+#define GEOMETRY_PUBLIC __declspec(dllexport)
+#define GEOMETRY_LOCAL
+#else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS)
+#define GEOMETRY_PUBLIC __declspec(dllimport)
+#define GEOMETRY_LOCAL
+#endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS)
#elif defined(__linux__)
- #define GEOMETRY_PUBLIC __attribute__((visibility("default")))
- #define GEOMETRY_LOCAL __attribute__((visibility("hidden")))
+#define GEOMETRY_PUBLIC __attribute__((visibility("default")))
+#define GEOMETRY_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
- #define GEOMETRY_PUBLIC __attribute__((visibility("default")))
- #define GEOMETRY_LOCAL __attribute__((visibility("hidden")))
+#define GEOMETRY_PUBLIC __attribute__((visibility("default")))
+#define GEOMETRY_LOCAL __attribute__((visibility("hidden")))
#elif defined(QNX)
- #define GEOMETRY_PUBLIC __attribute__((visibility("default")))
- #define GEOMETRY_LOCAL __attribute__((visibility("hidden")))
+#define GEOMETRY_PUBLIC __attribute__((visibility("default")))
+#define GEOMETRY_LOCAL __attribute__((visibility("hidden")))
#else // defined(__linux__)
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif // defined(__WIN32)
#endif // GEOMETRY__VISIBILITY_CONTROL_HPP_
diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml
index db117a380e325..777bf573a0456 100644
--- a/common/autoware_auto_geometry/package.xml
+++ b/common/autoware_auto_geometry/package.xml
@@ -1,26 +1,28 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
- <name>autoware_auto_geometry</name>
- <version>1.0.0</version>
- <description>Geometry related algorithms</description>
- <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
- <license>Apache 2</license>
+ <name>autoware_auto_geometry</name>
+ <version>1.0.0</version>
+ <description>Geometry related algorithms</description>
+ <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
+ <license>Apache 2</license>
- <buildtool_depend>ament_cmake_auto</buildtool_depend>
- <buildtool_depend>autoware_auto_cmake</buildtool_depend>
+ <buildtool_depend>ament_cmake_auto</buildtool_depend>
+ <buildtool_depend>autoware_auto_cmake</buildtool_depend>
- <depend>autoware_auto_common</depend>
- <depend>autoware_auto_geometry_msgs</depend>
- <depend>autoware_auto_planning_msgs</depend>
- <depend>autoware_auto_vehicle_msgs</depend>
- <depend>autoware_auto_tf2</depend>
- <depend>geometry_msgs</depend>
+ <depend>autoware_auto_common</depend>
+ <depend>autoware_auto_geometry_msgs</depend>
+ <depend>autoware_auto_planning_msgs</depend>
+ <depend>autoware_auto_tf2</depend>
+ <depend>autoware_auto_vehicle_msgs</depend>
+ <depend>geometry_msgs</depend>
- <test_depend>ament_cmake_gtest</test_depend>
- <!-- <test_depend>ament_lint_auto</test_depend> -->
- <!-- <test_depend>ament_lint_common</test_depend> -->
- <test_depend>osrf_testing_tools_cpp</test_depend>
+ <test_depend>ament_cmake_gtest</test_depend>
+ <test_depend>osrf_testing_tools_cpp</test_depend>
+ <!-- <test_depend>ament_lint_auto</test_depend> -->
+ <!-- <test_depend>ament_lint_common</test_depend> -->
- <export><build_type>ament_cmake</build_type></export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp
index 93cba784d0e0e..c6197902a7bfb 100644
--- a/common/autoware_auto_geometry/src/spatial_hash.cpp
+++ b/common/autoware_auto_geometry/src/spatial_hash.cpp
@@ -15,10 +15,11 @@
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
#include <geometry/spatial_hash.hpp>
+
#include <geometry_msgs/msg/point32.hpp>
-//lint -e537 NOLINT repeated include file due to cpplint rule
+// lint -e537 NOLINT repeated include file due to cpplint rule
#include <algorithm>
-//lint -e537 NOLINT repeated include file due to cpplint rule
+// lint -e537 NOLINT repeated include file due to cpplint rule
#include <limits>
namespace autoware
@@ -31,14 +32,9 @@ namespace spatial_hash
{
////////////////////////////////////////////////////////////////////////////////
Config2d::Config2d(
- const float32_t min_x,
- const float32_t max_x,
- const float32_t min_y,
- const float32_t max_y,
- const float32_t radius,
- const Index capacity)
-: Config(min_x, max_x, min_y, max_y, {}, std::numeric_limits<float32_t>::min(),
- radius, capacity)
+ const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y,
+ const float32_t radius, const Index capacity)
+: Config(min_x, max_x, min_y, max_y, {}, std::numeric_limits<float32_t>::min(), radius, capacity)
{
}
////////////////////////////////////////////////////////////////////////////////
@@ -49,9 +45,7 @@ Index Config2d::bin_(const float32_t x, const float32_t y, const float32_t z) co
}
////////////////////////////////////////////////////////////////////////////////
bool Config2d::valid(
- const details::Index3 & ref,
- const details::Index3 & query,
- const float ref_distance2) const
+ const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const
{
const float dx = idx_distance(ref.x, query.x);
const float dy = idx_distance(ref.y, query.y);
@@ -65,20 +59,11 @@ details::Index3 Config2d::index3_(const float32_t x, const float32_t y, const fl
return {x_index(x), y_index(y), Index{}}; // zero initialization
}
////////////////////////////////////////////////////////////////////////////////
-Index Config2d::index_(const details::Index3 & idx) const
-{
- return bin_impl(idx.x, idx.y);
-}
+Index Config2d::index_(const details::Index3 & idx) const { return bin_impl(idx.x, idx.y); }
////////////////////////////////////////////////////////////////////////////////
Config3d::Config3d(
- const float32_t min_x,
- const float32_t max_x,
- const float32_t min_y,
- const float32_t max_y,
- const float32_t min_z,
- const float32_t max_z,
- const float32_t radius,
- const Index capacity)
+ const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y,
+ const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity)
: Config(min_x, max_x, min_y, max_y, min_z, max_z, radius, capacity)
{
}
@@ -89,9 +74,7 @@ Index Config3d::bin_(const float32_t x, const float32_t y, const float32_t z) co
}
////////////////////////////////////////////////////////////////////////////////
bool Config3d::valid(
- const details::Index3 & ref,
- const details::Index3 & query,
- const float ref_distance2) const
+ const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const
{
const float dx = idx_distance(ref.x, query.x);
const float dy = idx_distance(ref.y, query.y);
@@ -105,10 +88,7 @@ details::Index3 Config3d::index3_(const float32_t x, const float32_t y, const fl
return {x_index(x), y_index(y), z_index(z)}; // zero initialization
}
////////////////////////////////////////////////////////////////////////////////
-Index Config3d::index_(const details::Index3 & idx) const
-{
- return bin_impl(idx.x, idx.y, idx.z);
-}
+Index Config3d::index_(const details::Index3 & idx) const { return bin_impl(idx.x, idx.y, idx.z); }
////////////////////////////////////////////////////////////////////////////////
template class SpatialHash<geometry_msgs::msg::Point32, Config2d>;
template class SpatialHash<geometry_msgs::msg::Point32, Config3d>;
diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp
index f112517fe0a8f..56d51c922b8d6 100644
--- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp
+++ b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp
@@ -17,22 +17,22 @@
#ifndef TEST_BOUNDING_BOX_HPP_
#define TEST_BOUNDING_BOX_HPP_
+#include "geometry/bounding_box/lfit.hpp"
+#include "geometry/bounding_box/rotating_calipers.hpp"
+
#include <geometry_msgs/msg/point32.hpp>
#include <limits>
#include <list>
#include <vector>
-#include "geometry/bounding_box/rotating_calipers.hpp"
-#include "geometry/bounding_box/lfit.hpp"
-
-using autoware_auto_perception_msgs::msg::BoundingBox;
using autoware::common::geometry::point_adapter::x_;
-using autoware::common::geometry::point_adapter::y_;
using autoware::common::geometry::point_adapter::xr_;
+using autoware::common::geometry::point_adapter::y_;
using autoware::common::geometry::point_adapter::yr_;
+using autoware_auto_perception_msgs::msg::BoundingBox;
-template<typename PointT>
+template <typename PointT>
class BoxTest : public ::testing::Test
{
protected:
@@ -52,14 +52,14 @@ class BoxTest : public ::testing::Test
box = autoware::common::geometry::bounding_box::minimum_perimeter_bounding_box(points);
// apex_test_tools::memory_test::stop();
}
- template<typename IT>
+ template <typename IT>
void eigenbox(const IT begin, const IT end)
{
// apex_test_tools::memory_test::start();
box = autoware::common::geometry::bounding_box::eigenbox_2d(begin, end);
// apex_test_tools::memory_test::stop();
}
- template<typename IT>
+ template <typename IT>
void lfit_bounding_box_2d(const IT begin, const IT end)
{
// apex_test_tools::memory_test::start();
@@ -86,9 +86,7 @@ class BoxTest : public ::testing::Test
ASSERT_LT(fabsf(box.centroid.y - cy), TOL);
}
- void test_corners(
- const std::vector<PointT> & expect,
- const float TOL = 1.0E-6F) const
+ void test_corners(const std::vector<PointT> & expect, const float TOL = 1.0E-6F) const
{
for (uint32_t idx = 0U; idx < 4U; ++idx) {
bool found = false;
@@ -109,14 +107,9 @@ class BoxTest : public ::testing::Test
}
/// \brief converts a radian value to a degree value
- float32_t rad2deg(const float rad_val) const
- {
- return rad_val * 57.2958F;
- }
+ float32_t rad2deg(const float rad_val) const { return rad_val * 57.2958F; }
- void test_orientation(
- const float ref_angle_deg,
- const float TOL = 1.0E-4F) const
+ void test_orientation(const float ref_angle_deg, const float TOL = 1.0E-4F) const
{
bool found = false;
const float angle_deg = rad2deg(2.0F * asinf(box.orientation.z));
@@ -193,7 +186,6 @@ TYPED_TEST(BoxTest, ClosestPointOnLine)
EXPECT_THROW(t = closest_line_point_2d(p, q, r), std::runtime_error);
}
-
TYPED_TEST(BoxTest, Basic)
{
const std::vector<TypeParam> data{
@@ -216,9 +208,7 @@ TYPED_TEST(BoxTest, Basic)
//
TYPED_TEST(BoxTest, OrientedTriangle)
{
- this->points.insert(
- this->points.begin(),
- {this->make(5, 5), this->make(7, 7), this->make(5, 7)});
+ this->points.insert(this->points.begin(), {this->make(5, 5), this->make(7, 7), this->make(5, 7)});
this->minimum_area_bounding_box();
@@ -258,10 +248,8 @@ TYPED_TEST(BoxTest, Hull)
ASSERT_LT(fabsf(this->box.centroid.y - dy), TOL_M);
this->test_corners(
- {this->make(dx + rx, dy + ry),
- this->make(dx - rx, dy + ry),
- this->make(dx - rx, dy - ry),
- this->make(dx + rx, dy - ry)},
+ {this->make(dx + rx, dy + ry), this->make(dx - rx, dy + ry), this->make(dx - rx, dy - ry),
+ this->make(dx + rx, dy - ry)},
TOL_M);
this->test_orientation(this->rad2deg(dth), 1.0F);
@@ -276,18 +264,9 @@ TYPED_TEST(BoxTest, Hull)
TYPED_TEST(BoxTest, Collinear)
{
this->points.insert(
- this->points.begin(),
- {
- this->make(-2, -2),
- this->make(-3, -2),
- this->make(-4, -2),
- this->make(-2, -4),
- this->make(-3, -4),
- this->make(-4, -4),
- this->make(-2, -3),
- this->make(-2, -3),
- this->make(-2, -4)
- });
+ this->points.begin(), {this->make(-2, -2), this->make(-3, -2), this->make(-4, -2),
+ this->make(-2, -4), this->make(-3, -4), this->make(-4, -4),
+ this->make(-2, -3), this->make(-2, -3), this->make(-2, -4)});
this->minimum_area_bounding_box();
@@ -297,57 +276,36 @@ TYPED_TEST(BoxTest, Collinear)
this->test_orientation(0.0F);
}
-
//
TYPED_TEST(BoxTest, Line1)
{
this->points.insert(
- this->points.begin(), {
- this->make(-4, 3),
- this->make(-8, 6),
- this->make(-12, 9),
- this->make(-16, 12),
- this->make(-20, 15),
- this->make(-24, 18),
- this->make(-28, 21),
- this->make(-32, 24),
- this->make(-36, 27)
- });
+ this->points.begin(), {this->make(-4, 3), this->make(-8, 6), this->make(-12, 9),
+ this->make(-16, 12), this->make(-20, 15), this->make(-24, 18),
+ this->make(-28, 21), this->make(-32, 24), this->make(-36, 27)});
this->minimum_area_bounding_box();
this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F);
this->test_orientation(this->rad2deg(atan2f(3, -4)));
this->test_corners(
- {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(
- -36,
- 27)});
+ {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)});
this->minimum_perimeter_bounding_box();
this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 40.00001F);
this->test_orientation(this->rad2deg(atan2f(3, -4)));
this->test_corners(
- {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(
- -36,
- 27)});
+ {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)});
}
//
TYPED_TEST(BoxTest, Line2)
{
this->points.insert(
- this->points.begin(), {
- this->make(4, 0),
- this->make(8, 0),
- this->make(12, 0),
- this->make(16, 0),
- this->make(20, 0),
- this->make(24, 0),
- this->make(28, 0),
- this->make(32, 0),
- this->make(36, 0)
- });
+ this->points.begin(),
+ {this->make(4, 0), this->make(8, 0), this->make(12, 0), this->make(16, 0), this->make(20, 0),
+ this->make(24, 0), this->make(28, 0), this->make(32, 0), this->make(36, 0)});
this->minimum_area_bounding_box();
@@ -360,17 +318,9 @@ TYPED_TEST(BoxTest, Line2)
TYPED_TEST(BoxTest, Line3)
{
this->points.insert(
- this->points.begin(), {
- this->make(4, 3),
- this->make(8, 6),
- this->make(12, 9),
- this->make(16, 12),
- this->make(20, 15),
- this->make(24, 18),
- this->make(28, 21),
- this->make(32, 24),
- this->make(36, 27)
- });
+ this->points.begin(),
+ {this->make(4, 3), this->make(8, 6), this->make(12, 9), this->make(16, 12), this->make(20, 15),
+ this->make(24, 18), this->make(28, 21), this->make(32, 24), this->make(36, 27)});
this->minimum_area_bounding_box();
@@ -386,12 +336,8 @@ TYPED_TEST(BoxTest, Line3)
TYPED_TEST(BoxTest, SuboptInit)
{
this->points.insert(
- this->points.begin(), {
- this->make(8, 15),
- this->make(17, 0),
- this->make(0, 0),
- this->make(25, 15)
- });
+ this->points.begin(),
+ {this->make(8, 15), this->make(17, 0), this->make(0, 0), this->make(25, 15)});
this->minimum_area_bounding_box();
@@ -399,8 +345,8 @@ TYPED_TEST(BoxTest, SuboptInit)
this->test_orientation(this->rad2deg(atan2f(15, 8)));
// these are approximate.
this->test_corners(
- {this->make(0, 0), this->make(25, 15),
- this->make(11.7647F, 22.0588F), this->make(13.2353F, -7.05882F)},
+ {this->make(0, 0), this->make(25, 15), this->make(11.7647F, 22.0588F),
+ this->make(13.2353F, -7.05882F)},
0.1F);
}
@@ -408,12 +354,8 @@ TYPED_TEST(BoxTest, SuboptInit)
TYPED_TEST(BoxTest, Centered)
{
this->points.insert(
- this->points.begin(), {
- this->make(-1, 0),
- this->make(1, 0),
- this->make(0, -1),
- this->make(0, 1)
- });
+ this->points.begin(),
+ {this->make(-1, 0), this->make(1, 0), this->make(0, -1), this->make(0, 1)});
this->minimum_area_bounding_box();
@@ -426,16 +368,8 @@ TYPED_TEST(BoxTest, Centered)
TYPED_TEST(BoxTest, OverlappingPoints)
{
this->points.insert(
- this->points.begin(), {
- this->make(0, 0),
- this->make(1, 0),
- this->make(1, 1),
- this->make(0, 1),
- this->make(0, 0),
- this->make(1, 0),
- this->make(1, 1),
- this->make(0, 1)
- });
+ this->points.begin(), {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1),
+ this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)});
this->minimum_area_bounding_box();
@@ -448,24 +382,16 @@ TYPED_TEST(BoxTest, OverlappingPoints)
TYPED_TEST(BoxTest, Perimeter)
{
this->points.insert(
- this->points.begin(), {
- this->make(0, 0),
- this->make(0, 1),
- this->make(0, 2),
- this->make(0, 3),
- this->make(0, 4),
- this->make(1, -0.1), // small fudge to force diagonal
- this->make(2, 0),
- this->make(3, 0)
- });
+ this->points.begin(), {this->make(0, 0), this->make(0, 1), this->make(0, 2), this->make(0, 3),
+ this->make(0, 4), this->make(1, -0.1), // small fudge to force diagonal
+ this->make(2, 0), this->make(3, 0)});
this->minimum_area_bounding_box();
this->check(0.54F, 1.28F, 5.0F, 12.0F / 5.0F, 12.0F);
this->test_orientation(-53.13F, 0.001F);
this->test_corners(
- {this->make(3, 0), this->make(0, 4), this->make(-1.92, 2.56),
- this->make(1.08, -1.44)});
+ {this->make(3, 0), this->make(0, 4), this->make(-1.92, 2.56), this->make(1.08, -1.44)});
// eigenbox should produce AABB TODO(c.ho)
// compute minimum perimeter box
@@ -474,24 +400,17 @@ TYPED_TEST(BoxTest, Perimeter)
// perimeter for first box would be 14.8
this->test_orientation(0.0F, 0.001F);
this->test_corners(
- {this->make(3, -0.1), this->make(0, 4), this->make(3, 4),
- this->make(0, -0.1)});
+ {this->make(3, -0.1), this->make(0, 4), this->make(3, 4), this->make(0, -0.1)});
}
// bounding box is diagonal on an L
TYPED_TEST(BoxTest, Eigenbox1)
{
- std::vector<TypeParam> v{
- this->make(0, 0),
- this->make(0, 1),
- this->make(-1, 2), // small fudge to force diagonal
- this->make(0, 3),
- this->make(0, 4),
- this->make(1, 0),
- this->make(2, -1), // small fudge to force diagonal
- this->make(3, 0),
- this->make(4, 0)
- };
+ std::vector<TypeParam> v{this->make(0, 0), this->make(0, 1),
+ this->make(-1, 2), // small fudge to force diagonal
+ this->make(0, 3), this->make(0, 4),
+ this->make(1, 0), this->make(2, -1), // small fudge to force diagonal
+ this->make(3, 0), this->make(4, 0)};
this->points.insert(this->points.begin(), v.begin(), v.end());
// rotating calipers should produce a diagonal box
@@ -499,16 +418,15 @@ TYPED_TEST(BoxTest, Eigenbox1)
const float r = 4.0F;
this->check(1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * r);
- const std::vector<TypeParam>
- diag_corners{this->make(4, 0), this->make(0, 4), this->make(-2, 2), this->make(2, -2)};
+ const std::vector<TypeParam> diag_corners{
+ this->make(4, 0), this->make(0, 4), this->make(-2, 2), this->make(2, -2)};
this->test_corners(diag_corners);
this->test_orientation(45.0F, 0.001F);
//// perimeter should also produce diagonal ////
this->minimum_perimeter_bounding_box();
this->check(
- 1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r,
- r * (sqrtf(2.0F) + (1.0F / sqrtf(2.0F))));
+ 1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * (sqrtf(2.0F) + (1.0F / sqrtf(2.0F))));
this->test_corners(diag_corners);
this->test_orientation(45.0F, 0.001F);
//// eigenbox should also produce aabb ////
@@ -528,21 +446,15 @@ TYPED_TEST(BoxTest, Eigenbox1)
// same as above test, just rotated
TYPED_TEST(BoxTest, Eigenbox2)
{
- std::vector<TypeParam> v{
- this->make(0, 0),
- this->make(1, 1),
- this->make(1, 2), // small fudge to force diagonal
- this->make(3, 3),
- this->make(4, 4),
- this->make(1, -1),
- this->make(1, -2), // small fudge to force diagonal
- this->make(3, -3),
- this->make(4, -4)
- };
+ std::vector<TypeParam> v{this->make(0, 0), this->make(1, 1),
+ this->make(1, 2), // small fudge to force diagonal
+ this->make(3, 3), this->make(4, 4),
+ this->make(1, -1), this->make(1, -2), // small fudge to force diagonal
+ this->make(3, -3), this->make(4, -4)};
this->points.insert(this->points.begin(), v.begin(), v.end());
- const std::vector<TypeParam>
- diag_corners{this->make(4, 4), this->make(0, 4), this->make(0, -4), this->make(4, -4)};
+ const std::vector<TypeParam> diag_corners{
+ this->make(4, 4), this->make(0, 4), this->make(0, -4), this->make(4, -4)};
// rotating calipers should produce a aabb
this->minimum_area_bounding_box();
this->check(2.0F, 0.0F, 8, 4, 32);
@@ -568,17 +480,9 @@ TYPED_TEST(BoxTest, Eigenbox2)
TYPED_TEST(BoxTest, Eigenbox3)
{
// horizontal line with some noise
- std::vector<TypeParam> v{
- this->make(0, 0.00),
- this->make(1, -0.01),
- this->make(3, 0.02),
- this->make(3, 0.03),
- this->make(4, -0.04),
- this->make(-1, 0.01),
- this->make(-2, -0.02),
- this->make(-3, -0.03),
- this->make(-4, 0.04)
- };
+ std::vector<TypeParam> v{this->make(0, 0.00), this->make(1, -0.01), this->make(3, 0.02),
+ this->make(3, 0.03), this->make(4, -0.04), this->make(-1, 0.01),
+ this->make(-2, -0.02), this->make(-3, -0.03), this->make(-4, 0.04)};
this->lfit_bounding_box_2d(v.begin(), v.end());
this->test_corners(
{this->make(-4, -0.04), this->make(-4, 0.04), this->make(4, -0.04), this->make(4, 0.04)}, 0.2F);
@@ -590,44 +494,19 @@ TYPED_TEST(BoxTest, Eigenbox3)
TYPED_TEST(BoxTest, IntersectFail)
{
std::vector<TypeParam> vals{
- this->make(-13.9, 0.100006),
- this->make(-14.1, 0.100006),
- this->make(-13.9, 0.300003),
- this->make(-14.1, 0.300003),
- this->make(-13.9, 0.5),
- this->make(-14.1, 0.5),
- this->make(-13.9, 0.699997),
- this->make(-14.1, 0.699997),
- this->make(-14.3, 0.699997)
- };
+ this->make(-13.9, 0.100006), this->make(-14.1, 0.100006), this->make(-13.9, 0.300003),
+ this->make(-14.1, 0.300003), this->make(-13.9, 0.5), this->make(-14.1, 0.5),
+ this->make(-13.9, 0.699997), this->make(-14.1, 0.699997), this->make(-14.3, 0.699997)};
EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end()));
- vals = {
- this->make(-2.1, 10.1),
- this->make(-1.89999, 10.1),
- this->make(-1.89999, 10.3),
- this->make(-1.7, 10.3),
- this->make(-1.5, 10.3),
- this->make(-1.3, 10.3),
- this->make(-0.5, 10.3),
- this->make(-0.300003, 10.3),
- this->make(-0.0999908, 10.3),
- this->make(0.699997, 10.3),
- this->make(0.900009, 10.3),
- this->make(1.3, 10.3),
- this->make(1.7, 10.3)
- };
+ vals = {this->make(-2.1, 10.1), this->make(-1.89999, 10.1), this->make(-1.89999, 10.3),
+ this->make(-1.7, 10.3), this->make(-1.5, 10.3), this->make(-1.3, 10.3),
+ this->make(-0.5, 10.3), this->make(-0.300003, 10.3), this->make(-0.0999908, 10.3),
+ this->make(0.699997, 10.3), this->make(0.900009, 10.3), this->make(1.3, 10.3),
+ this->make(1.7, 10.3)};
EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end()));
- vals = {
- this->make(2.7, -5.5),
- this->make(2.7, -5.3),
- this->make(2.7, -5.1),
- this->make(2.7, -4.9),
- this->make(2.7, -4.7),
- this->make(2.7, -4.5),
- this->make(2.7, -4.3),
- this->make(2.7, -4.1),
- this->make(2.7, -3.9)
- };
+ vals = {this->make(2.7, -5.5), this->make(2.7, -5.3), this->make(2.7, -5.1),
+ this->make(2.7, -4.9), this->make(2.7, -4.7), this->make(2.7, -4.5),
+ this->make(2.7, -4.3), this->make(2.7, -4.1), this->make(2.7, -3.9)};
EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end()));
}
/// Handle slight floating point underflow case
@@ -663,7 +542,6 @@ TYPED_TEST(BoxTest, EigUnderflow)
EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c6, u, v));
}
-
////////////////////////////////////////////////
#endif // TEST_BOUNDING_BOX_HPP_
diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp
index 9b0563ff010f7..7d1d6cf388ab9 100644
--- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp
+++ b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp
@@ -17,27 +17,27 @@
#ifndef TEST_SPATIAL_HASH_HPP_
#define TEST_SPATIAL_HASH_HPP_
+#include "geometry/spatial_hash.hpp"
+
#include <geometry_msgs/msg/point32.hpp>
-#include <vector>
+
#include <limits>
-#include "geometry/spatial_hash.hpp"
+#include <vector>
-using autoware::common::types::float32_t;
-using autoware::common::types::float64_t;
-using autoware::common::types::bool8_t;
using autoware::common::geometry::spatial_hash::Config2d;
using autoware::common::geometry::spatial_hash::Config3d;
using autoware::common::geometry::spatial_hash::SpatialHash;
using autoware::common::geometry::spatial_hash::SpatialHash2d;
using autoware::common::geometry::spatial_hash::SpatialHash3d;
+using autoware::common::types::bool8_t;
+using autoware::common::types::float32_t;
+using autoware::common::types::float64_t;
-template<typename PointT>
+template <typename PointT>
class TypedSpatialHashTest : public ::testing::Test
{
public:
- TypedSpatialHashTest()
- : ref(),
- EPS(0.001F)
+ TypedSpatialHashTest() : ref(), EPS(0.001F)
{
ref.x = 0.0F;
ref.y = 0.0F;
@@ -45,14 +45,10 @@ class TypedSpatialHashTest : public ::testing::Test
}
protected:
- template<typename Cfg>
+ template <typename Cfg>
void add_points(
- SpatialHash<PointT, Cfg> & hash,
- const uint32_t points_per_ring,
- const uint32_t num_rings,
- const float32_t dr,
- const float32_t dx = 0.0F,
- const float32_t dy = 0.0F)
+ SpatialHash<PointT, Cfg> & hash, const uint32_t points_per_ring, const uint32_t num_rings,
+ const float32_t dr, const float32_t dx = 0.0F, const float32_t dy = 0.0F)
{
const float32_t dth = 2.0F * 3.14159F / points_per_ring;
@@ -82,7 +78,6 @@ using PointTypesSpatialHash = ::testing::Types<geometry_msgs::msg::Point32>;
TYPED_TEST_SUITE(TypedSpatialHashTest, PointTypesSpatialHash, );
/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE
-
///////////////////////////////////////////////////////////////
// TODO(christopher.ho) helper functions to simplify this stuff
/// all points in one bin
@@ -130,7 +125,7 @@ TYPED_TEST(TypedSpatialHashTest, OneBin)
count = 0U;
for (auto it : hash) {
// TODO(c.ho) check uniqueness of stuff
- (void) it;
+ (void)it;
++count;
}
EXPECT_EQ(count, 0U);
@@ -224,7 +219,7 @@ TYPED_TEST(TypedSpatialHashTest, 3d)
count = 0U;
for (auto it : hash) {
// TODO(c.ho) check uniqueness of stuff
- (void) it;
+ (void)it;
++count;
}
EXPECT_EQ(count, 0U);
@@ -241,8 +236,7 @@ TEST(SpatialHashConfig, BadCases)
EXPECT_THROW(Config2d({-30.0F, 30.0F, 30.1F, 30.0F, 1.0F, 1024U}), std::domain_error);
// min_z >= max_z
EXPECT_THROW(
- Config3d({-30.0F, 30.0F, -30.0F, 30.0F, 31.0F, 30.0F, 1.0F, 1024U}),
- std::domain_error);
+ Config3d({-30.0F, 30.0F, -30.0F, 30.0F, 31.0F, 30.0F, 1.0F, 1024U}), std::domain_error);
// floating point limit
constexpr float32_t max_float = std::numeric_limits<float32_t>::max();
EXPECT_THROW(Config2d({-max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error);
diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp
index f825d9055f9b2..f1f775bfde1bf 100644
--- a/common/autoware_auto_geometry/test/src/lookup_table.cpp
+++ b/common/autoware_auto_geometry/test/src/lookup_table.cpp
@@ -14,21 +14,21 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <gtest/gtest.h>
-
-#include <geometry/lookup_table.hpp>
#include <common/types.hpp>
+#include <geometry/lookup_table.hpp>
+
+#include <gtest/gtest.h>
#include <memory>
#include <vector>
-using autoware::common::helper_functions::lookup_1d;
using autoware::common::helper_functions::interpolate;
+using autoware::common::helper_functions::lookup_1d;
using autoware::common::helper_functions::LookupTable1D;
using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
-template<typename T>
+template <typename T>
class BadCase : public ::testing::Test
{
};
@@ -60,7 +60,7 @@ TYPED_TEST(BadCase, DomainNotSorted)
////////////////////////////////////////////////////////////////////////////////
-template<typename Type>
+template <typename Type>
class SanityCheck : public ::testing::Test
{
public:
@@ -129,7 +129,8 @@ TYPED_TEST(SanityCheck, BelowRange)
this->check(result, this->range_.front());
}
-TEST(LookupTableHelpers, Interpolate) {
+TEST(LookupTableHelpers, Interpolate)
+{
{
const auto scaling = 0.0f;
EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f);
diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp
index c7f141a77d16f..914f711ef8a7b 100644
--- a/common/autoware_auto_geometry/test/src/test_area.cpp
+++ b/common/autoware_auto_geometry/test/src/test_area.cpp
@@ -14,16 +14,17 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <gtest/gtest.h>
+#include <geometry/common_2d.hpp>
#include <geometry_msgs/msg/point32.hpp>
-#include <geometry/common_2d.hpp>
+
+#include <gtest/gtest.h>
#include <list>
#include <utility>
#include <vector>
-template<typename DataStructure>
+template <typename DataStructure>
class AreaTest : public ::testing::Test
{
protected:
@@ -31,10 +32,7 @@ class AreaTest : public ::testing::Test
using Point = typename DataStructure::value_type;
using Real = decltype(autoware::common::geometry::point_adapter::x_(std::declval<Point>()));
- auto area()
- {
- return autoware::common::geometry::area_checked_2d(data_.begin(), data_.end());
- }
+ auto area() { return autoware::common::geometry::area_checked_2d(data_.begin(), data_.end()); }
void add_point(Real x, Real y)
{
@@ -47,21 +45,15 @@ class AreaTest : public ::testing::Test
};
// Data structures to test...
-template<typename ... Points>
-using TestTypes_ = ::testing::Types<
- std::vector<Points>...,
- std::list<Points>...
->;
+template <typename... Points>
+using TestTypes_ = ::testing::Types<std::vector<Points>..., std::list<Points>...>;
// ... and point types to test
using TestTypes = TestTypes_<geometry_msgs::msg::Point32>;
// cppcheck-suppress syntaxError
TYPED_TEST_SUITE(AreaTest, TestTypes, );
// The empty set has zero area
-TYPED_TEST(AreaTest, DegenerateZero)
-{
- EXPECT_FLOAT_EQ(0.0, this->area());
-}
+TYPED_TEST(AreaTest, DegenerateZero) { EXPECT_FLOAT_EQ(0.0, this->area()); }
// An individual point has zero area
TYPED_TEST(AreaTest, DegenerateOne)
@@ -82,8 +74,8 @@ TYPED_TEST(AreaTest, DegenerateTwo)
TYPED_TEST(AreaTest, Triangle)
{
this->add_point(1.0, 0.0);
- this->add_point(3.0, 0.0); // 2.0 wide
- this->add_point(2.0, 2.0); // 2.0 tall
+ this->add_point(3.0, 0.0); // 2.0 wide
+ this->add_point(2.0, 2.0); // 2.0 tall
EXPECT_FLOAT_EQ(2.0, this->area()); // A = (1/2) * b * h
}
diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp
index b1abf7182f15d..d0e4d69f30dd8 100644
--- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp
+++ b/common/autoware_auto_geometry/test/src/test_common_2d.cpp
@@ -14,20 +14,21 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <gtest/gtest.h>
-#include <autoware_auto_perception_msgs/msg/point_clusters.hpp>
#include <geometry/common_2d.hpp>
+
+#include <autoware_auto_perception_msgs/msg/point_clusters.hpp>
#include <geometry_msgs/msg/point32.hpp>
+#include <gtest/gtest.h>
+
#include <utility>
#include <vector>
using autoware::common::geometry::point_adapter::xr_;
using autoware::common::geometry::point_adapter::yr_;
-
// Helper function for adding new points
-template<typename T>
+template <typename T>
T make_points(const float x, const float y)
{
T ret;
@@ -37,12 +38,12 @@ T make_points(const float x, const float y)
}
// PointTypes to be tested
-using PointTypes = ::testing::Types<geometry_msgs::msg::Point32,
- autoware::common::types::PointXYZIF>;
+using PointTypes =
+ ::testing::Types<geometry_msgs::msg::Point32, autoware::common::types::PointXYZIF>;
// Wrapper function for stubbing output of
// autoware::common::geometry::check_point_position_to_line_2d
-template<typename T>
+template <typename T>
int point_position_checker(const T & p1, const T & p2, const T & q)
{
auto result = autoware::common::geometry::check_point_position_to_line_2d(p1, p2, q);
@@ -57,7 +58,7 @@ int point_position_checker(const T & p1, const T & p2, const T & q)
// Templated struct defining the function parameters for
// autoware::common::geometry::check_point_position_to_line_2d and input_output vector for
// assertion of return values from the function
-template<typename T>
+template <typename T>
struct PointPositionToLine : public ::testing::Test
{
struct Parameters
@@ -71,25 +72,23 @@ struct PointPositionToLine : public ::testing::Test
TYPED_TEST_SUITE_P(PointPositionToLine);
-
-template<typename T>
+template <typename T>
std::vector<std::pair<typename PointPositionToLine<T>::Parameters, int>>
-PointPositionToLine<T>::input_output{
- {{make_points<T>(0.0F, 0.0F), make_points<T>(-1.0F, 1.0F), make_points<T>(1.0F, 5.0F)},
- -1},
- {{make_points<T>(0.0F, 0.0F), make_points<T>(-1.0F, 1.0F), make_points<T>(-1.0F, 0.5F)},
- 1},
- // Check point on the line
- {{make_points<T>(0.0F, 0.0F), make_points<T>(-1.0F, 1.0F), make_points<T>(-2.0F, 2.0F)},
- 0},
-};
+ PointPositionToLine<T>::input_output{
+ {{make_points<T>(0.0F, 0.0F), make_points<T>(-1.0F, 1.0F), make_points<T>(1.0F, 5.0F)}, -1},
+ {{make_points<T>(0.0F, 0.0F), make_points<T>(-1.0F, 1.0F), make_points<T>(-1.0F, 0.5F)}, 1},
+ // Check point on the line
+ {{make_points<T>(0.0F, 0.0F), make_points<T>(-1.0F, 1.0F), make_points<T>(-2.0F, 2.0F)}, 0},
+ };
-TYPED_TEST_P(PointPositionToLine, PointPositionToLineTest) {
+TYPED_TEST_P(PointPositionToLine, PointPositionToLineTest)
+{
for (size_t i = 0; i < PointPositionToLine<TypeParam>::input_output.size(); ++i) {
const auto & input = PointPositionToLine<TypeParam>::input_output[i].first;
EXPECT_EQ(
point_position_checker(input.p1, input.p2, input.q),
- PointPositionToLine<TypeParam>::input_output[i].second) << "Index " << i;
+ PointPositionToLine<TypeParam>::input_output[i].second)
+ << "Index " << i;
}
}
@@ -102,7 +101,7 @@ INSTANTIATE_TYPED_TEST_SUITE_P(Test, PointPositionToLine, PointTypes, );
// Templated struct defining the function parameters for
// autoware::common::geometry::is_point_inside_polygon_2d and input_output vector for
// assertion of return values from the function
-template<typename T>
+template <typename T>
struct InsidePolygon : public ::testing::Test
{
struct Parameters
@@ -115,35 +114,39 @@ struct InsidePolygon : public ::testing::Test
TYPED_TEST_SUITE_P(InsidePolygon);
-template<typename T>
-std::vector<std::pair<typename InsidePolygon<T>::Parameters, bool>>
-InsidePolygon<T>::input_output{
+template <typename T>
+std::vector<std::pair<typename InsidePolygon<T>::Parameters, bool>> InsidePolygon<T>::input_output{
// point inside the rectangle
{{{make_points<T>(0.0F, 0.0F), make_points<T>(1.0F, 1.0F), make_points<T>(0.5F, 1.5F),
- make_points<T>(-0.5F, 0.5F)}, make_points<T>(0.F, 0.5F)},
- true},
+ make_points<T>(-0.5F, 0.5F)},
+ make_points<T>(0.F, 0.5F)},
+ true},
// point below the rectangle
{{{make_points<T>(0.0F, 0.0F), make_points<T>(1.0F, 1.0F), make_points<T>(0.5F, 1.5F),
- make_points<T>(-0.5F, 0.5F)}, make_points<T>(0.5F, 0.F)},
- false},
+ make_points<T>(-0.5F, 0.5F)},
+ make_points<T>(0.5F, 0.F)},
+ false},
// point above the rectangle
{{{make_points<T>(0.0F, 0.0F), make_points<T>(1.0F, 1.0F), make_points<T>(0.5F, 1.5F),
- make_points<T>(-0.5F, 0.5F)}, make_points<T>(0.5F, 1.75F)},
- false},
+ make_points<T>(-0.5F, 0.5F)},
+ make_points<T>(0.5F, 1.75F)},
+ false},
// point on the rectangle
{{{make_points<T>(0.0F, 0.0F), make_points<T>(1.0F, 1.0F), make_points<T>(0.5F, 1.5F),
- make_points<T>(-0.5F, 0.5F)}, make_points<T>(0.5F, 0.5F)},
- true},
+ make_points<T>(-0.5F, 0.5F)},
+ make_points<T>(0.5F, 0.5F)},
+ true},
};
-TYPED_TEST_P(InsidePolygon, InsidePolygonTest) {
+TYPED_TEST_P(InsidePolygon, InsidePolygonTest)
+{
for (size_t i = 0; i < InsidePolygon<TypeParam>::input_output.size(); ++i) {
const auto & input = InsidePolygon<TypeParam>::input_output[i].first;
EXPECT_EQ(
autoware::common::geometry::is_point_inside_polygon_2d(
- input.polygon.begin(),
- input.polygon.end(), input.q), InsidePolygon<TypeParam>::input_output[i].second) <<
- "Index " << i;
+ input.polygon.begin(), input.polygon.end(), input.q),
+ InsidePolygon<TypeParam>::input_output[i].second)
+ << "Index " << i;
}
}
@@ -151,14 +154,14 @@ REGISTER_TYPED_TEST_SUITE_P(InsidePolygon, InsidePolygonTest);
// cppcheck-suppress syntaxError
INSTANTIATE_TYPED_TEST_SUITE_P(Test, InsidePolygon, PointTypes, );
-TEST(ordered_check, basic) {
+TEST(ordered_check, basic)
+{
// CW
std::vector<autoware_auto_perception_msgs::msg::PointXYZIF> points_list = {
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(8.0, 4.0),
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(9.0, 1.0),
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(3.0, 2.0),
- make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(2.0, 5.0)
- };
+ make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(2.0, 5.0)};
EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end()));
// CCW
@@ -166,8 +169,7 @@ TEST(ordered_check, basic) {
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(2.0, 5.0),
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(3.0, 2.0),
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(9.0, 1.0),
- make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(8.0, 4.0)
- };
+ make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(8.0, 4.0)};
EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end()));
// Unordered
@@ -175,8 +177,7 @@ TEST(ordered_check, basic) {
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(2.0, 5.0),
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(3.0, 2.0),
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(8.0, 4.0),
- make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(9.0, 1.0)
- };
+ make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(9.0, 1.0)};
EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end()));
// Unordered
@@ -184,15 +185,13 @@ TEST(ordered_check, basic) {
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(0.0, 0.0),
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(1.0, 1.0),
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(1.0, 0.0),
- make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(2.0, 1.0)
- };
+ make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(2.0, 1.0)};
EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end()));
// Colinearity
points_list = {
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(2.0, 2.0),
make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(4.0, 4.0),
- make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(6.0, 6.0)
- };
+ make_points<autoware_auto_perception_msgs::msg::PointXYZIF>(6.0, 6.0)};
EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end()));
}
diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp
index b22c72c377ab6..072fe766bebbc 100644
--- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp
+++ b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp
@@ -14,17 +14,20 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <gtest/gtest.h>
+#include "geometry/convex_hull.hpp"
+
#include <geometry_msgs/msg/point32.hpp>
+
+#include <gtest/gtest.h>
+
#include <list>
#include <vector>
-#include "geometry/convex_hull.hpp"
+using autoware::common::types::bool8_t;
using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
-using autoware::common::types::bool8_t;
-template<typename PointT>
+template <typename PointT>
class TypedConvexHullTest : public ::testing::Test
{
protected:
@@ -37,8 +40,7 @@ class TypedConvexHullTest : public ::testing::Test
}
void check_hull(
- const typename std::list<PointT>::const_iterator last,
- const std::vector<PointT> & expect,
+ const typename std::list<PointT>::const_iterator last, const std::vector<PointT> & expect,
const bool8_t strict = true)
{
uint32_t items = 0U;
@@ -47,8 +49,8 @@ class TypedConvexHullTest : public ::testing::Test
auto it = list.begin();
while (it != last) {
constexpr float32_t TOL = 1.0E-6F;
- if (fabsf(pt.x - it->x) <= TOL &&
- fabsf(pt.y - it->y) <= TOL &&
+ if (
+ fabsf(pt.x - it->x) <= TOL && fabsf(pt.y - it->y) <= TOL &&
(fabsf(pt.z - it->z) <= TOL || !strict)) // TODO(@estive): z if only strict
{
found = true;
@@ -98,9 +100,12 @@ TYPED_TEST(TypedConvexHullTest, Triangle)
ASSERT_EQ(this->list.size(), 3U);
// check order
auto it = this->list.begin();
- ASSERT_FLOAT_EQ(it->x, 1); ++it; // node 1
- ASSERT_FLOAT_EQ(it->x, 3); ++it; // node 2
- ASSERT_FLOAT_EQ(it->x, 2); ++it; // node 3
+ ASSERT_FLOAT_EQ(it->x, 1);
+ ++it; // node 1
+ ASSERT_FLOAT_EQ(it->x, 3);
+ ++it; // node 2
+ ASSERT_FLOAT_EQ(it->x, 2);
+ ++it; // node 3
ASSERT_EQ(it, last);
}
/*
@@ -112,11 +117,8 @@ TYPED_TEST(TypedConvexHullTest, Triangle)
// test that things get reordered to ccw
TYPED_TEST(TypedConvexHullTest, Quadrilateral)
{
- std::vector<TypeParam> expect({
- this->make(-1, -1, 1),
- this->make(-5, -1, 2),
- this->make(-2, -6, 3),
- this->make(-6, -5, 4)});
+ std::vector<TypeParam> expect(
+ {this->make(-1, -1, 1), this->make(-5, -1, 2), this->make(-2, -6, 3), this->make(-6, -5, 4)});
this->list.insert(this->list.begin(), expect.begin(), expect.end());
const auto last = this->convex_hull();
@@ -125,22 +127,23 @@ TYPED_TEST(TypedConvexHullTest, Quadrilateral)
// check for order
auto it = this->list.begin();
- ASSERT_FLOAT_EQ(it->x, -6); ++it; // node 4
- ASSERT_FLOAT_EQ(it->x, -2); ++it; // node 3
- ASSERT_FLOAT_EQ(it->x, -1); ++it; // node 1
- ASSERT_FLOAT_EQ(it->x, -5); ++it; // node 2
+ ASSERT_FLOAT_EQ(it->x, -6);
+ ++it; // node 4
+ ASSERT_FLOAT_EQ(it->x, -2);
+ ++it; // node 3
+ ASSERT_FLOAT_EQ(it->x, -1);
+ ++it; // node 1
+ ASSERT_FLOAT_EQ(it->x, -5);
+ ++it; // node 2
ASSERT_EQ(it, last);
}
// test that things get reordered to ccw
TYPED_TEST(TypedConvexHullTest, Quadhull)
{
- std::vector<TypeParam> data({
- this->make(1, 1, 1),
- this->make(5, 1, 2),
- this->make(2, 6, 3),
- this->make(3, 3, 4),
- this->make(6, 5, 5)});
+ std::vector<TypeParam> data(
+ {this->make(1, 1, 1), this->make(5, 1, 2), this->make(2, 6, 3), this->make(3, 3, 4),
+ this->make(6, 5, 5)});
std::vector<TypeParam> expect{{data[0], data[1], data[2], data[4]}};
this->list.insert(this->list.begin(), data.begin(), data.end());
@@ -151,14 +154,17 @@ TYPED_TEST(TypedConvexHullTest, Quadhull)
// check for order
auto it = this->list.begin();
- ASSERT_FLOAT_EQ(it->x, 1); ++it; // node 1
- ASSERT_FLOAT_EQ(it->x, 5); ++it; // node 2
- ASSERT_FLOAT_EQ(it->x, 6); ++it; // node 4
- ASSERT_FLOAT_EQ(it->x, 2); ++it; // node 3
+ ASSERT_FLOAT_EQ(it->x, 1);
+ ++it; // node 1
+ ASSERT_FLOAT_EQ(it->x, 5);
+ ++it; // node 2
+ ASSERT_FLOAT_EQ(it->x, 6);
+ ++it; // node 4
+ ASSERT_FLOAT_EQ(it->x, 2);
+ ++it; // node 3
ASSERT_EQ(it, last);
}
-
// a ring plus a bunch of random stuff in the middle
TYPED_TEST(TypedConvexHullTest, Hull)
{
@@ -223,17 +229,10 @@ TYPED_TEST(TypedConvexHullTest, Hull)
TYPED_TEST(TypedConvexHullTest, Collinear)
{
- std::vector<TypeParam> data({
- this->make(0, 0, 1),
- this->make(1, 0, 2),
- this->make(2, 0, 3),
- this->make(0, 2, 4),
- this->make(1, 2, 8),
- this->make(2, 2, 7),
- this->make(1, 0, 6),
- this->make(1, 2, 5),
- this->make(1, 1, 0)
- });
+ std::vector<TypeParam> data(
+ {this->make(0, 0, 1), this->make(1, 0, 2), this->make(2, 0, 3), this->make(0, 2, 4),
+ this->make(1, 2, 8), this->make(2, 2, 7), this->make(1, 0, 6), this->make(1, 2, 5),
+ this->make(1, 1, 0)});
const std::vector<TypeParam> expect{{data[0], data[2], data[3], data[5]}};
this->list.insert(this->list.begin(), data.begin(), data.end());
@@ -244,27 +243,24 @@ TYPED_TEST(TypedConvexHullTest, Collinear)
// check for order
auto it = this->list.begin();
- ASSERT_FLOAT_EQ(it->z, 1); ++it; // node 1
- ASSERT_FLOAT_EQ(it->z, 3); ++it; // node 1
- ASSERT_FLOAT_EQ(it->z, 7); ++it; // node 2
- ASSERT_FLOAT_EQ(it->z, 4); ++it; // node 3
+ ASSERT_FLOAT_EQ(it->z, 1);
+ ++it; // node 1
+ ASSERT_FLOAT_EQ(it->z, 3);
+ ++it; // node 1
+ ASSERT_FLOAT_EQ(it->z, 7);
+ ++it; // node 2
+ ASSERT_FLOAT_EQ(it->z, 4);
+ ++it; // node 3
ASSERT_EQ(it, last);
}
// degenerate cases
TYPED_TEST(TypedConvexHullTest, OverlappingPoints)
{
- std::vector<TypeParam> data({
- this->make(3, -1, 1),
- this->make(4, -2, 2),
- this->make(5, -7, 3),
- this->make(4, -2, 4),
- this->make(5, -7, 8),
- this->make(3, -1, 7),
- this->make(5, -7, 6),
- this->make(4, -2, 5),
- this->make(3, -1, 0)
- });
+ std::vector<TypeParam> data(
+ {this->make(3, -1, 1), this->make(4, -2, 2), this->make(5, -7, 3), this->make(4, -2, 4),
+ this->make(5, -7, 8), this->make(3, -1, 7), this->make(5, -7, 6), this->make(4, -2, 5),
+ this->make(3, -1, 0)});
const std::vector<TypeParam> expect{{data[0], data[1], data[2]}};
this->list.insert(this->list.begin(), data.begin(), data.end());
@@ -276,17 +272,10 @@ TYPED_TEST(TypedConvexHullTest, OverlappingPoints)
TYPED_TEST(TypedConvexHullTest, Line)
{
- std::vector<TypeParam> data({
- this->make(-3, 3, 1),
- this->make(-2, 2, 2),
- this->make(-1, 1, 3),
- this->make(-8, 8, 4),
- this->make(-6, 6, 8),
- this->make(-4, 4, 7),
- this->make(-10, 10, 6),
- this->make(-12, 12, 5),
- this->make(-11, 11, 0)
- });
+ std::vector<TypeParam> data(
+ {this->make(-3, 3, 1), this->make(-2, 2, 2), this->make(-1, 1, 3), this->make(-8, 8, 4),
+ this->make(-6, 6, 8), this->make(-4, 4, 7), this->make(-10, 10, 6), this->make(-12, 12, 5),
+ this->make(-11, 11, 0)});
const std::vector<TypeParam> expect{{data[2], data[7]}};
this->list.insert(this->list.begin(), data.begin(), data.end());
@@ -297,8 +286,10 @@ TYPED_TEST(TypedConvexHullTest, Line)
// check for order: this part is a little loose
auto it = this->list.begin();
- ASSERT_FLOAT_EQ(it->z, 5); ++it; // node 8
- ASSERT_FLOAT_EQ(it->z, 3); ++it; // node 3
+ ASSERT_FLOAT_EQ(it->z, 5);
+ ++it; // node 8
+ ASSERT_FLOAT_EQ(it->z, 3);
+ ++it; // node 3
ASSERT_EQ(it, last);
}
@@ -326,10 +317,14 @@ TYPED_TEST(TypedConvexHullTest, LowerHull)
// check for order: this part is a little loose
auto it = this->list.begin();
- ASSERT_FLOAT_EQ(it->z, 1); ++it;
- ASSERT_FLOAT_EQ(it->z, 2); ++it;
- ASSERT_FLOAT_EQ(it->z, 3); ++it;
- ASSERT_FLOAT_EQ(it->z, 4); ++it;
+ ASSERT_FLOAT_EQ(it->z, 1);
+ ++it;
+ ASSERT_FLOAT_EQ(it->z, 2);
+ ++it;
+ ASSERT_FLOAT_EQ(it->z, 3);
+ ++it;
+ ASSERT_FLOAT_EQ(it->z, 4);
+ ++it;
ASSERT_EQ(it, last);
}
@@ -359,11 +354,16 @@ TYPED_TEST(TypedConvexHullTest, Root)
this->check_hull(last, expect);
auto it = this->list.begin();
- ASSERT_FLOAT_EQ(it->z, 1); ++it;
- ASSERT_FLOAT_EQ(it->z, 2); ++it;
- ASSERT_FLOAT_EQ(it->z, 3); ++it;
- ASSERT_FLOAT_EQ(it->z, 4); ++it;
- ASSERT_FLOAT_EQ(it->z, 5); ++it;
+ ASSERT_FLOAT_EQ(it->z, 1);
+ ++it;
+ ASSERT_FLOAT_EQ(it->z, 2);
+ ++it;
+ ASSERT_FLOAT_EQ(it->z, 3);
+ ++it;
+ ASSERT_FLOAT_EQ(it->z, 4);
+ ++it;
+ ASSERT_FLOAT_EQ(it->z, 5);
+ ++it;
ASSERT_EQ(it, last);
EXPECT_NE(last, this->list.cend());
EXPECT_EQ(last->z, 6);
diff --git a/common/autoware_auto_geometry/test/src/test_geometry.cpp b/common/autoware_auto_geometry/test/src/test_geometry.cpp
index cbc1f06cb8bb1..936176e6f6af8 100644
--- a/common/autoware_auto_geometry/test/src/test_geometry.cpp
+++ b/common/autoware_auto_geometry/test/src/test_geometry.cpp
@@ -15,10 +15,8 @@
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
#include "gtest/gtest.h"
-
-#include "test_spatial_hash.hpp"
#include "test_bounding_box.hpp"
-
+#include "test_spatial_hash.hpp"
int32_t main(int32_t argc, char ** argv)
{
diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp
index a926d9e36b3fe..35bb53490b5d7 100644
--- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp
+++ b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp
@@ -14,21 +14,24 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <gtest/gtest.h>
+#include "geometry/convex_hull.hpp"
+#include "geometry/hull_pockets.hpp"
+
#include <geometry_msgs/msg/point32.hpp>
+
+#include <gtest/gtest.h>
+
+#include <iterator>
#include <list>
-#include <vector>
#include <utility>
-#include <iterator>
-#include "geometry/convex_hull.hpp"
-#include "geometry/hull_pockets.hpp"
+#include <vector>
-using autoware::common::types::float32_t;
-using autoware::common::types::float64_t;
using autoware::common::geometry::point_adapter::x_;
using autoware::common::geometry::point_adapter::y_;
+using autoware::common::types::float32_t;
+using autoware::common::types::float64_t;
-template<typename PointT>
+template <typename PointT>
class TypedHullPocketsTest : public ::testing::Test
{
protected:
@@ -48,38 +51,29 @@ TYPED_TEST_SUITE(TypedHullPocketsTest, PointTypes, );
/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE
// Inner test function, shared among all the tests
-template<typename Iter>
+template <typename Iter>
typename std::vector<std::vector<typename std::iterator_traits<Iter>::value_type>>
-compute_hull_and_pockets(
- const Iter polygon_start,
- const Iter polygon_end)
+compute_hull_and_pockets(const Iter polygon_start, const Iter polygon_end)
{
- auto convex_hull_list = std::list<typename std::iterator_traits<Iter>::value_type>{
- polygon_start, polygon_end};
- const auto cvx_hull_end =
- autoware::common::geometry::convex_hull(convex_hull_list);
+ auto convex_hull_list =
+ std::list<typename std::iterator_traits<Iter>::value_type>{polygon_start, polygon_end};
+ const auto cvx_hull_end = autoware::common::geometry::convex_hull(convex_hull_list);
const typename decltype(convex_hull_list)::const_iterator list_beginning =
convex_hull_list.begin();
- const auto pockets =
- autoware::common::geometry::hull_pockets(
- polygon_start, polygon_end,
- list_beginning, cvx_hull_end);
+ const auto pockets = autoware::common::geometry::hull_pockets(
+ polygon_start, polygon_end, list_beginning, cvx_hull_end);
return pockets;
}
-
// Test for a triangle - any triangle should really not result in any pockets.
TYPED_TEST(TypedHullPocketsTest, Triangle)
{
- const auto polygon = std::vector<decltype(this->make(0, 0, 0))> {
- this->make(0, 0, 0),
- this->make(2, 0, 0),
- this->make(1, 1, 0)
- };
+ const auto polygon = std::vector<decltype(this->make(0, 0, 0))>{
+ this->make(0, 0, 0), this->make(2, 0, 0), this->make(1, 1, 0)};
- const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() );
+ const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end());
ASSERT_EQ(pockets.size(), 0u);
}
@@ -95,18 +89,13 @@ TYPED_TEST(TypedHullPocketsTest, Triangle)
// This should not result in any pockets.
TYPED_TEST(TypedHullPocketsTest, Box)
{
- const auto polygon = std::vector<decltype(this->make(0, 0, 0))> {
- this->make(0, 0, 0),
- this->make(2, 0, 0),
- this->make(2, 1, 0),
- this->make(0, 1, 0)
- };
+ const auto polygon = std::vector<decltype(this->make(0, 0, 0))>{
+ this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 1, 0), this->make(0, 1, 0)};
- const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() );
+ const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end());
ASSERT_EQ(pockets.size(), 0u);
}
-
// Test for the use case:
// +-----+ +-----+
// / | | |
@@ -123,18 +112,12 @@ TYPED_TEST(TypedHullPocketsTest, Box)
// This should come up with a single box on the top left.
TYPED_TEST(TypedHullPocketsTest, Ushape)
{
- const auto polygon = std::vector<decltype(this->make(0, 0, 0))> {
- this->make(0, 0, 0),
- this->make(5, 0, 0),
- this->make(5, 4.5, 0),
- this->make(4, 5, 0),
- this->make(4, 2, 0),
- this->make(2, 2, 0),
- this->make(2, 5, 0),
- this->make(0, 4.5, 0),
+ const auto polygon = std::vector<decltype(this->make(0, 0, 0))>{
+ this->make(0, 0, 0), this->make(5, 0, 0), this->make(5, 4.5, 0), this->make(4, 5, 0),
+ this->make(4, 2, 0), this->make(2, 2, 0), this->make(2, 5, 0), this->make(0, 4.5, 0),
};
- const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() );
+ const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end());
ASSERT_EQ(pockets.size(), 1u);
ASSERT_EQ(pockets[0].size(), 4u);
@@ -159,18 +142,12 @@ TYPED_TEST(TypedHullPocketsTest, Ushape)
// top right.
TYPED_TEST(TypedHullPocketsTest, TypicalGap)
{
- const auto polygon = std::vector<decltype(this->make(0, 0, 0))> {
- this->make(0, 0, 0),
- this->make(10, 0, 0),
- this->make(10, 2, 0),
- this->make(8, 2, 0),
- this->make(8, 4, 0),
- this->make(6, 4, 0),
- this->make(6, 2, 0),
- this->make(0, 2, 0),
+ const auto polygon = std::vector<decltype(this->make(0, 0, 0))>{
+ this->make(0, 0, 0), this->make(10, 0, 0), this->make(10, 2, 0), this->make(8, 2, 0),
+ this->make(8, 4, 0), this->make(6, 4, 0), this->make(6, 2, 0), this->make(0, 2, 0),
};
- const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() );
+ const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end());
ASSERT_EQ(pockets.size(), 2u);
ASSERT_EQ(pockets[0].size(), 3u);
@@ -192,15 +169,12 @@ TYPED_TEST(TypedHullPocketsTest, TypicalGap)
// the segment of the final to the first point.
TYPED_TEST(TypedHullPocketsTest, EndsInPocket)
{
- const auto polygon = std::vector<decltype(this->make(0, 0, 0))> {
- this->make(0, 0, 0),
- this->make(2, 0, 0),
- this->make(2, 2, 0),
- this->make(0, 2, 0),
- this->make(0.1, 1, 0),
+ const auto polygon = std::vector<decltype(this->make(0, 0, 0))>{
+ this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 2, 0),
+ this->make(0, 2, 0), this->make(0.1, 1, 0),
};
- const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() );
+ const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end());
ASSERT_EQ(pockets.size(), 1u);
ASSERT_EQ(pockets[0].size(), 3u);
diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp
index f71bf335f4624..69c54960d4fc5 100644
--- a/common/autoware_auto_geometry/test/src/test_intersection.cpp
+++ b/common/autoware_auto_geometry/test/src/test_intersection.cpp
@@ -14,9 +14,11 @@
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <gtest/gtest.h>
-#include <geometry/intersection.hpp>
#include <geometry/convex_hull.hpp>
+#include <geometry/intersection.hpp>
+
+#include <gtest/gtest.h>
+
#include <list>
struct TestPoint
@@ -42,19 +44,18 @@ class IntersectionTest : public ::testing::TestWithParam<IntersectionTestParams>
{
};
-
-TEST_P(IntersectionTest, Basic) {
+TEST_P(IntersectionTest, Basic)
+{
const auto get_ordered_polygon = [](auto polygon) {
- order_ccw(polygon);
- return polygon;
- };
+ order_ccw(polygon);
+ return polygon;
+ };
const auto polygon1 = get_ordered_polygon(GetParam().polygon1_pts);
const auto polygon2 = get_ordered_polygon(GetParam().polygon2_pts);
const auto expected_intersection = get_ordered_polygon(GetParam().expected_intersection_pts);
- const auto result =
- autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2);
+ const auto result = autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2);
ASSERT_EQ(result.size(), expected_intersection.size());
auto expected_shape_it = expected_intersection.begin();
@@ -68,71 +69,56 @@ TEST_P(IntersectionTest, Basic) {
INSTANTIATE_TEST_SUITE_P(
Basic, IntersectionTest,
::testing::Values(
- IntersectionTestParams{
- {},
- {},
- {}
-},
- IntersectionTestParams{ // Partial intersection
- {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
- {{5.0F, 5.0F}, {15.0F, 5.0F}, {5.0F, 15.0F}, {15.0F, 15.0F}},
- {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}
-},
+ IntersectionTestParams{{}, {}, {}},
+ IntersectionTestParams{// Partial intersection
+ {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
+ {{5.0F, 5.0F}, {15.0F, 5.0F}, {5.0F, 15.0F}, {15.0F, 15.0F}},
+ {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}},
// Full intersection with overlapping edges
// TODO(yunus.caliskan): enable after #1231
-// IntersectionTestParams{
-// {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
-// {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}},
-// {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}},
-// },
- IntersectionTestParams{ // Fully contained
- {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
- {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}},
- {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}},
-},
- IntersectionTestParams{ // Fully contained triangle
- {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
- {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}},
- {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}},
-},
- IntersectionTestParams{ // Triangle rectangle intersection.
- {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
- {{5.0F, 1.0F}, {5.0F, 9.0F}, {15.0F, 5.0F}},
- {{5.0F, 1.0F}, {5.0F, 9.0F}, {10.0F, 3.0F}, {10.0F, 7.0F}}
-},
- IntersectionTestParams{ // No intersection
- {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
- {{15.0F, 15.0F}, {20.0F, 15.0F}, {15.0F, 20.0F}, {20.0F, 20.0F}},
- {}
-}
- // cppcheck-suppress syntaxError
-));
-
-TEST(PolygonPointTest, Basic) {
+ // IntersectionTestParams{
+ // {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
+ // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}},
+ // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}},
+ // },
+ IntersectionTestParams{
+ // Fully contained
+ {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
+ {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}},
+ {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}},
+ },
+ IntersectionTestParams{
+ // Fully contained triangle
+ {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
+ {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}},
+ {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}},
+ },
+ IntersectionTestParams{// Triangle rectangle intersection.
+ {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
+ {{5.0F, 1.0F}, {5.0F, 9.0F}, {15.0F, 5.0F}},
+ {{5.0F, 1.0F}, {5.0F, 9.0F}, {10.0F, 3.0F}, {10.0F, 7.0F}}},
+ IntersectionTestParams{// No intersection
+ {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}},
+ {{15.0F, 15.0F}, {20.0F, 15.0F}, {15.0F, 20.0F}, {20.0F, 20.0F}},
+ {}} // cppcheck-suppress syntaxError
+ ));
+
+TEST(PolygonPointTest, Basic)
+{
GTEST_SKIP(); // TODO(yunus.caliskan): enable after #1231
std::list<TestPoint> polygon{{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}};
order_ccw(polygon);
- EXPECT_FALSE(
- autoware::common::geometry::is_point_inside_polygon_2d(
- polygon.begin(), polygon.end(), TestPoint{0.0F, 10.0F}));
+ EXPECT_FALSE(autoware::common::geometry::is_point_inside_polygon_2d(
+ polygon.begin(), polygon.end(), TestPoint{0.0F, 10.0F}));
}
// IoU of two intersecting shapes: a pentagon and a square. The test includes pen and paper
// computations for the intermediate steps as assertions.
-TEST(IoUTest, PentagonRectangleIntersection) {
+TEST(IoUTest, PentagonRectangleIntersection)
+{
std::list<TestPoint> polygon1{
- {0.0F, 3.0F},
- {3.0F, 4.0F},
- {6.0F, 3.0F},
- {4.0F, 1.0F},
- {2.0F, 1.0F}
- };
- std::list<TestPoint> polygon2{
- {3.0F, 0.0F},
- {3.0F, 2.0F},
- {5.0F, 2.0F},
- {5.0F, 0.0F}
- };
+ {0.0F, 3.0F}, {3.0F, 4.0F}, {6.0F, 3.0F}, {4.0F, 1.0F}, {2.0F, 1.0F}};
+ std::list<TestPoint> polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}};
order_ccw(polygon1);
order_ccw(polygon2);
@@ -156,19 +142,10 @@ TEST(IoUTest, PentagonRectangleIntersection) {
}
// IoU of two non-intersecting rectangles.
-TEST(IoUTest, NoIntersection) {
- std::list<TestPoint> polygon1{
- {0.0F, 0.0F},
- {0.0F, 1.0F},
- {1.0F, 1.0F},
- {1.0F, 0.0F}
- };
- std::list<TestPoint> polygon2{
- {3.0F, 0.0F},
- {3.0F, 2.0F},
- {5.0F, 2.0F},
- {5.0F, 0.0F}
- };
+TEST(IoUTest, NoIntersection)
+{
+ std::list<TestPoint> polygon1{{0.0F, 0.0F}, {0.0F, 1.0F}, {1.0F, 1.0F}, {1.0F, 0.0F}};
+ std::list<TestPoint> polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}};
order_ccw(polygon1);
order_ccw(polygon2);
diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp
index 2314159d2ba1c..ba8d5742dadc5 100644
--- a/common/autoware_auto_geometry/test/src/test_interval.cpp
+++ b/common/autoware_auto_geometry/test/src/test_interval.cpp
@@ -18,17 +18,17 @@
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
+#include "geometry/interval.hpp"
+
#include <gtest/gtest.h>
#include <algorithm>
#include <cmath>
#include <limits>
-#include "geometry/interval.hpp"
-
+using autoware::common::geometry::Interval;
using autoware::common::geometry::Interval_d;
using autoware::common::geometry::Interval_f;
-using autoware::common::geometry::Interval;
namespace
{
@@ -41,7 +41,8 @@ const auto epsilon = 1e-5;
//------------------------------------------------------------------------------
-TEST(GeometryInterval, AbsEq) {
+TEST(GeometryInterval, AbsEq)
+{
const auto i1 = Interval_d(-1.0, 1.0);
const auto i2 = Interval_d(-1.0 + 0.5 * epsilon, 1.0 + 0.5 * epsilon);
const auto shift = (2.0 * epsilon);
@@ -60,30 +61,19 @@ TEST(GeometryInterval, AbsEq) {
//------------------------------------------------------------------------------
-TEST(GeometryInterval, IsSubsetEq) {
- EXPECT_TRUE(
- Interval_d::is_subset_eq(
- Interval_d(-0.5, 0.5),
- Interval_d(-1.0, 1.0)));
- EXPECT_TRUE(
- Interval_d::is_subset_eq(
- Interval_d(3.2, 4.2),
- Interval_d(3.2, 4.2)));
- EXPECT_FALSE(
- Interval_d::is_subset_eq(
- Interval_d(-3.0, -1.0),
- Interval_d(1.0, 3.0)));
- EXPECT_FALSE(
- Interval_d::is_subset_eq(
- Interval_d(1.0, 3.0),
- Interval_d(2.0, 4.0)));
- EXPECT_FALSE(
- Interval_d::is_subset_eq(Interval_d(), Interval_d()));
+TEST(GeometryInterval, IsSubsetEq)
+{
+ EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(-0.5, 0.5), Interval_d(-1.0, 1.0)));
+ EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(3.2, 4.2), Interval_d(3.2, 4.2)));
+ EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(-3.0, -1.0), Interval_d(1.0, 3.0)));
+ EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(1.0, 3.0), Interval_d(2.0, 4.0)));
+ EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(), Interval_d()));
}
//------------------------------------------------------------------------------
-TEST(GeometryInterval, ClampTo) {
+TEST(GeometryInterval, ClampTo)
+{
const auto i = Interval_d(-1.0, 1.0);
{
const auto val = 0.0;
@@ -111,7 +101,8 @@ TEST(GeometryInterval, ClampTo) {
//------------------------------------------------------------------------------
-TEST(GeometryInterval, Comparisons) {
+TEST(GeometryInterval, Comparisons)
+{
{
const auto i1 = Interval_d(0.25, 1);
const auto i2 = Interval_d(0, 1);
@@ -157,7 +148,8 @@ TEST(GeometryInterval, Comparisons) {
//------------------------------------------------------------------------------
-TEST(GeometryInterval, Contains) {
+TEST(GeometryInterval, Contains)
+{
{
const auto i = Interval_d();
EXPECT_FALSE(Interval_d::contains(i, 0.0));
@@ -172,7 +164,8 @@ TEST(GeometryInterval, Contains) {
//------------------------------------------------------------------------------
-TEST(GeometryInterval, Empty) {
+TEST(GeometryInterval, Empty)
+{
{
const auto i1 = Interval_d();
const auto i2 = Interval_d();
@@ -190,7 +183,8 @@ TEST(GeometryInterval, Empty) {
//------------------------------------------------------------------------------
-TEST(GeometryInterval, ZeroMeasure) {
+TEST(GeometryInterval, ZeroMeasure)
+{
{
const auto i = Interval_d(0, 1);
EXPECT_FALSE(Interval_d::zero_measure(i));
@@ -209,7 +203,8 @@ TEST(GeometryInterval, ZeroMeasure) {
//------------------------------------------------------------------------------
-TEST(GeometryInterval, IntersectionMeasure) {
+TEST(GeometryInterval, IntersectionMeasure)
+{
{
const auto i1 = Interval_d(-1.0, 1.0);
const auto i2 = Interval_d(-0.5, 1.5);
@@ -232,7 +227,8 @@ TEST(GeometryInterval, IntersectionMeasure) {
//------------------------------------------------------------------------------
-TEST(GeometryInterval, ConstructionMeasure) {
+TEST(GeometryInterval, ConstructionMeasure)
+{
{
const auto i = Interval_d();
EXPECT_TRUE(std::isnan(Interval_d::min(i)));
@@ -256,7 +252,7 @@ TEST(GeometryInterval, ConstructionMeasure) {
}
{
- EXPECT_THROW({Interval_d(1.0, -1.0);}, std::runtime_error);
+ EXPECT_THROW({ Interval_d(1.0, -1.0); }, std::runtime_error);
}
}
diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_auto_perception_rviz_plugin/README.md
index 68f96d654e258..cb5deb48f411c 100644
--- a/common/autoware_auto_perception_rviz_plugin/README.md
+++ b/common/autoware_auto_perception_rviz_plugin/README.md
@@ -25,7 +25,7 @@ Example:
#### Visualization Result
-
+
### TrackedObjects
@@ -39,7 +39,7 @@ Example:
Overwrite tracking results with detection results.
-
+
### PredictedObjects
@@ -53,10 +53,10 @@ Overwrite tracking results with detection results.
Overwrite prediction results with tracking results.
-
+
## References/External links
-[1] https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/tree/master/src/tools/visualization/autoware_rviz_plugins
+[1] <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/tree/master/src/tools/visualization/autoware_rviz_plugins>
## Future extensions / Unimplemented parts
diff --git a/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp b/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp
index 50edcf844e6a6..8c43192a26bd6 100644
--- a/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp
+++ b/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp
@@ -14,13 +14,15 @@
#ifndef COMMON__COLOR_ALPHA_PROPERTY_HPP_
#define COMMON__COLOR_ALPHA_PROPERTY_HPP_
-#include <memory>
#include <rviz_common/display.hpp>
#include <rviz_common/properties/color_property.hpp>
#include <rviz_common/properties/float_property.hpp>
-#include <std_msgs/msg/color_rgba.hpp>
#include <visibility_control.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+
+#include <memory>
+
namespace autoware
{
namespace rviz_plugins
diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp
index 9ff8adac9d2e4..67dac25c796bb 100644
--- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp
+++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp
@@ -14,9 +14,10 @@
#ifndef OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_
#define OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_
-#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <object_detection/object_polygon_display_base.hpp>
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+
namespace autoware
{
namespace rviz_plugins
@@ -25,7 +26,7 @@ namespace object_detection
{
/// \brief Class defining rviz plugin to visualize DetectedObjects
class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay
- : public ObjectPolygonDisplayBase<autoware_auto_perception_msgs::msg::DetectedObjects>
+: public ObjectPolygonDisplayBase<autoware_auto_perception_msgs::msg::DetectedObjects>
{
Q_OBJECT
diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp
index 54414ab442b1b..e1b84abbcec34 100644
--- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp
+++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp
@@ -15,20 +15,22 @@
#ifndef OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_
#define OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_
+#include <rclcpp/logging.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <visibility_control.hpp>
+
#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
#include <autoware_auto_perception_msgs/msg/shape.hpp>
-#include <algorithm>
#include <geometry_msgs/msg/pose_with_covariance.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/twist_with_covariance.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+
+#include <algorithm>
#include <map>
-#include <rclcpp/logging.hpp>
-#include <rclcpp/rclcpp.hpp>
#include <string>
#include <vector>
-#include <visibility_control.hpp>
-#include <visualization_msgs/msg/marker.hpp>
namespace autoware
{
@@ -53,20 +55,20 @@ struct ObjectPropertyValues
// Map defining colors according to value of label field in ObjectClassification msg
const std::map<
autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues>
-// Color map is based on cityscapes color
-kDefaultObjectPropertyValues = {
- {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN,
- {"UNKNOWN", {255, 255, 255}}},
- {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}},
- {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}},
- {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN,
- {"PEDESTRIAN", {255, 192, 203}}},
- {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}},
- {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE,
- {"MOTORCYCLE", {119, 11, 32}}},
- {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER,
- {"TRAILER", {30, 144, 255}}},
- {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}};
+ // Color map is based on cityscapes color
+ kDefaultObjectPropertyValues = {
+ {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN,
+ {"UNKNOWN", {255, 255, 255}}},
+ {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}},
+ {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}},
+ {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN,
+ {"PEDESTRIAN", {255, 192, 203}}},
+ {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}},
+ {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE,
+ {"MOTORCYCLE", {119, 11, 32}}},
+ {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER,
+ {"TRAILER", {30, 144, 255}}},
+ {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}};
/// \brief Convert the given polygon into a marker representing the shape in 3d
/// \param shape_msg Shape msg to be converted. Corners should be in object-local frame
@@ -182,14 +184,14 @@ inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose init
/// \param labels List of ObjectClassificationMsg objects
/// \param logger_name Name to use for logger in case of a warning (if labels is empty)
/// \return Id of the best classification, Unknown if there is no best label
-template<typename ClassificationContainerT>
+template <typename ClassificationContainerT>
AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC
-autoware_auto_perception_msgs::msg::ObjectClassification::_label_type
-get_best_label(ClassificationContainerT labels, const std::string & logger_name)
+ autoware_auto_perception_msgs::msg::ObjectClassification::_label_type
+ get_best_label(ClassificationContainerT labels, const std::string & logger_name)
{
const auto best_class_label = std::max_element(
labels.begin(), labels.end(),
- [](const auto & a, const auto & b) -> bool {return a.probability < b.probability;});
+ [](const auto & a, const auto & b) -> bool { return a.probability < b.probability; });
if (best_class_label == labels.end()) {
RCLCPP_WARN(
rclcpp::get_logger(logger_name),
diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp
index 1222d4c82d12e..bab877d9d7053 100644
--- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp
+++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp
@@ -14,22 +14,24 @@
#ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
#define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
-#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
-#include <bitset>
#include <common/color_alpha_property.hpp>
-#include <list>
-#include <memory>
#include <object_detection/object_polygon_detail.hpp>
#include <rviz_common/display.hpp>
#include <rviz_common/properties/color_property.hpp>
#include <rviz_common/properties/float_property.hpp>
#include <rviz_default_plugins/displays/marker/marker_common.hpp>
#include <rviz_default_plugins/displays/marker_array/marker_array_display.hpp>
-#include <string>
+#include <visibility_control.hpp>
+
+#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
#include <unique_identifier_msgs/msg/uuid.hpp>
+
+#include <bitset>
+#include <list>
+#include <memory>
+#include <string>
#include <unordered_map>
#include <vector>
-#include <visibility_control.hpp>
namespace autoware
{
@@ -41,9 +43,9 @@ namespace object_detection
/// for the plugin and also defines common helper functions that can be used by its derived
/// classes.
/// \tparam MsgT PredictedObjects or TrackedObjects or DetectedObjects type
-template<typename MsgT>
+template <typename MsgT>
class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
- : public rviz_common::RosTopicDisplay<MsgT>
+: public rviz_common::RosTopicDisplay<MsgT>
{
public:
using Color = std::array<float, 3U>;
@@ -109,7 +111,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
m_marker_common.load(config);
}
- void update(float wall_dt, float ros_dt) override {m_marker_common.update(wall_dt, ros_dt);}
+ void update(float wall_dt, float ros_dt) override { m_marker_common.update(wall_dt, ros_dt); }
void reset() override
{
@@ -117,7 +119,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
m_marker_common.clearMarkers();
}
- void clear_markers() {m_marker_common.clearMarkers();}
+ void clear_markers() { m_marker_common.clearMarkers(); }
void add_marker(visualization_msgs::msg::Marker::ConstSharedPtr marker_ptr)
{
@@ -132,7 +134,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
/// \param orientation Orientation of the shape in Object.header.frame_id frame
/// \param labels List of ObjectClassificationMsg objects
/// \return Marker ptr. Id and header will have to be set by the caller
- template<typename ClassificationContainerT>
+ template <typename ClassificationContainerT>
std::optional<Marker::SharedPtr> get_shape_marker_ptr(
const autoware_auto_perception_msgs::msg::Shape & shape_msg,
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
@@ -152,7 +154,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
/// \param centroid Centroid position of the shape in Object.header.frame_id frame
/// \param labels List of ObjectClassificationMsg objects
/// \return Marker ptr. Id and header will have to be set by the caller
- template<typename ClassificationContainerT>
+ template <typename ClassificationContainerT>
std::optional<Marker::SharedPtr> get_label_marker_ptr(
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const ClassificationContainerT & labels) const
@@ -166,7 +168,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
}
}
- template<typename ClassificationContainerT>
+ template <typename ClassificationContainerT>
std::optional<Marker::SharedPtr> get_uuid_marker_ptr(
const unique_identifier_msgs::msg::UUID & uuid, const geometry_msgs::msg::Point & centroid,
const ClassificationContainerT & labels) const
@@ -190,7 +192,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
}
}
- template<typename ClassificationContainerT>
+ template <typename ClassificationContainerT>
std::optional<Marker::SharedPtr> get_velocity_text_marker_ptr(
const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos,
const ClassificationContainerT & labels) const
@@ -246,7 +248,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
/// \param labels list of classifications
/// \return Color and alpha for the best class in the given list. Unknown class is used in
/// degenerate cases
- template<typename ClassificationContainerT>
+ template <typename ClassificationContainerT>
std_msgs::msg::ColorRGBA get_color_rgba(const ClassificationContainerT & labels) const
{
static const std::string kLoggerName("ObjectPolygonDisplayBase");
@@ -262,7 +264,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
/// \tparam ClassificationContainerT Container of ObjectClassification
/// \param labels list of classifications
/// \return best label string
- template<typename ClassificationContainerT>
+ template <typename ClassificationContainerT>
std::string get_best_label(const ClassificationContainerT & labels) const
{
static const std::string kLoggerName("ObjectPolygonDisplayBase");
@@ -287,7 +289,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
get_color_from_uuid(const std::string & uuid) const
{
int i = (static_cast<int>(uuid.at(0)) * 4 + static_cast<int>(uuid.at(1))) %
- static_cast<int>(predicted_path_colors.size());
+ static_cast<int>(predicted_path_colors.size());
std_msgs::msg::ColorRGBA color;
color.r = predicted_path_colors.at(i).r;
diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp
index 82c0898718f4b..737fb8a59efbd 100644
--- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp
+++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp
@@ -14,12 +14,15 @@
#ifndef OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_
#define OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_
+#include <object_detection/object_polygon_display_base.hpp>
+
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
+
#include <list>
#include <map>
-#include <object_detection/object_polygon_display_base.hpp>
#include <string>
#include <vector>
@@ -31,7 +34,7 @@ namespace object_detection
{
/// \brief Class defining rviz plugin to visualize PredictedObjects
class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
- : public ObjectPolygonDisplayBase<autoware_auto_perception_msgs::msg::PredictedObjects>
+: public ObjectPolygonDisplayBase<autoware_auto_perception_msgs::msg::PredictedObjects>
{
Q_OBJECT
@@ -65,8 +68,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
auto itr = id_map.begin();
while (itr != id_map.end()) {
if (
- std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end())
- {
+ std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) {
unused_marker_ids.push_back(itr->second);
itr = id_map.erase(itr);
} else {
diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp
index 43077ce1ce32e..9f00a2cb1cde2 100644
--- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp
+++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp
@@ -14,12 +14,15 @@
#ifndef OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_
#define OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_
+#include <object_detection/object_polygon_display_base.hpp>
+
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
+
#include <list>
#include <map>
-#include <object_detection/object_polygon_display_base.hpp>
#include <string>
#include <vector>
@@ -31,7 +34,7 @@ namespace object_detection
{
/// \brief Class defining rviz plugin to visualize TrackedObjects
class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay
- : public ObjectPolygonDisplayBase<autoware_auto_perception_msgs::msg::TrackedObjects>
+: public ObjectPolygonDisplayBase<autoware_auto_perception_msgs::msg::TrackedObjects>
{
Q_OBJECT
@@ -65,8 +68,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay
auto itr = id_map.begin();
while (itr != id_map.end()) {
if (
- std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end())
- {
+ std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) {
unused_marker_ids.push_back(itr->second);
itr = id_map.erase(itr);
} else {
diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml
index 24d48fe5e3ee4..3bcbaa6294535 100644
--- a/common/autoware_auto_perception_rviz_plugin/package.xml
+++ b/common/autoware_auto_perception_rviz_plugin/package.xml
@@ -1,26 +1,28 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
- <name>autoware_auto_perception_rviz_plugin</name>
- <version>1.0.0</version>
- <description>Contains plugins to visualize object detection outputs</description>
- <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
- <maintainer email="taichi.higashide@tier4.jp">Taichi Higashide</maintainer>
- <license>Apache 2.0</license>
+ <name>autoware_auto_perception_rviz_plugin</name>
+ <version>1.0.0</version>
+ <description>Contains plugins to visualize object detection outputs</description>
+ <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
+ <maintainer email="taichi.higashide@tier4.jp">Taichi Higashide</maintainer>
+ <license>Apache 2.0</license>
- <buildtool_depend>ament_cmake</buildtool_depend>
+ <buildtool_depend>ament_cmake</buildtool_depend>
- <build_depend>qtbase5-dev</build_depend>
+ <build_depend>qtbase5-dev</build_depend>
- <depend>autoware_auto_perception_msgs</depend>
- <depend>rviz_common</depend>
- <depend>rviz_default_plugins</depend>
+ <depend>autoware_auto_perception_msgs</depend>
+ <depend>rviz_common</depend>
+ <depend>rviz_default_plugins</depend>
- <exec_depend>libqt5-widgets</exec_depend>
- <exec_depend>rviz2</exec_depend>
+ <exec_depend>libqt5-widgets</exec_depend>
+ <exec_depend>rviz2</exec_depend>
- <test_depend>ament_lint_auto</test_depend>
- <test_depend>ament_lint_common</test_depend>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>autoware_lint_common</test_depend>
- <export><build_type>ament_cmake</build_type></export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp
index ebb3783845567..ac9c5af4ddeef 100644
--- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp
+++ b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp
@@ -15,6 +15,7 @@
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
#include <common/color_alpha_property.hpp>
+
#include <memory>
namespace autoware
diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp
index b3c645e8eff48..9feb7433422c2 100644
--- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp
+++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp
@@ -14,17 +14,17 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <memory>
#include <object_detection/detected_objects_display.hpp>
+#include <memory>
+
namespace autoware
{
namespace rviz_plugins
{
namespace object_detection
{
-DetectedObjectsDisplay::DetectedObjectsDisplay()
-: ObjectPolygonDisplayBase("detected_objects") {}
+DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("detected_objects") {}
void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg)
{
diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
index 5f65fce8c72ea..f957d42b2ce70 100644
--- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
+++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
@@ -12,13 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License..
-#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Eigen>
-#include <cmath>
+#include <object_detection/object_polygon_detail.hpp>
+
#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <algorithm>
+#include <cmath>
#include <memory>
-#include <object_detection/object_polygon_detail.hpp>
#include <string>
#include <vector>
@@ -375,23 +377,23 @@ void calc_cylinder_line_list(
for (int i = 0; i < n; ++i) {
geometry_msgs::msg::Point point;
point.x = std::cos(
- (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
- M_PI / static_cast<double>(n)) *
- radius;
+ (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
+ M_PI / static_cast<double>(n)) *
+ radius;
point.y = std::sin(
- (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
- M_PI / static_cast<double>(n)) *
- radius;
+ (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
+ M_PI / static_cast<double>(n)) *
+ radius;
point.z = shape.dimensions.z * 0.5;
points.push_back(point);
point.x = std::cos(
- (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
- M_PI / static_cast<double>(n)) *
- radius;
+ (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
+ M_PI / static_cast<double>(n)) *
+ radius;
point.y = std::sin(
- (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
- M_PI / static_cast<double>(n)) *
- radius;
+ (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
+ M_PI / static_cast<double>(n)) *
+ radius;
point.z = -shape.dimensions.z * 0.5;
points.push_back(point);
}
@@ -405,27 +407,27 @@ void calc_circle_line_list(
for (int i = 0; i < n; ++i) {
geometry_msgs::msg::Point point;
point.x = std::cos(
- (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
- M_PI / static_cast<double>(n)) *
- radius +
- center.x;
+ (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
+ M_PI / static_cast<double>(n)) *
+ radius +
+ center.x;
point.y = std::sin(
- (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
- M_PI / static_cast<double>(n)) *
- radius +
- center.y;
+ (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
+ M_PI / static_cast<double>(n)) *
+ radius +
+ center.y;
point.z = center.z;
points.push_back(point);
point.x = std::cos(
- (static_cast<double>(i + 1.0) / static_cast<double>(n)) * 2.0 * M_PI +
- M_PI / static_cast<double>(n)) *
- radius +
- center.x;
+ (static_cast<double>(i + 1.0) / static_cast<double>(n)) * 2.0 * M_PI +
+ M_PI / static_cast<double>(n)) *
+ radius +
+ center.x;
point.y = std::sin(
- (static_cast<double>(i + 1.0) / static_cast<double>(n)) * 2.0 * M_PI +
- M_PI / static_cast<double>(n)) *
- radius +
- center.y;
+ (static_cast<double>(i + 1.0) / static_cast<double>(n)) * 2.0 * M_PI +
+ M_PI / static_cast<double>(n)) *
+ radius +
+ center.y;
point.z = center.z;
points.push_back(point);
}
@@ -448,11 +450,11 @@ void calc_polygon_line_list(
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
point.x = shape.footprint.points
- .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
- .x;
+ .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
+ .x;
point.y = shape.footprint.points
- .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
- .y;
+ .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
+ .y;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
}
@@ -463,11 +465,11 @@ void calc_polygon_line_list(
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
point.x = shape.footprint.points
- .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
- .x;
+ .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
+ .x;
point.y = shape.footprint.points
- .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
- .y;
+ .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
+ .y;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
}
diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp
index 9bb4e79f5f75d..45f60fadacfb7 100644
--- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp
+++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp
@@ -12,17 +12,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <memory>
#include <object_detection/predicted_objects_display.hpp>
+#include <memory>
+
namespace autoware
{
namespace rviz_plugins
{
namespace object_detection
{
-PredictedObjectsDisplay::PredictedObjectsDisplay()
-: ObjectPolygonDisplayBase("tracks") {}
+PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") {}
void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr msg)
{
diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp
index bbdcca607bf35..f04d4adfe1cd0 100644
--- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp
+++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp
@@ -14,17 +14,17 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-#include <memory>
#include <object_detection/tracked_objects_display.hpp>
+#include <memory>
+
namespace autoware
{
namespace rviz_plugins
{
namespace object_detection
{
-TrackedObjectsDisplay::TrackedObjectsDisplay()
-: ObjectPolygonDisplayBase("tracks") {}
+TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") {}
void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg)
{
diff --git a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md
index fac3a78b0dbc2..0d8ed4e76319a 100644
--- a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md
+++ b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md
@@ -1,10 +1,8 @@
-autoware_auto_tf2 {#autoware-auto-tf2-design}
-=================
+# autoware_auto_tf2 {#autoware-auto-tf2-design}
This is the design document for the `autoware_auto_tf2` package.
-
-# Purpose / Use cases
+## Purpose / Use cases
In general, users of ROS rely on tf (and its successor, tf2) for publishing and utilizing coordinate
frame transforms. This is true even to the extent that the tf2 contains the packages
@@ -15,21 +13,23 @@ The `autoware_auto_tf2` package aims to provide developers with tools to transfo
`autoware_auto_msgs` types. In addition to this, this package also provides transform tools for
messages types in `geometry_msgs` missing in `tf2_geometry_msgs`.
-
-# Design
+## Design
While writing `tf2_some_msgs` or contributing to `tf2_geometry_msgs`, compatibility and design
intent was ensured with the following files in the existing tf2 framework:
- * `tf2/convert.h`
- * `tf2_ros/buffer_interface.h`
+
+- `tf2/convert.h`
+- `tf2_ros/buffer_interface.h`
For example:
-```
+
+```cpp
void tf2::convert( const A & a,B & b)
```
The method `tf2::convert` is dependent on the following:
-```
+
+```cpp
template<typename A, typename B>
B tf2::toMsg(const A& a);
template<typename A, typename B>
@@ -40,7 +40,8 @@ tf2_ros::BufferInterface::transform(...)
```
Which, in turn, is dependent on the following:
-```
+
+```cpp
void tf2::convert( const A & a,B & b)
const std::string& tf2::getFrameId(const T& t)
const ros::Time& tf2::getTimestamp(const T& t);
@@ -50,16 +51,18 @@ const ros::Time& tf2::getTimestamp(const T& t);
In both ROS1 and ROS2 stamped msgs like `Vector3Stamped`, `QuaternionStamped` have associated
functions like:
- * `getTimestamp`
- * `getFrameId`
- * `doTransform`
- * `toMsg`
- * `fromMsg`
+
+- `getTimestamp`
+- `getFrameId`
+- `doTransform`
+- `toMsg`
+- `fromMsg`
In ROS1, to support `tf2::convert` and need in `doTransform` of the stamped data, non-stamped
underlying data like `Vector3`, `Point`, have implementations of the following functions:
- * `toMsg`
- * `fromMsg`
+
+- `toMsg`
+- `fromMsg`
In ROS2, much of the `doTransform` method is not using `toMsg` and `fromMsg` as data types from tf2
are not used. Instead `doTransform` is done using `KDL`, thus functions relating to underlying data
@@ -79,7 +82,8 @@ implementation should be done such that upstream contributions could also be mad
Due to conflicts in a function signatures, the predefined template of `convert.h`/
`transform_functions.h` is not followed and compatibility with `tf2::convert(..)` is broken and
`toMsg` is written differently.
-```
+
+```cpp
// Old style
geometry_msgs::Vector3 toMsg(const tf2::Vector3& in)
geometry_msgs::Point& toMsg(const tf2::Vector3& in)
@@ -88,8 +92,8 @@ geometry_msgs::Point& toMsg(const tf2::Vector3& in)
geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out)
```
-
## Inputs / Outputs / API
+
<!-- Required -->
The library provides API `doTransform` for the following data-types that are either not available
@@ -97,28 +101,35 @@ in `tf2_geometry_msgs` or the messages types are part of `autoware_auto_msgs` an
custom and not inherently supported by any of the tf2 libraries. The following APIs are provided
for the following data types:
-* `Point32`
-```
+- `Point32`
+
+```cpp
inline void doTransform(
const geometry_msgs::msg::Point32 & t_in,
geometry_msgs::msg::Point32 & t_out,
const geometry_msgs::msg::TransformStamped & transform)
```
-* `Quarternion32` (`autoware_auto_msgs`)
-```
+
+- `Quarternion32` (`autoware_auto_msgs`)
+
+```cpp
inline void doTransform(
const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in,
autoware_auto_geometry_msgs::msg::Quaternion32 & t_out,
const geometry_msgs::msg::TransformStamped & transform)
```
-* `BoundingBox` (`autoware_auto_msgs`)
-```
+
+- `BoundingBox` (`autoware_auto_msgs`)
+
+```cpp
inline void doTransform(
const BoundingBox & t_in, BoundingBox & t_out,
const geometry_msgs::msg::TransformStamped & transform)
```
-* `BoundingBoxArray`
-```
+
+- `BoundingBoxArray`
+
+```cpp
inline void doTransform(
const BoundingBoxArray & t_in,
BoundingBoxArray & t_out,
@@ -126,8 +137,10 @@ inline void doTransform(
```
In addition, the following helper methods are also added:
-* `BoundingBoxArray`
-```
+
+- `BoundingBoxArray`
+
+```cpp
inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t)
inline std::string getFrameId(const BoundingBoxArray & t)
@@ -136,11 +149,9 @@ inline std::string getFrameId(const BoundingBoxArray & t)
<!-- ## Inner-workings / Algorithms -->
<!-- If applicable -->
-
<!-- ## Error detection and handling -->
<!-- Required -->
-
<!-- # Security considerations -->
<!-- Required -->
<!-- Things to consider:
@@ -151,25 +162,22 @@ inline std::string getFrameId(const BoundingBoxArray & t)
- Denial of Service (How do you handle spamming?).
- Elevation of Privilege (Do you need to change permission levels during execution?) -->
-
<!-- # References / External links -->
<!-- Optional -->
-
-# Future extensions / Unimplemented parts
+## Future extensions / Unimplemented parts
## Challenges
- * `tf2_geometry_msgs` does not implement `doTransform` for any non-stamped datatypes, but it is
- possible with the same function template. It is needed when transforming sub-data, with main data
- that does have a stamp and can call doTransform on the sub-data with the same transform. Is this a useful upstream contribution?
- * `tf2_geometry_msgs` does not have `Point`, `Point32`, does not seem it needs one, also the
- implementation of non-standard `toMsg` would not help the convert.
- * `BoundingBox` uses 32-bit float like `Quaternion32` and `Point32` to save space, as they are used
- repeatedly in `BoundingBoxArray`. While transforming is it better to convert to 64-bit `Quaternion`,
- `Point`, or `PoseStamped`, to re-use existing implementation of `doTransform`, or does it need to be
- implemented? Templatization may not be simple.
-
+- `tf2_geometry_msgs` does not implement `doTransform` for any non-stamped datatypes, but it is
+ possible with the same function template. It is needed when transforming sub-data, with main data
+ that does have a stamp and can call doTransform on the sub-data with the same transform. Is this a useful upstream contribution?
+- `tf2_geometry_msgs` does not have `Point`, `Point32`, does not seem it needs one, also the
+ implementation of non-standard `toMsg` would not help the convert.
+- `BoundingBox` uses 32-bit float like `Quaternion32` and `Point32` to save space, as they are used
+ repeatedly in `BoundingBoxArray`. While transforming is it better to convert to 64-bit `Quaternion`,
+ `Point`, or `PoseStamped`, to re-use existing implementation of `doTransform`, or does it need to be
+ implemented? Templatization may not be simple.
<!-- # Related issues -->
<!-- Required -->
diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp
index c8ce96b1a6eca..f4bba95790e6b 100644
--- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp
+++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp
@@ -17,19 +17,21 @@
#ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_
#define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_
+#include <common/types.hpp>
+#include <kdl/frames.hpp>
+
+#include <autoware_auto_geometry_msgs/msg/quaternion32.hpp>
+#include <autoware_auto_perception_msgs/msg/bounding_box.hpp>
+#include <autoware_auto_perception_msgs/msg/bounding_box_array.hpp>
+#include <geometry_msgs/msg/point32.hpp>
+#include <geometry_msgs/msg/polygon.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
#include <tf2/convert.h>
#include <tf2/time.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <autoware_auto_perception_msgs/msg/bounding_box_array.hpp>
-#include <autoware_auto_perception_msgs/msg/bounding_box.hpp>
-#include <geometry_msgs/msg/transform_stamped.hpp>
-#include <autoware_auto_geometry_msgs/msg/quaternion32.hpp>
-#include <geometry_msgs/msg/polygon.hpp>
-#include <geometry_msgs/msg/point32.hpp>
-#include <kdl/frames.hpp>
-#include <common/types.hpp>
-#include <string>
+#include <string>
using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
@@ -39,7 +41,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
namespace tf2
{
-
/*************/
/** Point32 **/
/*************/
@@ -50,9 +51,8 @@ namespace tf2
* \param t_out The transformed point, as a Point32 message.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
-template<>
-inline
-void doTransform(
+template <>
+inline void doTransform(
const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out,
const geometry_msgs::msg::TransformStamped & transform)
{
@@ -62,7 +62,6 @@ void doTransform(
t_out.z = static_cast<float>(v_out[2]);
}
-
/*************/
/** Polygon **/
/*************/
@@ -73,9 +72,8 @@ void doTransform(
* \param t_out The transformed polygon.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
-template<>
-inline
-void doTransform(
+template <>
+inline void doTransform(
const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out,
const geometry_msgs::msg::TransformStamped & transform)
{
@@ -84,8 +82,8 @@ void doTransform(
// We don't use std::back_inserter to allow aliasing between t_in and t_out
t_out.points.resize(t_in.points.size());
for (size_t i = 0; i < t_in.points.size(); ++i) {
- const KDL::Vector v_out = kdl_frame * KDL::Vector(
- t_in.points[i].x, t_in.points[i].y, t_in.points[i].z);
+ const KDL::Vector v_out =
+ kdl_frame * KDL::Vector(t_in.points[i].x, t_in.points[i].y, t_in.points[i].z);
t_out.points[i].x = static_cast<float>(v_out[0]);
t_out.points[i].y = static_cast<float>(v_out[1]);
t_out.points[i].z = static_cast<float>(v_out[2]);
@@ -102,9 +100,8 @@ void doTransform(
* \param t_out The transformed Quaternion32 message.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
-template<>
-inline
-void doTransform(
+template <>
+inline void doTransform(
const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in,
autoware_auto_geometry_msgs::msg::Quaternion32 & t_out,
const geometry_msgs::msg::TransformStamped & transform)
@@ -120,7 +117,6 @@ void doTransform(
t_out.w = static_cast<float32_t>(qw);
}
-
/******************/
/** BoundingBox **/
/******************/
@@ -131,9 +127,8 @@ void doTransform(
* \param t_out The transformed BoundingBox message.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
-template<>
-inline
-void doTransform(
+template <>
+inline void doTransform(
const BoundingBox & t_in, BoundingBox & t_out,
const geometry_msgs::msg::TransformStamped & transform)
{
@@ -147,7 +142,6 @@ void doTransform(
// TODO(jitrc): add conversion for other fields of BoundingBox, such as heading, variance, size
}
-
/**********************/
/** BoundingBoxArray **/
/**********************/
@@ -157,9 +151,8 @@ void doTransform(
* \param t A timestamped BoundingBoxArray message to extract the timestamp from.
* \return The timestamp of the message.
*/
-template<>
-inline
-tf2::TimePoint getTimestamp(const BoundingBoxArray & t)
+template <>
+inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t)
{
return tf2_ros::fromMsg(t.header.stamp);
}
@@ -169,9 +162,11 @@ tf2::TimePoint getTimestamp(const BoundingBoxArray & t)
* \param t A timestamped BoundingBoxArray message to extract the frame ID from.
* \return A string containing the frame ID of the message.
*/
-template<>
-inline
-std::string getFrameId(const BoundingBoxArray & t) {return t.header.frame_id;}
+template <>
+inline std::string getFrameId(const BoundingBoxArray & t)
+{
+ return t.header.frame_id;
+}
/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBoxArray type.
* This function is a specialization of the doTransform template defined in tf2/convert.h.
@@ -179,11 +174,9 @@ std::string getFrameId(const BoundingBoxArray & t) {return t.header.frame_id;}
* \param t_out The transformed BoundingBoxArray, as a timestamped BoundingBoxArray message.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
-template<>
-inline
-void doTransform(
- const BoundingBoxArray & t_in,
- BoundingBoxArray & t_out,
+template <>
+inline void doTransform(
+ const BoundingBoxArray & t_in, BoundingBoxArray & t_out,
const geometry_msgs::msg::TransformStamped & transform)
{
t_out = t_in;
diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml
index 27f07c37912d4..d4ac0a42ee20d 100644
--- a/common/autoware_auto_tf2/package.xml
+++ b/common/autoware_auto_tf2/package.xml
@@ -1,27 +1,29 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
- <name>autoware_auto_tf2</name>
- <version>1.0.0</version>
- <description>Transform related utilites for different msg types</description>
- <maintainer email="jit.ray.c@gmail.com">Jit Ray Chowdhury</maintainer>
- <license>Apache 2</license>
+ <name>autoware_auto_tf2</name>
+ <version>1.0.0</version>
+ <description>Transform related utilites for different msg types</description>
+ <maintainer email="jit.ray.c@gmail.com">Jit Ray Chowdhury</maintainer>
+ <license>Apache 2</license>
- <buildtool_depend>ament_cmake</buildtool_depend>
- <buildtool_depend>autoware_auto_cmake</buildtool_depend>
+ <buildtool_depend>ament_cmake</buildtool_depend>
+ <buildtool_depend>autoware_auto_cmake</buildtool_depend>
- <depend>autoware_auto_perception_msgs</depend>
- <depend>autoware_auto_geometry_msgs</depend>
- <depend>autoware_auto_system_msgs</depend>
- <depend>autoware_auto_common</depend>
- <depend>geometry_msgs</depend>
- <depend>tf2</depend>
- <depend>tf2_ros</depend>
- <depend>orocos_kdl</depend>
+ <depend>autoware_auto_common</depend>
+ <depend>autoware_auto_geometry_msgs</depend>
+ <depend>autoware_auto_perception_msgs</depend>
+ <depend>autoware_auto_system_msgs</depend>
+ <depend>geometry_msgs</depend>
+ <depend>orocos_kdl</depend>
+ <depend>tf2</depend>
+ <depend>tf2_ros</depend>
- <test_depend>ament_cmake_gtest</test_depend>
- <!-- <test_depend>ament_lint_auto</test_depend> -->
- <!-- <test_depend>ament_lint_common</test_depend> -->
+ <test_depend>ament_cmake_gtest</test_depend>
+ <!-- <test_depend>ament_lint_auto</test_depend> -->
+ <!-- <test_depend>ament_lint_common</test_depend> -->
- <export><build_type>ament_cmake</build_type></export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp
old mode 100755
new mode 100644
index bf512b4405d57..a318fa644539a
--- a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp
+++ b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp
@@ -14,13 +14,14 @@
/// \file
/// \brief This file includes common transoform functionaly for autoware_auto_msgs
+#include <autoware_auto_tf2/tf2_autoware_auto_msgs.hpp>
+#include <rclcpp/clock.hpp>
#include <gtest/gtest.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <autoware_auto_tf2/tf2_autoware_auto_msgs.hpp>
-#include <rclcpp/clock.hpp>
+
#include <memory>
std::unique_ptr<tf2_ros::Buffer> tf_buffer = nullptr;
@@ -43,7 +44,6 @@ geometry_msgs::msg::TransformStamped filled_transfom()
return t;
}
-
TEST(Tf2AutowareAuto, DoTransformPoint32)
{
const auto trans = filled_transfom();
@@ -61,7 +61,6 @@ TEST(Tf2AutowareAuto, DoTransformPoint32)
EXPECT_NEAR(p_out.z, 27, EPS);
}
-
TEST(Tf2AutowareAuto, DoTransformPolygon)
{
const auto trans = filled_transfom();
@@ -80,7 +79,6 @@ TEST(Tf2AutowareAuto, DoTransformPolygon)
EXPECT_NEAR(poly_out.points[0].z, 27, EPS);
}
-
TEST(Tf2AutowareAuto, DoTransformQuaternion32)
{
const auto trans = filled_transfom();
@@ -100,7 +98,6 @@ TEST(Tf2AutowareAuto, DoTransformQuaternion32)
EXPECT_NEAR(q_out.w, 0.0, EPS);
}
-
TEST(Tf2AutowareAuto, DoTransformBoundingBox)
{
const auto trans = filled_transfom();
@@ -212,7 +209,6 @@ TEST(Tf2AutowareAuto, TransformBoundingBoxArray)
// simple api
const auto bba_simple = tf_buffer->transform(bba1, "B", tf2::durationFromSec(2.0));
-
EXPECT_EQ(bba_simple.header.frame_id, "B");
// checking boxes[0]
@@ -257,11 +253,9 @@ TEST(Tf2AutowareAuto, TransformBoundingBoxArray)
EXPECT_NEAR(bba_simple.boxes[1].corners[3].y, -33, EPS);
EXPECT_NEAR(bba_simple.boxes[1].corners[3].z, -24, EPS);
-
// advanced api
- const auto bba_advanced = tf_buffer->transform(
- bba1, "B",
- tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0));
+ const auto bba_advanced =
+ tf_buffer->transform(bba1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0));
EXPECT_EQ(bba_advanced.header.frame_id, "B");
diff --git a/common/autoware_testing/autoware_testing/smoke_test.py b/common/autoware_testing/autoware_testing/smoke_test.py
index 89892e85dd824..d907a5002c30c 100644
--- a/common/autoware_testing/autoware_testing/smoke_test.py
+++ b/common/autoware_testing/autoware_testing/smoke_test.py
@@ -14,35 +14,34 @@
#
# Developed by Robotec.ai.
+import os
+import shlex
+import unittest
+
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
from launch.substitutions import LaunchConfiguration
-
from launch_ros.actions import Node
import launch_testing
-
-import os
import pytest
-import unittest
-import shlex
def resolve_node(context, *args, **kwargs):
smoke_test_node = Node(
- package=LaunchConfiguration('arg_package'),
- executable=LaunchConfiguration('arg_package_exe'),
- namespace='test',
+ package=LaunchConfiguration("arg_package"),
+ executable=LaunchConfiguration("arg_package_exe"),
+ namespace="test",
parameters=[
os.path.join(
- get_package_share_directory(LaunchConfiguration('arg_package').perform(context)),
+ get_package_share_directory(LaunchConfiguration("arg_package").perform(context)),
"param",
- LaunchConfiguration('arg_param_filename').perform(context)
+ LaunchConfiguration("arg_param_filename").perform(context),
)
],
- arguments=shlex.split(LaunchConfiguration('arg_executable_arguments').perform(context))
+ arguments=shlex.split(LaunchConfiguration("arg_executable_arguments").perform(context)),
)
return [smoke_test_node]
@@ -51,39 +50,32 @@ def resolve_node(context, *args, **kwargs):
def generate_test_description():
arg_package = DeclareLaunchArgument(
- 'arg_package',
- default_value=['default'],
- description='Package containing tested executable'
+ "arg_package", default_value=["default"], description="Package containing tested executable"
)
arg_package_exe = DeclareLaunchArgument(
- 'arg_package_exe',
- default_value=['default'],
- description='Tested executable'
+ "arg_package_exe", default_value=["default"], description="Tested executable"
)
arg_param_filename = DeclareLaunchArgument(
- 'arg_param_filename',
- default_value=['test.param.yaml'],
- description='Test param file'
+ "arg_param_filename", default_value=["test.param.yaml"], description="Test param file"
)
arg_executable_arguments = DeclareLaunchArgument(
- 'arg_executable_arguments',
- default_value=[''],
- description='Tested executable arguments'
+ "arg_executable_arguments", default_value=[""], description="Tested executable arguments"
)
- return LaunchDescription([
- arg_package,
- arg_package_exe,
- arg_param_filename,
- arg_executable_arguments,
- OpaqueFunction(function=resolve_node),
- launch_testing.actions.ReadyToTest()]
+ return LaunchDescription(
+ [
+ arg_package,
+ arg_package_exe,
+ arg_param_filename,
+ arg_executable_arguments,
+ OpaqueFunction(function=resolve_node),
+ launch_testing.actions.ReadyToTest(),
+ ]
)
@launch_testing.post_shutdown_test()
class TestProcessOutput(unittest.TestCase):
-
def test_exit_code(self, proc_output, proc_info):
# Check that process exits with code -15 code: termination request, sent to the program
launch_testing.asserts.assertExitCodes(proc_info, [-15])
diff --git a/common/autoware_testing/design/autoware_testing-design.md b/common/autoware_testing/design/autoware_testing-design.md
index fffbb3aa1341a..fea223ff6fa77 100644
--- a/common/autoware_testing/design/autoware_testing-design.md
+++ b/common/autoware_testing/design/autoware_testing-design.md
@@ -1,14 +1,14 @@
-autoware_testing {#autoware_testing-package-design}
-===========
+# autoware_testing {#autoware_testing-package-design}
This is the design document for the `autoware_testing` package.
-# Purpose / Use cases
+## Purpose / Use cases
The package aims to provide a unified way to add standard testing functionality to the package, currently supporting:
+
- Smoke testing (`add_smoke_test`): launch a node with default configuration and ensure that it starts up and does not crash.
-# Design
+## Design
Uses `ros_testing` (which is an extension of `launch_testing`) and provides some parametrized, reusable standard tests to run.
@@ -21,18 +21,18 @@ Parameters file for the package is expected to be in `param` directory inside pa
To add a smoke test to your package tests, add test dependency on `autoware_testing` to `package.xml`
-```{xml}
+```xml
<test_depend>autoware_testing</test_depend>
```
and add the following two lines to `CMakeLists.txt` in the `IF (BUILD_TESTING)` section:
-```{cmake}
+```cmake
find_package(autoware_testing REQUIRED)
add_smoke_test(<package_name> <executable_name> [PARAM_FILENAME <param_filename>] [EXECUTABLE_ARGUMENTS <arguments>])
```
-Where
+Where
`<package_name>` - [required] tested node package name.
@@ -46,20 +46,21 @@ which adds `<executable_name>_smoke_test` test to suite.
Example test result:
-```
+```text
build/<package_name>/test_results/<package_name>/<executable_name>_smoke_test.xunit.xml: 1 test, 0 errors, 0 failures, 0 skipped
-
```
-# References / External links
-- https://en.wikipedia.org/wiki/Smoke_testing_(software)
-- https://github.com/ros2/ros_testing
-- https://github.com/ros2/launch/blob/master/launch_testing
+## References / External links
-# Future extensions / Unimplemented parts
+- <https://en.wikipedia.org/wiki/Smoke_testing_(software>)
+- <https://github.com/ros2/ros_testing>
+- <https://github.com/ros2/launch/blob/master/launch_testing>
+
+## Future extensions / Unimplemented parts
- Adding more types of standard tests.
-# Related issues
+## Related issues
+
- Issue #700: add smoke test
- Issue #1224: Port other packages with smoke tests to use `autoware_testing`
diff --git a/common/autoware_testing/package.xml b/common/autoware_testing/package.xml
index d5d7720dbbea5..9ed51521a1db7 100644
--- a/common/autoware_testing/package.xml
+++ b/common/autoware_testing/package.xml
@@ -10,11 +10,11 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>ament_cmake_lint_cmake</buildtool_depend>
- <test_depend>ros_testing</test_depend>
+ <test_depend>ament_cmake_core</test_depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
- <test_depend>ament_cmake_core</test_depend>
+ <test_depend>ros_testing</test_depend>
<export>
<build_type>ament_cmake</build_type>
diff --git a/common/autoware_testing/setup.py b/common/autoware_testing/setup.py
index 16f2c1a6042a9..d4a5158dd0055 100644
--- a/common/autoware_testing/setup.py
+++ b/common/autoware_testing/setup.py
@@ -1,25 +1,23 @@
from setuptools import setup
-package_name = 'autoware_testing'
+package_name = "autoware_testing"
setup(
name=package_name,
- version='0.1.0',
+ version="0.1.0",
packages=[package_name],
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='Adam Dabrowski',
- maintainer_email='adam.dabrowski@robotec.ai',
- description='Tools for handling standard tests based on ros_testing',
- license='Apache 2.0',
- tests_require=['pytest'],
+ maintainer="Adam Dabrowski",
+ maintainer_email="adam.dabrowski@robotec.ai",
+ description="Tools for handling standard tests based on ros_testing",
+ license="Apache 2.0",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- ],
+ "console_scripts": [],
},
)
diff --git a/common/fake_test_node/design/fake_test_node-design.md b/common/fake_test_node/design/fake_test_node-design.md
index afd4d1e6b3ae5..ff83a2fd0f58f 100644
--- a/common/fake_test_node/design/fake_test_node-design.md
+++ b/common/fake_test_node/design/fake_test_node-design.md
@@ -1,7 +1,6 @@
-Fake Test Node {#fake-test-node-design}
-=====================
+# Fake Test Node {#fake-test-node-design}
-# What this package provides
+## What this package provides
When writing an integration test for a node in C++ using GTest, there is quite some boilerplate code
that needs to be written to set up a fake node that would publish expected messages on an expected
@@ -10,6 +9,7 @@ fixture.
This package contains a library that introduces two utility classes that can be used in place of
custom fixtures described above to write integration tests for a node:
+
- `autoware::tools::testing::FakeTestNode` - to use as a custom test fixture with `TEST_F` tests
- `autoware::tools::testing::FakeTestNodeParametrized` - to use a custom test fixture with the
parametrized `TEST_P` tests (accepts a template parameter that gets forwarded to
@@ -19,11 +19,13 @@ These fixtures take care of initializing and re-initializing rclcpp as well as o
subscribers and publishers have a match, thus reducing the amount of boilerplate code that the user
needs to write.
-# How to use this library
+## How to use this library
+
After including the relevant header the user can use a typedef to use a custom fixture name and use
the provided classes as fixtures in `TEST_F` and `TEST_P` tests directly.
-## Example usage
+### Example usage
+
Let's say there is a node `NodeUnderTest` that requires testing. It just
subscribes to `std_msgs::msg::Int32` messages and publishes a
`std_msgs::msg::Bool` to indicate that the input is positive. To test such a
diff --git a/common/fake_test_node/include/fake_test_node/fake_test_node.hpp b/common/fake_test_node/include/fake_test_node/fake_test_node.hpp
index e1346d0b55dd7..2ac836205e0a8 100644
--- a/common/fake_test_node/include/fake_test_node/fake_test_node.hpp
+++ b/common/fake_test_node/include/fake_test_node/fake_test_node.hpp
@@ -20,13 +20,11 @@
#ifndef FAKE_TEST_NODE__FAKE_TEST_NODE_HPP_
#define FAKE_TEST_NODE__FAKE_TEST_NODE_HPP_
-
#include <fake_test_node/visibility_control.hpp>
#include <rclcpp/rclcpp.hpp>
-#include <tf2_ros/transform_listener.h>
-
#include <gtest/gtest.h>
+#include <tf2_ros/transform_listener.h>
#include <memory>
#include <string>
@@ -44,7 +42,7 @@ namespace testing
///
/// @tparam T Type of the parameter as forwarded to testing::TestWithParam<T>.
///
-template<typename T>
+template <typename T>
class FAKE_TEST_NODE_PUBLIC FakeTestNodeParametrized;
///
@@ -98,7 +96,7 @@ class FAKE_TEST_NODE_PUBLIC FakeNodeCore
///
/// @return A publisher pointer;
///
- template<typename MsgT>
+ template <typename MsgT>
typename rclcpp::Publisher<MsgT>::SharedPtr create_publisher(
const std::string & topic,
const std::chrono::milliseconds & timeout = std::chrono::seconds{10LL},
@@ -114,8 +112,7 @@ class FAKE_TEST_NODE_PUBLIC FakeNodeCore
spent_time += dt;
if (spent_time > timeout) {
throw std::runtime_error(
- std::string(
- "No matching subscriber to the mock topic '") + topic + "' that we publish");
+ std::string("No matching subscriber to the mock topic '") + topic + "' that we publish");
}
std::this_thread::sleep_for(dt);
}
@@ -140,10 +137,9 @@ class FAKE_TEST_NODE_PUBLIC FakeNodeCore
///
/// @return Returns a subscription pointer.
///
- template<typename MsgT, typename NodeT>
+ template <typename MsgT, typename NodeT>
typename rclcpp::Subscription<MsgT>::SharedPtr create_subscription(
- const std::string & topic,
- const NodeT & publishing_node,
+ const std::string & topic, const NodeT & publishing_node,
std::function<void(const typename MsgT::SharedPtr msg)> callback,
const std::chrono::milliseconds & timeout = std::chrono::seconds{10LL},
const rclcpp::QoS & qos = rclcpp::QoS(rclcpp::KeepLast(10)))
@@ -155,10 +151,8 @@ class FAKE_TEST_NODE_PUBLIC FakeNodeCore
spent_time += dt;
if (spent_time > timeout) {
throw std::runtime_error(
- std::string(
- "The node under test '") + publishing_node.get_name() +
- "' is not publishing the topic '" + topic +
- "' that we listen to.");
+ std::string("The node under test '") + publishing_node.get_name() +
+ "' is not publishing the topic '" + topic + "' that we listen to.");
}
std::this_thread::sleep_for(dt);
}
@@ -170,14 +164,14 @@ class FAKE_TEST_NODE_PUBLIC FakeNodeCore
///
/// @return The mock node.
///
- inline rclcpp::Node::SharedPtr get_fake_node() {return m_fake_node;}
+ inline rclcpp::Node::SharedPtr get_fake_node() { return m_fake_node; }
///
/// @brief Gets the tf buffer.
///
/// @return The tf buffer.
///
- inline tf2::BufferCore & get_tf_buffer() {return m_tf_buffer;}
+ inline tf2::BufferCore & get_tf_buffer() { return m_tf_buffer; }
private:
std::shared_ptr<rclcpp::Node> m_fake_node{nullptr};
@@ -196,16 +190,15 @@ FAKE_TEST_NODE_PUBLIC std::string get_test_name(const ::testing::TestInfo * info
} // namespace detail
-
///
/// @brief This class describes a fake test node that inherits from the parametrized GTest
/// fixture.
///
/// @tparam T Type of parameter to be used in the fixture.
///
-template<typename T>
-class FAKE_TEST_NODE_PUBLIC FakeTestNodeParametrized
- : public detail::FakeNodeCore, public ::testing::TestWithParam<T>
+template <typename T>
+class FAKE_TEST_NODE_PUBLIC FakeTestNodeParametrized : public detail::FakeNodeCore,
+ public ::testing::TestWithParam<T>
{
using FakeNodeCore::FakeNodeCore;
@@ -221,15 +214,14 @@ class FAKE_TEST_NODE_PUBLIC FakeTestNodeParametrized
///
/// @brief Override the tear down function of the fixture.
///
- void TearDown() override {tear_down();}
+ void TearDown() override { tear_down(); }
};
///
/// @brief This class describes a fake test node that inherits from the parametrized GTest
/// fixture.
///
-class FAKE_TEST_NODE_PUBLIC FakeTestNode
- : public detail::FakeNodeCore, public ::testing::Test
+class FAKE_TEST_NODE_PUBLIC FakeTestNode : public detail::FakeNodeCore, public ::testing::Test
{
using FakeNodeCore::FakeNodeCore;
@@ -245,7 +237,6 @@ class FAKE_TEST_NODE_PUBLIC FakeTestNode
void TearDown() override;
};
-
} // namespace testing
} // namespace tools
} // namespace autoware
diff --git a/common/fake_test_node/include/fake_test_node/visibility_control.hpp b/common/fake_test_node/include/fake_test_node/visibility_control.hpp
index c8d60d0b17e90..ec62d082eb5dc 100644
--- a/common/fake_test_node/include/fake_test_node/visibility_control.hpp
+++ b/common/fake_test_node/include/fake_test_node/visibility_control.hpp
@@ -22,21 +22,21 @@
////////////////////////////////////////////////////////////////////////////////
#if defined(__WIN32)
- #if defined(FAKE_TEST_NODE_BUILDING_DLL) || defined(FAKE_TEST_NODE_EXPORTS)
- #define FAKE_TEST_NODE_PUBLIC __declspec(dllexport)
- #define FAKE_TEST_NODE_LOCAL
- #else // defined(FAKE_TEST_NODE_BUILDING_DLL) || defined(FAKE_TEST_NODE_EXPORTS)
- #define FAKE_TEST_NODE_PUBLIC __declspec(dllimport)
- #define FAKE_TEST_NODE_LOCAL
- #endif // defined(FAKE_TEST_NODE_BUILDING_DLL) || defined(FAKE_TEST_NODE_EXPORTS)
+#if defined(FAKE_TEST_NODE_BUILDING_DLL) || defined(FAKE_TEST_NODE_EXPORTS)
+#define FAKE_TEST_NODE_PUBLIC __declspec(dllexport)
+#define FAKE_TEST_NODE_LOCAL
+#else // defined(FAKE_TEST_NODE_BUILDING_DLL) || defined(FAKE_TEST_NODE_EXPORTS)
+#define FAKE_TEST_NODE_PUBLIC __declspec(dllimport)
+#define FAKE_TEST_NODE_LOCAL
+#endif // defined(FAKE_TEST_NODE_BUILDING_DLL) || defined(FAKE_TEST_NODE_EXPORTS)
#elif defined(__linux__)
- #define FAKE_TEST_NODE_PUBLIC __attribute__((visibility("default")))
- #define FAKE_TEST_NODE_LOCAL __attribute__((visibility("hidden")))
+#define FAKE_TEST_NODE_PUBLIC __attribute__((visibility("default")))
+#define FAKE_TEST_NODE_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
- #define FAKE_TEST_NODE_PUBLIC __attribute__((visibility("default")))
- #define FAKE_TEST_NODE_LOCAL __attribute__((visibility("hidden")))
+#define FAKE_TEST_NODE_PUBLIC __attribute__((visibility("default")))
+#define FAKE_TEST_NODE_LOCAL __attribute__((visibility("hidden")))
#else // defined(__linux__)
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif // defined(__WIN32)
#endif // FAKE_TEST_NODE__VISIBILITY_CONTROL_HPP_
diff --git a/common/fake_test_node/package.xml b/common/fake_test_node/package.xml
index 4ce6fc2ddc5c0..96b66a6ce1aa6 100644
--- a/common/fake_test_node/package.xml
+++ b/common/fake_test_node/package.xml
@@ -1,25 +1,27 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
- <name>fake_test_node</name>
- <version>1.0.0</version>
- <description>A fake node that we can use in the integration-like cpp tests.</description>
- <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
- <license>Apache 2.0</license>
+ <name>fake_test_node</name>
+ <version>1.0.0</version>
+ <description>A fake node that we can use in the integration-like cpp tests.</description>
+ <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
+ <license>Apache 2.0</license>
- <buildtool_depend>ament_cmake_auto</buildtool_depend>
- <buildtool_depend>autoware_auto_cmake</buildtool_depend>
+ <buildtool_depend>ament_cmake_auto</buildtool_depend>
+ <buildtool_depend>autoware_auto_cmake</buildtool_depend>
- <depend>autoware_auto_common</depend>
- <depend>rclcpp</depend>
- <depend>rclcpp_components</depend>
- <depend>ament_cmake_gtest</depend>
- <depend>tf2</depend>
- <depend>tf2_ros</depend>
+ <depend>ament_cmake_gtest</depend>
+ <depend>autoware_auto_common</depend>
+ <depend>rclcpp</depend>
+ <depend>rclcpp_components</depend>
+ <depend>tf2</depend>
+ <depend>tf2_ros</depend>
- <test_depend>ament_index_python</test_depend>
- <!-- <test_depend>ament_lint_auto</test_depend> -->
- <!-- <test_depend>ament_lint_common</test_depend> -->
+ <test_depend>ament_index_python</test_depend>
+ <!-- <test_depend>ament_lint_auto</test_depend> -->
+ <!-- <test_depend>ament_lint_common</test_depend> -->
- <export><build_type>ament_cmake</build_type></export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/common/fake_test_node/src/fake_test_node.cpp b/common/fake_test_node/src/fake_test_node.cpp
index dadfb89f6033e..582dcab245b07 100644
--- a/common/fake_test_node/src/fake_test_node.cpp
+++ b/common/fake_test_node/src/fake_test_node.cpp
@@ -17,7 +17,6 @@
/// \copyright Copyright 2021 Apex.AI, Inc.
/// All rights reserved.
-
#include <fake_test_node/fake_test_node.hpp>
#include <memory>
@@ -50,18 +49,17 @@ void detail::FakeNodeCore::set_up(const std::string & test_name)
rclcpp::init(kArgc, nullptr);
ASSERT_TRUE(rclcpp::ok());
m_fake_node = std::make_shared<rclcpp::Node>("FakeNodeForTest_" + sanitize_test_name(test_name));
- m_tf_listener = std::make_shared<tf2_ros::TransformListener>(
- m_tf_buffer, m_fake_node, kSpinThread);
+ m_tf_listener =
+ std::make_shared<tf2_ros::TransformListener>(m_tf_buffer, m_fake_node, kSpinThread);
}
-void detail::FakeNodeCore::tear_down()
-{
- (void)rclcpp::shutdown();
-}
+void detail::FakeNodeCore::tear_down() { (void)rclcpp::shutdown(); }
std::string detail::get_test_name(const ::testing::TestInfo * info)
{
- if (!info) {throw std::runtime_error("No test info available.");}
+ if (!info) {
+ throw std::runtime_error("No test info available.");
+ }
return std::string{info->test_case_name()} + "_" + info->name();
}
@@ -70,11 +68,7 @@ void FakeTestNode::SetUp()
set_up(detail::get_test_name(::testing::UnitTest::GetInstance()->current_test_info()));
}
-void FakeTestNode::TearDown()
-{
- tear_down();
-}
-
+void FakeTestNode::TearDown() { tear_down(); }
} // namespace testing
} // namespace tools
diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp
index fbacf0aa162eb..d7a00792acab8 100644
--- a/common/fake_test_node/test/test_fake_test_node.cpp
+++ b/common/fake_test_node/test/test_fake_test_node.cpp
@@ -19,6 +19,7 @@
#include <common/types.hpp>
#include <fake_test_node/fake_test_node.hpp>
+
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int32.hpp>
@@ -44,13 +45,11 @@ class NodeUnderTest : public rclcpp::Node
NodeUnderTest()
: rclcpp::Node{"is_positive_node"},
m_pub{this->create_publisher<Bool>("/output_topic", 10)},
- m_sub{this->create_subscription<Int32>(
- "/input_topic", 10,
- [&](const Int32::SharedPtr msg) {
- Bool output;
- output.data = msg->data > 0;
- m_pub->publish(output);
- })}
+ m_sub{this->create_subscription<Int32>("/input_topic", 10, [&](const Int32::SharedPtr msg) {
+ Bool output;
+ output.data = msg->data > 0;
+ m_pub->publish(output);
+ })}
{
}
@@ -59,7 +58,7 @@ class NodeUnderTest : public rclcpp::Node
rclcpp::Subscription<Int32>::SharedPtr m_sub{};
};
-template<typename FixtureT>
+template <typename FixtureT>
void run_test(int32_t value_in_message, FixtureT * fixture)
{
Int32 msg{};
@@ -70,7 +69,7 @@ void run_test(int32_t value_in_message, FixtureT * fixture)
auto fake_odom_publisher = fixture->template create_publisher<Int32>("/input_topic");
auto result_odom_subscription = fixture->template create_subscription<Bool>(
"/output_topic", *node,
- [&last_received_msg](const Bool::SharedPtr msg) {last_received_msg = msg;});
+ [&last_received_msg](const Bool::SharedPtr msg) { last_received_msg = msg; });
const auto dt{std::chrono::milliseconds{100LL}};
const auto max_wait_time{std::chrono::seconds{10LL}};
@@ -89,21 +88,15 @@ void run_test(int32_t value_in_message, FixtureT * fixture)
SUCCEED();
}
-
} // namespace
/// @test Test that we can use a non-parametrized test.
-TEST_F(FakeNodeFixture, Test) {
- run_test(15, this);
-}
+TEST_F(FakeNodeFixture, Test) { run_test(15, this); }
INSTANTIATE_TEST_SUITE_P(
- FakeNodeFixtureTests,
- FakeNodeFixtureParametrized,
+ FakeNodeFixtureTests, FakeNodeFixtureParametrized,
// cppcheck-suppress syntaxError // cppcheck doesn't like the trailing comma.
::testing::Values(-5, 0, 42));
/// @test Test that we can use a parametrized test.
-TEST_P(FakeNodeFixtureParametrized, Test) {
- run_test(GetParam(), this);
-}
+TEST_P(FakeNodeFixtureParametrized, Test) { run_test(GetParam(), this); }
diff --git a/common/had_map_utils/include/had_map_utils/had_map_computation.hpp b/common/had_map_utils/include/had_map_utils/had_map_computation.hpp
index 35f3f3dcf31d2..9c5f683984e12 100644
--- a/common/had_map_utils/include/had_map_utils/had_map_computation.hpp
+++ b/common/had_map_utils/include/had_map_utils/had_map_computation.hpp
@@ -15,10 +15,11 @@
#ifndef HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_
#define HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_
+#include "visibility_control.hpp"
+
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
-#include <lanelet2_core/LaneletMap.h>
-#include "visibility_control.hpp"
+#include <lanelet2_core/LaneletMap.h>
namespace autoware
{
diff --git a/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp b/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp
index e0c5ca8c8c9a9..080773ace6f8d 100644
--- a/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp
+++ b/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp
@@ -15,10 +15,13 @@
#ifndef HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_
#define HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_
+#include "had_map_utils/visibility_control.hpp"
+
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+
#include <lanelet2_core/LaneletMap.h>
+
#include <memory>
-#include "had_map_utils/visibility_control.hpp"
namespace autoware
{
diff --git a/common/had_map_utils/include/had_map_utils/had_map_query.hpp b/common/had_map_utils/include/had_map_utils/had_map_query.hpp
index de51b11f5e1d2..9a6654605d5e6 100644
--- a/common/had_map_utils/include/had_map_utils/had_map_query.hpp
+++ b/common/had_map_utils/include/had_map_utils/had_map_query.hpp
@@ -15,16 +15,16 @@
#ifndef HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_
#define HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_
+#include "had_map_utils/visibility_control.hpp"
#include <lanelet2_core/LaneletMap.h>
-#include <lanelet2_core/primitives/Point.h>
#include <lanelet2_core/primitives/LineString.h>
+#include <lanelet2_core/primitives/Point.h>
#include <lanelet2_core/primitives/Polygon.h>
#include <lanelet2_core/utility/Units.h>
#include <cmath>
#include <memory>
-#include "had_map_utils/visibility_control.hpp"
namespace autoware
{
@@ -35,24 +35,23 @@ namespace had_map_utils
lanelet::Areas HAD_MAP_UTILS_PUBLIC getAreaLayer(const lanelet::LaneletMapPtr ll_map);
-lanelet::Areas HAD_MAP_UTILS_PUBLIC subtypeAreas(
- const lanelet::Areas areas, const char subtype[]);
+lanelet::Areas HAD_MAP_UTILS_PUBLIC subtypeAreas(const lanelet::Areas areas, const char subtype[]);
lanelet::Polygons3d HAD_MAP_UTILS_PUBLIC getPolygonLayer(const lanelet::LaneletMapPtr ll_map);
-lanelet::Polygons3d HAD_MAP_UTILS_PUBLIC subtypePolygons(
- const lanelet::Polygons3d polygons, const char subtype[]);
+lanelet::Polygons3d HAD_MAP_UTILS_PUBLIC
+subtypePolygons(const lanelet::Polygons3d polygons, const char subtype[]);
lanelet::LineStrings3d HAD_MAP_UTILS_PUBLIC getLineStringLayer(const lanelet::LaneletMapPtr ll_map);
-lanelet::LineStrings3d HAD_MAP_UTILS_PUBLIC subtypeLineStrings(
- const lanelet::LineStrings3d linestrings, const char subtype[]);
+lanelet::LineStrings3d HAD_MAP_UTILS_PUBLIC
+subtypeLineStrings(const lanelet::LineStrings3d linestrings, const char subtype[]);
-lanelet::ConstLanelets HAD_MAP_UTILS_PUBLIC getConstLaneletLayer(
- const std::shared_ptr<lanelet::LaneletMap> & ll_map);
+lanelet::ConstLanelets HAD_MAP_UTILS_PUBLIC
+getConstLaneletLayer(const std::shared_ptr<lanelet::LaneletMap> & ll_map);
-lanelet::Lanelets HAD_MAP_UTILS_PUBLIC getLaneletLayer(
- const std::shared_ptr<lanelet::LaneletMap> & ll_map);
+lanelet::Lanelets HAD_MAP_UTILS_PUBLIC
+getLaneletLayer(const std::shared_ptr<lanelet::LaneletMap> & ll_map);
} // namespace had_map_utils
} // namespace common
diff --git a/common/had_map_utils/include/had_map_utils/had_map_utils.hpp b/common/had_map_utils/include/had_map_utils/had_map_utils.hpp
index b5b06016bdcc9..3d5549b72ad35 100644
--- a/common/had_map_utils/include/had_map_utils/had_map_utils.hpp
+++ b/common/had_map_utils/include/had_map_utils/had_map_utils.hpp
@@ -15,14 +15,15 @@
#ifndef HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_
#define HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_
+#include "had_map_utils/visibility_control.hpp"
+
+#include <common/types.hpp>
#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/primitives/Point.h>
#include <lanelet2_core/utility/Units.h>
-#include <common/types.hpp>
#include <cmath>
-#include "had_map_utils/visibility_control.hpp"
namespace autoware
{
@@ -35,8 +36,8 @@ using autoware::common::types::float64_t;
void HAD_MAP_UTILS_PUBLIC overwriteLaneletsCenterline(
lanelet::LaneletMapPtr lanelet_map, const autoware::common::types::bool8_t force_overwrite);
-lanelet::LineString3d HAD_MAP_UTILS_PUBLIC generateFineCenterline(
- const lanelet::ConstLanelet & lanelet_obj, const float64_t resolution);
+lanelet::LineString3d HAD_MAP_UTILS_PUBLIC
+generateFineCenterline(const lanelet::ConstLanelet & lanelet_obj, const float64_t resolution);
} // namespace had_map_utils
} // namespace common
diff --git a/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp b/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp
index 2952724fde4ff..f1113778de64f 100644
--- a/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp
+++ b/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp
@@ -15,28 +15,29 @@
#ifndef HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_
#define HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_
-#include <visualization_msgs/msg/marker.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
+#include "had_map_utils/visibility_control.hpp"
+
+#include <common/types.hpp>
+#include <rclcpp/rclcpp.hpp>
+
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/point32.hpp>
#include <geometry_msgs/msg/polygon.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <common/types.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
#include <lanelet2_core/LaneletMap.h>
#include <chrono>
-#include <unordered_set>
+#include <cmath>
#include <memory>
#include <string>
-#include <cmath>
+#include <unordered_set>
#include <vector>
-#include "had_map_utils/visibility_control.hpp"
-
+using autoware::common::types::bool8_t;
using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
-using autoware::common::types::bool8_t;
namespace autoware
{
@@ -53,8 +54,8 @@ namespace had_map_utils
* \param a alpha value
*/
void HAD_MAP_UTILS_PUBLIC setColor(
- std_msgs::msg::ColorRGBA * cl,
- const float32_t & r, const float32_t & g, const float32_t & b, const float32_t & a);
+ std_msgs::msg::ColorRGBA * cl, const float32_t & r, const float32_t & g, const float32_t & b,
+ const float32_t & a);
/**
* \brief Set the header information to a marker object
@@ -70,11 +71,9 @@ void HAD_MAP_UTILS_PUBLIC setColor(
* \return visualization_msgs::msg::Marker
*/
void HAD_MAP_UTILS_PUBLIC setMarkerHeader(
- visualization_msgs::msg::Marker * m,
- const int32_t & id, const rclcpp::Time & t,
- const std::string & frame_id, const std::string & ns,
- const std_msgs::msg::ColorRGBA & c, const int32_t & action, const int32_t & type,
- const float32_t & scale);
+ visualization_msgs::msg::Marker * m, const int32_t & id, const rclcpp::Time & t,
+ const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c,
+ const int32_t & action, const int32_t & type, const float32_t & scale);
/**
* \brief creates marker with type LINE_STRIP from a lanelet::LineString3d object
@@ -87,10 +86,8 @@ void HAD_MAP_UTILS_PUBLIC setMarkerHeader(
* \return created visualization_msgs::msg::Marker
*/
visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker(
- const rclcpp::Time & t,
- const lanelet::LineString3d & ls,
- const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c,
- const float32_t & lss);
+ const rclcpp::Time & t, const lanelet::LineString3d & ls, const std::string & frame_id,
+ const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss);
/**
* \brief creates marker with type LINE_STRIP from a lanelet::ConstLineString3d object
@@ -103,10 +100,8 @@ visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker(
* \return created visualization_msgs::msg::Marker
*/
visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker(
- const rclcpp::Time & t,
- const lanelet::ConstLineString3d & ls,
- const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c,
- const float32_t & lss);
+ const rclcpp::Time & t, const lanelet::ConstLineString3d & ls, const std::string & frame_id,
+ const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss);
/**
* \brief converts lanelet::LineString into markers with type LINE_STRIP
@@ -116,11 +111,8 @@ visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker(
* \param c color of the marker
* \return created visualization_msgs::msg::MarkerArray
*/
-visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC
-lineStringsAsMarkerArray(
- const rclcpp::Time & t,
- const std::string & ns,
- const lanelet::LineStrings3d & linestrings,
+visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC lineStringsAsMarkerArray(
+ const rclcpp::Time & t, const std::string & ns, const lanelet::LineStrings3d & linestrings,
const std_msgs::msg::ColorRGBA & c);
/**
@@ -132,10 +124,8 @@ lineStringsAsMarkerArray(
* \return created visualization_msgs::msg::MarkerArray
*/
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsBoundaryAsMarkerArray(
- const rclcpp::Time & t,
- const lanelet::ConstLanelets & lanelets,
- const std_msgs::msg::ColorRGBA & c,
- const bool8_t & viz_centerline);
+ const rclcpp::Time & t, const lanelet::ConstLanelets & lanelets,
+ const std_msgs::msg::ColorRGBA & c, const bool8_t & viz_centerline);
/**
* \brief creates marker with type LINE_STRIP from a lanelet::BasicPolygon object
@@ -149,10 +139,9 @@ visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsBoundaryAsMark
* \return created visualization_msgs::msg::Marker
*/
visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC basicPolygon2Marker(
- const rclcpp::Time & t,
- const int32_t & line_id, const lanelet::BasicPolygon3d & pg,
- const std::string & frame_id, const std::string & ns,
- const std_msgs::msg::ColorRGBA & c, const float32_t & lss);
+ const rclcpp::Time & t, const int32_t & line_id, const lanelet::BasicPolygon3d & pg,
+ const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c,
+ const float32_t & lss);
/**
* \brief converts outer bound of lanelet::Area into markers with type LINE_STRIP
@@ -163,8 +152,7 @@ visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC basicPolygon2Marker(
* \return created visualization_msgs::msg::MarkerArray
*/
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasBoundaryAsMarkerArray(
- const rclcpp::Time & t,
- const std::string & ns, const lanelet::Areas & areas,
+ const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas,
const std_msgs::msg::ColorRGBA & c);
/**
@@ -176,8 +164,7 @@ visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasBoundaryAsMarkerA
* \return created visualization_msgs::msg::MarkerArray
*/
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC polygonsBoundaryAsMarkerArray(
- const rclcpp::Time & t,
- const std::string & ns, const lanelet::Polygons3d & polygons,
+ const rclcpp::Time & t, const std::string & ns, const lanelet::Polygons3d & polygons,
const std_msgs::msg::ColorRGBA & c);
/**
@@ -193,10 +180,9 @@ visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC polygonsBoundaryAsMark
* \return created visualization_msgs::msg::Marker
*/
visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC bbox2Marker(
- const rclcpp::Time & t, const int32_t & line_id,
- const float64_t lower[], const float64_t upper[],
- const std::string & frame_id, const std::string & ns,
- const std_msgs::msg::ColorRGBA & c, const float32_t & lss);
+ const rclcpp::Time & t, const int32_t & line_id, const float64_t lower[], const float64_t upper[],
+ const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c,
+ const float32_t & lss);
/**
* \brief creates marker array from bounding box
@@ -208,8 +194,7 @@ visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC bbox2Marker(
* \return created visualization_msgs::msg::MarkerArray
*/
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC boundingBoxAsMarkerArray(
- const rclcpp::Time & t,
- const std::string & ns, const float64_t upper[], const float64_t lower[],
+ const rclcpp::Time & t, const std::string & ns, const float64_t upper[], const float64_t lower[],
const std_msgs::msg::ColorRGBA & c);
/**
@@ -217,32 +202,30 @@ visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC boundingBoxAsMarkerArr
* \param ll input lanelet
* \return result of triangulation
*/
-std::vector<geometry_msgs::msg::Polygon> HAD_MAP_UTILS_PUBLIC lanelet2Triangle(
- const lanelet::ConstLanelet & ll);
+std::vector<geometry_msgs::msg::Polygon> HAD_MAP_UTILS_PUBLIC
+lanelet2Triangle(const lanelet::ConstLanelet & ll);
/**
* \brief converts area enclosed by geometry_msg::msg::Polygon into list of triangles.
* \param polygon input polygon
* \return result of triangulation
*/
-std::vector<geometry_msgs::msg::Polygon> HAD_MAP_UTILS_PUBLIC polygon2Triangle(
- const geometry_msgs::msg::Polygon & polygon);
+std::vector<geometry_msgs::msg::Polygon> HAD_MAP_UTILS_PUBLIC
+polygon2Triangle(const geometry_msgs::msg::Polygon & polygon);
/**
* \brief converts lanelet::Area into geometry_msgs::msg::Polygon type
* \param area input area
* \return converted geometry_msgs::msg::Polygon
*/
-geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC area2Polygon(
- const lanelet::ConstArea & area);
+geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC area2Polygon(const lanelet::ConstArea & area);
/**
* \brief converts lanelet::Lanelet into geometry_msgs::msg::Polygon type
* \param ll input lanelet
* \return converted geometry_msgs::msg::Polygon
*/
-geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC lanelet2Polygon(
- const lanelet::ConstLanelet & ll);
+geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC lanelet2Polygon(const lanelet::ConstLanelet & ll);
/**
* \brief converts bounded area by lanelet::Lanelet into triangle markers
diff --git a/common/had_map_utils/include/had_map_utils/visibility_control.hpp b/common/had_map_utils/include/had_map_utils/visibility_control.hpp
index f53a7dce48093..978163fdc85ad 100644
--- a/common/had_map_utils/include/had_map_utils/visibility_control.hpp
+++ b/common/had_map_utils/include/had_map_utils/visibility_control.hpp
@@ -18,21 +18,21 @@
#define HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_
#if defined(_MSC_VER) && defined(_WIN64)
- #if defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS)
- #define HAD_MAP_UTILS_PUBLIC __declspec(dllexport)
- #define HAD_MAP_UTILS_LOCAL
- #else // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS)
- #define HAD_MAP_UTILS_PUBLIC __declspec(dllimport)
- #define HAD_MAP_UTILS_LOCAL
- #endif // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS)
+#if defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS)
+#define HAD_MAP_UTILS_PUBLIC __declspec(dllexport)
+#define HAD_MAP_UTILS_LOCAL
+#else // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS)
+#define HAD_MAP_UTILS_PUBLIC __declspec(dllimport)
+#define HAD_MAP_UTILS_LOCAL
+#endif // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS)
#elif defined(__GNUC__) && defined(__linux__)
- #define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default")))
- #define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden")))
+#define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default")))
+#define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden")))
#elif defined(__GNUC__) && defined(__APPLE__)
- #define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default")))
- #define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden")))
+#define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default")))
+#define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden")))
#else // !(defined(__GNUC__) && defined(__APPLE__))
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif // _MSC_VER
#endif // HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_
diff --git a/common/had_map_utils/package.xml b/common/had_map_utils/package.xml
index 74d03b4f1e186..50e8c8ad6ad9a 100644
--- a/common/had_map_utils/package.xml
+++ b/common/had_map_utils/package.xml
@@ -1,33 +1,32 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
- <name>had_map_utils</name>
- <version>1.0.0</version>
- <description>Common utility functions and classes for HAD maps</description>
- <maintainer email="simon.thompson@tier4.jp">TierIV, Inc.</maintainer>
- <license>Apache 2.0</license>
+ <name>had_map_utils</name>
+ <version>1.0.0</version>
+ <description>Common utility functions and classes for HAD maps</description>
+ <maintainer email="simon.thompson@tier4.jp">TierIV, Inc.</maintainer>
+ <license>Apache 2.0</license>
- <buildtool_depend>ament_cmake_auto</buildtool_depend>
- <buildtool_depend>autoware_auto_cmake</buildtool_depend>
- <build_depend>rclcpp</build_depend>
- <build_depend>sensor_msgs</build_depend>
+ <buildtool_depend>ament_cmake_auto</buildtool_depend>
+ <buildtool_depend>autoware_auto_cmake</buildtool_depend>
+ <build_depend>rclcpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
- <depend>cgal</depend>
- <depend>lanelet2_core</depend>
- <depend>lanelet2_io</depend>
- <depend>lanelet2</depend>
-
- <depend>autoware_auto_mapping_msgs</depend>
- <depend>autoware_auto_planning_msgs</depend>
- <depend>autoware_auto_common</depend>
- <depend>visualization_msgs</depend>
- <depend>geometry_msgs</depend>
+ <depend>autoware_auto_common</depend>
+ <depend>autoware_auto_mapping_msgs</depend>
+ <depend>autoware_auto_planning_msgs</depend>
+ <depend>cgal</depend>
+ <depend>geometry_msgs</depend>
+ <depend>lanelet2</depend>
+ <depend>lanelet2_core</depend>
+ <depend>lanelet2_io</depend>
+ <depend>visualization_msgs</depend>
- <test_depend>ament_cmake_gtest</test_depend>
- <test_depend>ament_lint_auto</test_depend>
- <test_depend>ament_lint_common</test_depend>
+ <test_depend>ament_cmake_gtest</test_depend>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
- <export>
- <build_type>ament_cmake</build_type>
- </export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/common/had_map_utils/src/had_map_computation.cpp b/common/had_map_utils/src/had_map_computation.cpp
index 0a8bd8186d3fd..fc1471a1a3557 100644
--- a/common/had_map_utils/src/had_map_computation.cpp
+++ b/common/had_map_utils/src/had_map_computation.cpp
@@ -12,14 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <CGAL/Boolean_set_operations_2.h>
+#include "had_map_utils/had_map_computation.hpp"
+
+#include "had_map_utils/had_map_visualization.hpp"
#include <geometry_msgs/msg/polygon.hpp>
-#include <vector>
+#include <CGAL/Boolean_set_operations_2.h>
-#include "had_map_utils/had_map_computation.hpp"
-#include "had_map_utils/had_map_visualization.hpp"
+#include <vector>
namespace autoware
{
@@ -55,9 +56,8 @@ lanelet::Polygon3d coalesce_drivable_areas(
} else {
// This might happen if a primitive is on the route, but outside of the bounding box that we
// query the map for. Not sure how to deal with this at this point though.
- std::cerr << "Error: primitive ID " << map_segment.preferred_primitive_id <<
- " not found, skipping" <<
- std::endl;
+ std::cerr << "Error: primitive ID " << map_segment.preferred_primitive_id
+ << " not found, skipping" << std::endl;
continue;
}
}
@@ -67,18 +67,16 @@ lanelet::Polygon3d coalesce_drivable_areas(
CGAL_Polygon to_join{};
CGAL_Polygon_with_holes temporary_union;
const auto first_point = current_area_polygon.points.begin();
- for (auto area_point_it =
- current_area_polygon.points.begin();
- // Stop if we run out of points, or if we encounter the first point again
- area_point_it < current_area_polygon.points.end() &&
- !(first_point != area_point_it && first_point->x == area_point_it->x &&
- first_point->y == area_point_it->y);
- area_point_it++)
- {
+ for (auto area_point_it = current_area_polygon.points.begin();
+ // Stop if we run out of points, or if we encounter the first point again
+ area_point_it < current_area_polygon.points.end() &&
+ !(first_point != area_point_it && first_point->x == area_point_it->x &&
+ first_point->y == area_point_it->y);
+ area_point_it++) {
to_join.push_back(CGAL_Point(area_point_it->x, area_point_it->y));
}
- if (to_join.is_clockwise_oriented() ) {
+ if (to_join.is_clockwise_oriented()) {
to_join.reverse_orientation();
}
@@ -97,17 +95,15 @@ lanelet::Polygon3d coalesce_drivable_areas(
// Otherwise, just set the current drivable area equal to the area to add to it, because
// CGAL seems to do "union(empty, non-empty) = empty" for some reason.
const auto first_point = current_area_polygon.points.begin();
- for (auto area_point_it =
- current_area_polygon.points.begin();
- area_point_it < current_area_polygon.points.end() &&
- // Stop if we run out of points, or if we encounter the first point again
- !(first_point != area_point_it && first_point->x == area_point_it->x &&
- first_point->y == area_point_it->y);
- area_point_it++)
- {
+ for (auto area_point_it = current_area_polygon.points.begin();
+ area_point_it < current_area_polygon.points.end() &&
+ // Stop if we run out of points, or if we encounter the first point again
+ !(first_point != area_point_it && first_point->x == area_point_it->x &&
+ first_point->y == area_point_it->y);
+ area_point_it++) {
drivable_area.outer_boundary().push_back(CGAL_Point(area_point_it->x, area_point_it->y));
}
- if (drivable_area.outer_boundary().is_clockwise_oriented() ) {
+ if (drivable_area.outer_boundary().is_clockwise_oriented()) {
drivable_area.outer_boundary().reverse_orientation();
}
}
@@ -118,18 +114,14 @@ lanelet::Polygon3d coalesce_drivable_areas(
std::vector<lanelet::Point3d> lanelet_drivable_area_points{};
lanelet_drivable_area_points.reserve(drivable_area.outer_boundary().size());
for (auto p = drivable_area.outer_boundary().vertices_begin();
- p != drivable_area.outer_boundary().vertices_end(); p++)
- {
- lanelet_drivable_area_points.emplace_back(
- lanelet::Point3d(
- lanelet::utils::getId(), CGAL::to_double(p->x()),
- CGAL::to_double(p->y()), 0.0));
+ p != drivable_area.outer_boundary().vertices_end(); p++) {
+ lanelet_drivable_area_points.emplace_back(lanelet::Point3d(
+ lanelet::utils::getId(), CGAL::to_double(p->x()), CGAL::to_double(p->y()), 0.0));
}
lanelet::Polygon3d lanelet_drivable_area(lanelet::utils::getId(), lanelet_drivable_area_points);
return lanelet_drivable_area;
}
-
} // namespace had_map_utils
} // namespace common
} // namespace autoware
diff --git a/common/had_map_utils/src/had_map_conversion.cpp b/common/had_map_utils/src/had_map_conversion.cpp
index c7b2bc711270a..72fd5f017252c 100644
--- a/common/had_map_utils/src/had_map_conversion.cpp
+++ b/common/had_map_utils/src/had_map_conversion.cpp
@@ -14,22 +14,21 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-//lint -e537 pclint vs cpplint NOLINT
+// lint -e537 pclint vs cpplint NOLINT
-#include <lanelet2_io/io_handlers/Serialize.h>
+#include "had_map_utils/had_map_conversion.hpp"
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
+#include <lanelet2_io/io_handlers/Serialize.h>
+
#include <algorithm>
#include <limits>
#include <memory>
#include <string>
#include <vector>
-#include "had_map_utils/had_map_conversion.hpp"
-
-
namespace autoware
{
namespace common
diff --git a/common/had_map_utils/src/had_map_query.cpp b/common/had_map_utils/src/had_map_query.cpp
index eeca6543b06bc..667d404b3dbed 100644
--- a/common/had_map_utils/src/had_map_query.cpp
+++ b/common/had_map_utils/src/had_map_query.cpp
@@ -14,7 +14,9 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-//lint -e537 pclint vs cpplint NOLINT
+// lint -e537 pclint vs cpplint NOLINT
+
+#include "had_map_utils/had_map_query.hpp"
#include <algorithm>
#include <limits>
@@ -22,8 +24,6 @@
#include <string>
#include <vector>
-#include "had_map_utils/had_map_query.hpp"
-
namespace autoware
{
namespace common
@@ -79,21 +79,17 @@ lanelet::Polygons3d subtypePolygons(const lanelet::Polygons3d polygons, const ch
return subtype_polygons;
}
-
lanelet::LineStrings3d getLineStringLayer(const lanelet::LaneletMapPtr ll_map)
{
lanelet::LineStrings3d linestrings;
- for (auto lsi = ll_map->lineStringLayer.begin();
- lsi != ll_map->lineStringLayer.end(); lsi++)
- {
+ for (auto lsi = ll_map->lineStringLayer.begin(); lsi != ll_map->lineStringLayer.end(); lsi++) {
linestrings.push_back(*lsi);
}
return linestrings;
}
lanelet::LineStrings3d subtypeLineStrings(
- const lanelet::LineStrings3d linestrings,
- const char subtype[])
+ const lanelet::LineStrings3d linestrings, const char subtype[])
{
lanelet::LineStrings3d subtype_linestrings;
for (auto lsi = linestrings.begin(); lsi != linestrings.end(); lsi++) {
diff --git a/common/had_map_utils/src/had_map_utils.cpp b/common/had_map_utils/src/had_map_utils.cpp
index ff56754268864..e7a925e929be4 100644
--- a/common/had_map_utils/src/had_map_utils.cpp
+++ b/common/had_map_utils/src/had_map_utils.cpp
@@ -14,18 +14,19 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-//lint -e537 pclint vs cpplint NOLINT
-
+// lint -e537 pclint vs cpplint NOLINT
#include "had_map_utils/had_map_utils.hpp"
-#include <lanelet2_core/geometry/Lanelet.h>
#include <common/types.hpp>
-#include <utility>
+
+#include <lanelet2_core/geometry/Lanelet.h>
+
#include <algorithm>
#include <limits>
#include <memory>
#include <string>
+#include <utility>
#include <vector>
using autoware::common::types::float64_t;
@@ -83,15 +84,13 @@ std::pair<size_t, size_t> findNearestIndexPair(
for (size_t i = 1; i < N; ++i) {
if (
accumulated_lengths.at(i - 1) <= target_length &&
- target_length <= accumulated_lengths.at(i))
- {
+ target_length <= accumulated_lengths.at(i)) {
return std::make_pair(i - 1, i);
}
}
// Throw an exception because this never happens
- throw std::runtime_error(
- "findNearestIndexPair(): No nearest point found.");
+ throw std::runtime_error("findNearestIndexPair(): No nearest point found.");
}
std::vector<lanelet::BasicPoint3d> resamplePoints(
@@ -107,8 +106,8 @@ std::vector<lanelet::BasicPoint3d> resamplePoints(
std::vector<lanelet::BasicPoint3d> resampled_points;
for (auto i = 0; i <= num_segments; ++i) {
// Find two nearest points
- const float64_t target_length = (static_cast<float64_t>(i) / num_segments) *
- static_cast<float64_t>(line_length);
+ const float64_t target_length =
+ (static_cast<float64_t>(i) / num_segments) * static_cast<float64_t>(line_length);
const auto index_pair = findNearestIndexPair(accumulated_lengths, target_length);
// Apply linear interpolation
@@ -159,8 +158,7 @@ lanelet::LineString3d generateFineCenterline(
}
void overwriteLaneletsCenterline(
- lanelet::LaneletMapPtr lanelet_map,
- const autoware::common::types::bool8_t force_overwrite)
+ lanelet::LaneletMapPtr lanelet_map, const autoware::common::types::bool8_t force_overwrite)
{
for (auto & lanelet_obj : lanelet_map->laneletLayer) {
if (force_overwrite || !lanelet_obj.hasCustomCenterline()) {
diff --git a/common/had_map_utils/src/had_map_visualization.cpp b/common/had_map_utils/src/had_map_visualization.cpp
index 6133bc1eb22be..40120d953abc4 100644
--- a/common/had_map_utils/src/had_map_visualization.cpp
+++ b/common/had_map_utils/src/had_map_visualization.cpp
@@ -14,22 +14,23 @@
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
-//lint -e537 pclint vs cpplint NOLINT
+// lint -e537 pclint vs cpplint NOLINT
-#include <lanelet2_io/io_handlers/Serialize.h>
+#include "had_map_utils/had_map_visualization.hpp"
+
+#include <common/types.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
-#include <common/types.hpp>
+
+#include <lanelet2_io/io_handlers/Serialize.h>
#include <algorithm>
#include <limits>
#include <memory>
#include <string>
-#include <vector>
#include <unordered_set>
-
-#include "had_map_utils/had_map_visualization.hpp"
+#include <vector>
using autoware::common::types::float64_t;
@@ -40,15 +41,15 @@ namespace common
namespace had_map_utils
{
-template<typename T>
+template <typename T>
bool8_t exists(const std::unordered_set<T> & set, const T & element)
{
return std::find(set.begin(), set.end(), element) != set.end();
}
void setColor(
- std_msgs::msg::ColorRGBA * cl,
- const float32_t & r, const float32_t & g, const float32_t & b, const float32_t & a)
+ std_msgs::msg::ColorRGBA * cl, const float32_t & r, const float32_t & g, const float32_t & b,
+ const float32_t & a)
{
cl->r = r;
cl->g = g;
@@ -57,15 +58,9 @@ void setColor(
}
void setMarkerHeader(
- visualization_msgs::msg::Marker * m,
- const int32_t & id,
- const rclcpp::Time & t,
- const std::string & frame_id,
- const std::string & ns,
- const std_msgs::msg::ColorRGBA & c,
- const int32_t & action,
- const int32_t & type,
- const float32_t & scale)
+ visualization_msgs::msg::Marker * m, const int32_t & id, const rclcpp::Time & t,
+ const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c,
+ const int32_t & action, const int32_t & type, const float32_t & scale)
{
m->header.frame_id = frame_id;
m->header.stamp = t;
@@ -88,17 +83,13 @@ void setMarkerHeader(
}
visualization_msgs::msg::Marker lineString2Marker(
- const rclcpp::Time & t,
- const lanelet::LineString3d & ls,
- const std::string & frame_id,
+ const rclcpp::Time & t, const lanelet::LineString3d & ls, const std::string & frame_id,
const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss)
{
visualization_msgs::msg::Marker line_strip;
setMarkerHeader(
&line_strip, static_cast<int32_t>(ls.id()), t, frame_id, ns, c,
- visualization_msgs::msg::Marker::ADD,
- visualization_msgs::msg::Marker::LINE_STRIP,
- lss);
+ visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::LINE_STRIP, lss);
for (auto i = ls.begin(); i != ls.end(); i++) {
geometry_msgs::msg::Point p;
@@ -111,17 +102,13 @@ visualization_msgs::msg::Marker lineString2Marker(
}
visualization_msgs::msg::Marker lineString2Marker(
- const rclcpp::Time & t,
- const lanelet::ConstLineString3d & ls,
- const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c,
- const float32_t & lss)
+ const rclcpp::Time & t, const lanelet::ConstLineString3d & ls, const std::string & frame_id,
+ const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss)
{
visualization_msgs::msg::Marker line_strip;
setMarkerHeader(
&line_strip, static_cast<int32_t>(ls.id()), t, frame_id, ns, c,
- visualization_msgs::msg::Marker::ADD,
- visualization_msgs::msg::Marker::LINE_STRIP,
- lss);
+ visualization_msgs::msg::Marker::ADD, visualization_msgs::msg::Marker::LINE_STRIP, lss);
for (auto i = ls.begin(); i != ls.end(); i++) {
geometry_msgs::msg::Point p;
@@ -134,9 +121,7 @@ visualization_msgs::msg::Marker lineString2Marker(
}
visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray(
- const rclcpp::Time & t,
- const std::string & ns,
- const lanelet::LineStrings3d & linestrings,
+ const rclcpp::Time & t, const std::string & ns, const lanelet::LineStrings3d & linestrings,
const std_msgs::msg::ColorRGBA & c)
{
float32_t lss = 0.1f;
@@ -151,10 +136,8 @@ visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray(
}
visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray(
- const rclcpp::Time & t,
- const lanelet::ConstLanelets & lanelets,
- const std_msgs::msg::ColorRGBA & c,
- const bool8_t & viz_centerline)
+ const rclcpp::Time & t, const lanelet::ConstLanelets & lanelets,
+ const std_msgs::msg::ColorRGBA & c, const bool8_t & viz_centerline)
{
float32_t lss = 0.1f;
std::unordered_set<lanelet::Id> added;
@@ -173,15 +156,13 @@ visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray(
added.insert(left_ls.id());
}
if (!exists(added, right_ls.id())) {
- right_line_strip = lineString2Marker(
- t, right_ls, "map", "right_lane_bound", c, lss);
+ right_line_strip = lineString2Marker(t, right_ls, "map", "right_lane_bound", c, lss);
marker_array.markers.push_back(right_line_strip);
added.insert(right_ls.id());
}
if (viz_centerline && !exists(added, center_ls.id())) {
- center_line_strip = lineString2Marker(
- t, center_ls, "map", "center_lane_line",
- c, std::max(lss * 0.1f, 0.01f));
+ center_line_strip =
+ lineString2Marker(t, center_ls, "map", "center_lane_line", c, std::max(lss * 0.1f, 0.01f));
marker_array.markers.push_back(center_line_strip);
added.insert(center_ls.id());
}
@@ -190,20 +171,14 @@ visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray(
}
visualization_msgs::msg::Marker basicPolygon2Marker(
- const rclcpp::Time & t,
- const int32_t & line_id,
- const lanelet::BasicPolygon3d & pg,
- const std::string & frame_id,
- const std::string & ns,
- const std_msgs::msg::ColorRGBA & c,
+ const rclcpp::Time & t, const int32_t & line_id, const lanelet::BasicPolygon3d & pg,
+ const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c,
const float32_t & lss)
{
visualization_msgs::msg::Marker line_strip;
setMarkerHeader(
- &line_strip, line_id, t, frame_id, ns, c,
- visualization_msgs::msg::Marker::ADD,
- visualization_msgs::msg::Marker::LINE_STRIP,
- lss);
+ &line_strip, line_id, t, frame_id, ns, c, visualization_msgs::msg::Marker::ADD,
+ visualization_msgs::msg::Marker::LINE_STRIP, lss);
for (auto i = pg.begin(); i != pg.end(); i++) {
geometry_msgs::msg::Point p;
@@ -222,9 +197,7 @@ visualization_msgs::msg::Marker basicPolygon2Marker(
}
visualization_msgs::msg::MarkerArray areasBoundaryAsMarkerArray(
- const rclcpp::Time & t,
- const std::string & ns,
- const lanelet::Areas & areas,
+ const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas,
const std_msgs::msg::ColorRGBA & c)
{
int pg_count = 0;
@@ -234,9 +207,8 @@ visualization_msgs::msg::MarkerArray areasBoundaryAsMarkerArray(
lanelet::CompoundPolygon3d cpg = area.outerBoundPolygon();
lanelet::BasicPolygon3d bpg = cpg.basicPolygon();
- visualization_msgs::msg::Marker line_strip = basicPolygon2Marker(
- t, pg_count, bpg, "map", ns, c,
- lss);
+ visualization_msgs::msg::Marker line_strip =
+ basicPolygon2Marker(t, pg_count, bpg, "map", ns, c, lss);
marker_array.markers.push_back(line_strip);
pg_count++;
}
@@ -244,17 +216,16 @@ visualization_msgs::msg::MarkerArray areasBoundaryAsMarkerArray(
}
visualization_msgs::msg::MarkerArray polygonsBoundaryAsMarkerArray(
- const rclcpp::Time & t, const std::string & ns,
- const lanelet::Polygons3d & polygons, const std_msgs::msg::ColorRGBA & c)
+ const rclcpp::Time & t, const std::string & ns, const lanelet::Polygons3d & polygons,
+ const std_msgs::msg::ColorRGBA & c)
{
int32_t pg_count = 0;
float32_t lss = 0.1f;
visualization_msgs::msg::MarkerArray marker_array;
for (auto poly : polygons) {
lanelet::BasicPolygon3d bpg = poly.basicPolygon();
- visualization_msgs::msg::Marker line_strip = basicPolygon2Marker(
- t, pg_count, bpg, "map", ns, c,
- lss);
+ visualization_msgs::msg::Marker line_strip =
+ basicPolygon2Marker(t, pg_count, bpg, "map", ns, c, lss);
marker_array.markers.push_back(line_strip);
pg_count++;
}
@@ -263,21 +234,27 @@ visualization_msgs::msg::MarkerArray polygonsBoundaryAsMarkerArray(
visualization_msgs::msg::Marker bbox2Marker(
const rclcpp::Time & t, const int32_t & line_id, const float64_t lower[], const float64_t upper[],
- const std::string & frame_id, const std::string & ns,
- const std_msgs::msg::ColorRGBA & c, const float32_t & lss)
+ const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c,
+ const float32_t & lss)
{
visualization_msgs::msg::Marker line_strip;
setMarkerHeader(
- &line_strip, line_id, t, frame_id, ns, c,
- visualization_msgs::msg::Marker::ADD,
- visualization_msgs::msg::Marker::LINE_STRIP,
- lss);
+ &line_strip, line_id, t, frame_id, ns, c, visualization_msgs::msg::Marker::ADD,
+ visualization_msgs::msg::Marker::LINE_STRIP, lss);
geometry_msgs::msg::Point bl, br, tl, tr;
- bl.x = lower[0]; bl.y = lower[0]; bl.z = 0.0;
- br.x = upper[0]; br.y = lower[0]; br.z = 0.0;
- tl.x = lower[0]; tl.y = upper[0]; tl.z = 0.0;
- tr.x = upper[0]; tr.y = upper[0]; tr.z = 0.0;
+ bl.x = lower[0];
+ bl.y = lower[0];
+ bl.z = 0.0;
+ br.x = upper[0];
+ br.y = lower[0];
+ br.z = 0.0;
+ tl.x = lower[0];
+ tl.y = upper[0];
+ tl.z = 0.0;
+ tr.x = upper[0];
+ tr.y = upper[0];
+ tr.z = 0.0;
line_strip.points.push_back(bl);
line_strip.points.push_back(br);
@@ -288,8 +265,7 @@ visualization_msgs::msg::Marker bbox2Marker(
}
visualization_msgs::msg::MarkerArray boundingBoxAsMarkerArray(
- const rclcpp::Time & t,
- const std::string & ns, const float64_t upper[], const float64_t lower[],
+ const rclcpp::Time & t, const std::string & ns, const float64_t upper[], const float64_t lower[],
const std_msgs::msg::ColorRGBA & c)
{
float32_t lss = 0.2f;
@@ -317,10 +293,8 @@ geometry_msgs::msg::Point32 toGeomMsgPt32(const lanelet::BasicPoint3d & src)
return dst;
}
void adjacentPoints(
- const size_t i, const size_t N,
- const geometry_msgs::msg::Polygon poly,
- geometry_msgs::msg::Point32 * p0,
- geometry_msgs::msg::Point32 * p1,
+ const size_t i, const size_t N, const geometry_msgs::msg::Polygon poly,
+ geometry_msgs::msg::Point32 * p0, geometry_msgs::msg::Point32 * p1,
geometry_msgs::msg::Point32 * p2)
{
*p1 = poly.points[i];
@@ -336,15 +310,13 @@ void adjacentPoints(
}
}
-std::vector<geometry_msgs::msg::Polygon> lanelet2Triangle(
- const lanelet::ConstLanelet & ll)
+std::vector<geometry_msgs::msg::Polygon> lanelet2Triangle(const lanelet::ConstLanelet & ll)
{
geometry_msgs::msg::Polygon ls_poly = lanelet2Polygon(ll);
return polygon2Triangle(ls_poly);
}
-std::vector<geometry_msgs::msg::Polygon> area2Triangle(
- const lanelet::Area & area)
+std::vector<geometry_msgs::msg::Polygon> area2Triangle(const lanelet::Area & area)
{
geometry_msgs::msg::Polygon ls_poly = area2Polygon(area);
return polygon2Triangle(ls_poly);
@@ -353,27 +325,25 @@ std::vector<geometry_msgs::msg::Polygon> area2Triangle(
// Is angle AOB less than 180?
// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e
bool8_t isAcuteAngle(
- const geometry_msgs::msg::Point32 & vertex_a,
- const geometry_msgs::msg::Point32 & vertex_o,
+ const geometry_msgs::msg::Point32 & vertex_a, const geometry_msgs::msg::Point32 & vertex_o,
const geometry_msgs::msg::Point32 & vertex_b)
{
return (vertex_a.x - vertex_o.x) * (vertex_b.y - vertex_o.y) -
- (vertex_a.y - vertex_o.y) * (vertex_b.x - vertex_o.x) >= 0;
+ (vertex_a.y - vertex_o.y) * (vertex_b.x - vertex_o.x) >=
+ 0;
}
// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e
bool8_t isWithinTriangle(
- const geometry_msgs::msg::Point32 & vertex_a,
- const geometry_msgs::msg::Point32 & vertex_b,
- const geometry_msgs::msg::Point32 & vertex_c,
- const geometry_msgs::msg::Point32 & pt)
+ const geometry_msgs::msg::Point32 & vertex_a, const geometry_msgs::msg::Point32 & vertex_b,
+ const geometry_msgs::msg::Point32 & vertex_c, const geometry_msgs::msg::Point32 & pt)
{
float64_t c1 = (vertex_b.x - vertex_a.x) * (pt.y - vertex_b.y) -
- (vertex_b.y - vertex_a.y) * (pt.x - vertex_b.x);
+ (vertex_b.y - vertex_a.y) * (pt.x - vertex_b.x);
float64_t c2 = (vertex_c.x - vertex_b.x) * (pt.y - vertex_c.y) -
- (vertex_c.y - vertex_b.y) * (pt.x - vertex_c.x);
+ (vertex_c.y - vertex_b.y) * (pt.x - vertex_c.x);
float64_t c3 = (vertex_a.x - vertex_c.x) * (pt.y - vertex_a.y) -
- (vertex_a.y - vertex_c.y) * (pt.x - vertex_a.x);
+ (vertex_a.y - vertex_c.y) * (pt.x - vertex_a.x);
return (c1 > 0.0 && c2 > 0.0 && c3 > 0.0) || (c1 < 0.0 && c2 < 0.0 && c3 < 0.0);
}
@@ -447,24 +417,20 @@ std::vector<geometry_msgs::msg::Polygon> polygon2Triangle(
return triangles;
}
-
-geometry_msgs::msg::Polygon area2Polygon(
- const lanelet::ConstArea & area)
+geometry_msgs::msg::Polygon area2Polygon(const lanelet::ConstArea & area)
{
geometry_msgs::msg::Polygon polygon;
polygon.points.clear();
polygon.points.reserve(area.outerBoundPolygon().size());
std::transform(
- area.outerBoundPolygon().begin(),
- area.outerBoundPolygon().end(),
+ area.outerBoundPolygon().begin(), area.outerBoundPolygon().end(),
std::back_inserter(polygon.points),
- [](lanelet::ConstPoint3d pt) {return toGeomMsgPt32(pt.basicPoint());});
+ [](lanelet::ConstPoint3d pt) { return toGeomMsgPt32(pt.basicPoint()); });
return polygon;
}
-geometry_msgs::msg::Polygon lanelet2Polygon(
- const lanelet::ConstLanelet & ll)
+geometry_msgs::msg::Polygon lanelet2Polygon(const lanelet::ConstLanelet & ll)
{
geometry_msgs::msg::Polygon polygon;
@@ -473,17 +439,13 @@ geometry_msgs::msg::Polygon lanelet2Polygon(
polygon.points.reserve(ll_poly.size());
std::transform(
- ll_poly.begin(),
- ll_poly.end(),
- std::back_inserter(polygon.points),
- [](lanelet::ConstPoint3d pt) {return toGeomMsgPt32(pt.basicPoint());});
+ ll_poly.begin(), ll_poly.end(), std::back_inserter(polygon.points),
+ [](lanelet::ConstPoint3d pt) { return toGeomMsgPt32(pt.basicPoint()); });
return polygon;
}
visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray(
- const rclcpp::Time & t,
- const std::string & ns,
- const lanelet::ConstLanelets & lanelets,
+ const rclcpp::Time & t, const std::string & ns, const lanelet::ConstLanelets & lanelets,
const std_msgs::msg::ColorRGBA & c)
{
visualization_msgs::msg::MarkerArray marker_array;
@@ -494,10 +456,8 @@ visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray(
}
setMarkerHeader(
- &marker, 0, t, "map", ns, c,
- visualization_msgs::msg::Marker::ADD,
- visualization_msgs::msg::Marker::TRIANGLE_LIST,
- 1.0);
+ &marker, 0, t, "map", ns, c, visualization_msgs::msg::Marker::ADD,
+ visualization_msgs::msg::Marker::TRIANGLE_LIST, 1.0);
for (auto ll : lanelets) {
std::vector<geometry_msgs::msg::Polygon> triangles = lanelet2Triangle(ll);
@@ -532,10 +492,8 @@ visualization_msgs::msg::MarkerArray areasAsTriangleMarkerArray(
}
setMarkerHeader(
- &marker, 0, t, "map", ns, c,
- visualization_msgs::msg::Marker::ADD,
- visualization_msgs::msg::Marker::TRIANGLE_LIST,
- 1.0);
+ &marker, 0, t, "map", ns, c, visualization_msgs::msg::Marker::ADD,
+ visualization_msgs::msg::Marker::TRIANGLE_LIST, 1.0);
for (auto area : areas) {
std::vector<geometry_msgs::msg::Polygon> triangles = area2Triangle(area);
diff --git a/common/motion_common/include/motion_common/config.hpp b/common/motion_common/include/motion_common/config.hpp
index 3f4cdde7f45c7..f80309dcb2a87 100644
--- a/common/motion_common/include/motion_common/config.hpp
+++ b/common/motion_common/include/motion_common/config.hpp
@@ -14,27 +14,27 @@
#ifndef MOTION_COMMON__CONFIG_HPP_
#define MOTION_COMMON__CONFIG_HPP_
-#include <motion_common/visibility_control.hpp>
#include <motion_common/motion_common.hpp>
+#include <motion_common/visibility_control.hpp>
#define MOTION_COMMON_COPY_MOVE_ASSIGNABLE(Class) \
- Class(const Class &) = default; \
- Class(Class &&) = default; \
- Class & operator=(const Class &) = default; \
- Class & operator=(Class &&) = default; \
+ Class(const Class &) = default; \
+ Class(Class &&) = default; \
+ Class & operator=(const Class &) = default; \
+ Class & operator=(Class &&) = default; \
~Class() = default;
namespace motion
{
namespace motion_common
{
-using motion_common::Real;
using motion_common::Command;
-using motion_common::State;
-using motion_common::Trajectory;
-using motion_common::Point;
using motion_common::Heading;
using motion_common::Index;
+using motion_common::Point;
+using motion_common::Real;
+using motion_common::State;
+using motion_common::Trajectory;
/// Extreme values for state/control variables
class MOTION_COMMON_PUBLIC LimitsConfig
@@ -43,25 +43,21 @@ class MOTION_COMMON_PUBLIC LimitsConfig
/// \brief Class representing min and max values for a variable
class Extremum
{
-public:
+ public:
Extremum(Real min, Real max);
MOTION_COMMON_COPY_MOVE_ASSIGNABLE(Extremum)
Real min() const noexcept;
Real max() const noexcept;
-private:
+ private:
Real m_min;
Real m_max;
}; // class Extremum
LimitsConfig(
- Extremum longitudinal_velocity_mps,
- Extremum lateral_velocity_mps,
- Extremum acceleration_mps2,
- Extremum yaw_rate_rps,
- Extremum jerk_mps3,
- Extremum steer_angle_rad,
+ Extremum longitudinal_velocity_mps, Extremum lateral_velocity_mps, Extremum acceleration_mps2,
+ Extremum yaw_rate_rps, Extremum jerk_mps3, Extremum steer_angle_rad,
Extremum steer_angle_rate_rps);
MOTION_COMMON_COPY_MOVE_ASSIGNABLE(LimitsConfig)
@@ -88,15 +84,9 @@ class MOTION_COMMON_PUBLIC VehicleConfig
{
public:
VehicleConfig(
- Real length_cg_front_axel_m,
- Real length_cg_rear_axel_m,
- Real front_cornering_stiffness_N,
- Real rear_cornering_stiffness_N,
- Real mass_kg,
- Real inertia_kgm2,
- Real width_m,
- Real front_overhang_m,
- Real rear_overhang_m);
+ Real length_cg_front_axel_m, Real length_cg_rear_axel_m, Real front_cornering_stiffness_N,
+ Real rear_cornering_stiffness_N, Real mass_kg, Real inertia_kgm2, Real width_m,
+ Real front_overhang_m, Real rear_overhang_m);
MOTION_COMMON_COPY_MOVE_ASSIGNABLE(VehicleConfig)
Real length_cg_front_axel() const noexcept;
@@ -127,15 +117,8 @@ class MOTION_COMMON_PUBLIC StateWeight
{
public:
StateWeight(
- Real pose,
- Real heading,
- Real longitudinal_velocity,
- Real lateral_velocity,
- Real yaw_rate,
- Real acceleration,
- Real jerk,
- Real steer_angle,
- Real steer_angle_rate);
+ Real pose, Real heading, Real longitudinal_velocity, Real lateral_velocity, Real yaw_rate,
+ Real acceleration, Real jerk, Real steer_angle, Real steer_angle_rate);
MOTION_COMMON_COPY_MOVE_ASSIGNABLE(StateWeight)
Real pose() const noexcept;
@@ -165,9 +148,7 @@ class MOTION_COMMON_PUBLIC StateWeight
class MOTION_COMMON_PUBLIC OptimizationConfig
{
public:
- OptimizationConfig(
- StateWeight nominal_weights,
- StateWeight terminal_weights);
+ OptimizationConfig(StateWeight nominal_weights, StateWeight terminal_weights);
MOTION_COMMON_COPY_MOVE_ASSIGNABLE(OptimizationConfig)
StateWeight nominal() const noexcept;
diff --git a/common/motion_common/include/motion_common/motion_common.hpp b/common/motion_common/include/motion_common/motion_common.hpp
index 6856636439240..a9b52ad38ff9f 100644
--- a/common/motion_common/include/motion_common/motion_common.hpp
+++ b/common/motion_common/include/motion_common/motion_common.hpp
@@ -15,9 +15,11 @@
#define MOTION_COMMON__MOTION_COMMON_HPP_
#include <motion_common/visibility_control.hpp>
+#include <time_utils/time_utils.hpp>
+
#include <autoware_auto_geometry_msgs/msg/complex32.hpp>
-#include <autoware_auto_system_msgs/msg/control_diagnostic.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_system_msgs/msg/control_diagnostic.hpp>
#include <autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
@@ -25,7 +27,6 @@
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <time_utils/time_utils.hpp>
#include <algorithm>
#include <cmath>
@@ -56,12 +57,9 @@ MOTION_COMMON_PUBLIC
bool is_past_point(const Point & state, const Point & pt, double nx, double ny) noexcept;
/// Advance to the first trajectory point past state according to criterion is_past_point
-template<typename IsPastPointF>
+template <typename IsPastPointF>
Index update_reference_index(
- const Trajectory & traj,
- const State & state,
- Index start_idx,
- IsPastPointF is_past_point)
+ const Trajectory & traj, const State & state, Index start_idx, IsPastPointF is_past_point)
{
// Invariant: m_reference_trajectory.points.size > 0U
if (traj.points.empty()) {
@@ -90,13 +88,11 @@ MOTION_COMMON_PUBLIC bool heading_ok(const Trajectory & traj);
/// Apply transform to TrajectoryPoint
MOTION_COMMON_PUBLIC void doTransform(
- const Point & t_in,
- Point & t_out,
+ const Point & t_in, Point & t_out,
const geometry_msgs::msg::TransformStamped & transform) noexcept;
/// Apply transform to VehicleKinematicState
MOTION_COMMON_PUBLIC void doTransform(
- const State & t_in,
- State & t_out,
+ const State & t_in, State & t_out,
const geometry_msgs::msg::TransformStamped & transform) noexcept;
/// Converts 2D quaternion to simple heading representation
@@ -105,7 +101,7 @@ MOTION_COMMON_PUBLIC Real to_angle(Heading heading) noexcept;
MOTION_COMMON_PUBLIC Real to_angle(Orientation orientation) noexcept;
/// Basic conversion
-template<typename RealT>
+template <typename RealT>
Heading heading_from_angle(RealT angle) noexcept
{
static_assert(std::is_floating_point<RealT>::value, "angle must be floating point");
@@ -115,7 +111,7 @@ Heading heading_from_angle(RealT angle) noexcept
return ret;
}
-template<typename RealT>
+template <typename RealT>
Orientation from_angle(RealT angle) noexcept
{
static_assert(std::is_floating_point<RealT>::value, "angle must be floating point");
@@ -128,7 +124,7 @@ Orientation from_angle(RealT angle) noexcept
/// \tparam QuatT A quaternion-like object with at least z and w members
/// \param[in] quat A quaternion-like object to be converted to a heading object
/// \returns A converted heading object
-template<typename QuatT>
+template <typename QuatT>
Heading from_quat(QuatT quat) noexcept
{
Heading ret{};
@@ -138,7 +134,7 @@ Heading from_quat(QuatT quat) noexcept
}
/// Standard clamp implementation
-template<typename T>
+template <typename T>
T clamp(T val, T min, T max)
{
if (max < min) {
@@ -148,7 +144,7 @@ T clamp(T val, T min, T max)
}
/// Standard linear interpolation
-template<typename T, typename RealT = double>
+template <typename T, typename RealT = double>
T interpolate(T a, T b, RealT t)
{
static_assert(std::is_floating_point<RealT>::value, "t must be floating point");
@@ -161,7 +157,7 @@ T interpolate(T a, T b, RealT t)
MOTION_COMMON_PUBLIC Orientation slerp(const Orientation & a, const Orientation & b, const Real t);
/// Trajectory point interpolation
-template<typename SlerpF>
+template <typename SlerpF>
Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn)
{
Point ret{rosidl_runtime_cpp::MessageInitialization::ALL};
@@ -188,12 +184,9 @@ Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn)
MOTION_COMMON_PUBLIC Point interpolate(Point a, Point b, Real t);
/// Sample a trajectory using interpolation; does not extrapolate
-template<typename SlerpF>
+template <typename SlerpF>
void sample(
- const Trajectory & in,
- Trajectory & out,
- std::chrono::nanoseconds period,
- SlerpF slerp_fn)
+ const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period, SlerpF slerp_fn)
{
out.points.clear();
out.header = in.header;
@@ -240,9 +233,7 @@ void sample(
/// Trajectory sampling with default interpolation
MOTION_COMMON_PUBLIC void sample(
- const Trajectory & in,
- Trajectory & out,
- std::chrono::nanoseconds period);
+ const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period);
/// Diagnostic header stuff
MOTION_COMMON_PUBLIC
@@ -263,7 +254,7 @@ MOTION_COMMON_PUBLIC Complex32 operator-(Complex32 a) noexcept;
/// Difference operator
MOTION_COMMON_PUBLIC Complex32 operator-(Complex32 a, Complex32 b) noexcept;
} // namespace msg
-} // namespace autoware_auto_msgs
+} // namespace autoware_auto_geometry_msgs
namespace geometry_msgs
{
namespace msg
diff --git a/common/motion_common/include/motion_common/trajectory_common.hpp b/common/motion_common/include/motion_common/trajectory_common.hpp
index afd64496335ef..9e87353b15c42 100644
--- a/common/motion_common/include/motion_common/trajectory_common.hpp
+++ b/common/motion_common/include/motion_common/trajectory_common.hpp
@@ -15,21 +15,23 @@
#ifndef MOTION_COMMON__TRAJECTORY_COMMON_HPP_
#define MOTION_COMMON__TRAJECTORY_COMMON_HPP_
-#include <experimental/optional>
-#include <limits>
-#include <stdexcept>
-#include <vector>
-
-#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "geometry/common_2d.hpp"
-#include "geometry_msgs/msg/pose.hpp"
-#include "geometry_msgs/msg/point.hpp"
#include "helper_functions/angle_utils.hpp"
#include "motion_common/motion_common.hpp"
#include "tf2/utils.h"
+#include <experimental/optional>
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+
+#include <limits>
+#include <stdexcept>
+#include <vector>
+
namespace autoware
{
namespace motion
@@ -37,7 +39,7 @@ namespace motion
namespace motion_common
{
typedef autoware_auto_planning_msgs::msg::TrajectoryPoint Point;
-typedef decltype (autoware_auto_planning_msgs::msg::Trajectory::points) Points;
+typedef decltype(autoware_auto_planning_msgs::msg::Trajectory::points) Points;
using autoware::common::types::float64_t;
typedef Eigen::Matrix<float64_t, 3, 1> Vector3f;
@@ -53,9 +55,8 @@ MOTION_COMMON_PUBLIC void validateNonEmpty(const Points & points);
* @param [in] target_yaw target yaw angle [radians]
* @return normalized angle from the base to the target [radians]
*/
-MOTION_COMMON_PUBLIC float64_t calcYawDeviation(
- const float64_t & base_yaw,
- const float64_t & target_yaw);
+MOTION_COMMON_PUBLIC float64_t
+calcYawDeviation(const float64_t & base_yaw, const float64_t & target_yaw);
/**
* @brief search first index with a velocity of zero in the given range of points
@@ -82,12 +83,12 @@ MOTION_COMMON_PUBLIC std::experimental::optional<size_t> searchZeroVelocityIndex
* @param [in] point target point
* @return index of the point nearest to the target
*/
-MOTION_COMMON_PUBLIC size_t findNearestIndex(
- const Points & points,
- const geometry_msgs::msg::Point & point);
+MOTION_COMMON_PUBLIC size_t
+findNearestIndex(const Points & points, const geometry_msgs::msg::Point & point);
/**
- * @brief search the index of the point nearest to the given target with limits on the distance and yaw deviation
+ * @brief search the index of the point nearest to the given target with limits on the distance and
+ * yaw deviation
* @param [in] points list of points to search
* @param [in] pose target point
* @param [in] max_dist optional maximum distance from the pose when searching for the nearest index
@@ -100,66 +101,64 @@ MOTION_COMMON_PUBLIC std::experimental::optional<size_t> findNearestIndex(
const float64_t max_yaw = std::numeric_limits<float64_t>::max());
/**
- * @brief calculate length along trajectory from seg_idx point to nearest point to p_target on trajectory
- * If seg_idx point is after that nearest point, length is negative
- * @param points trajectory points
- * @param seg_idx segment index of point at beginning of length
- * @param p_target target point at end of length
- * @return signed length
- */
+ * @brief calculate length along trajectory from seg_idx point to nearest point to p_target on
+ * trajectory If seg_idx point is after that nearest point, length is negative
+ * @param points trajectory points
+ * @param seg_idx segment index of point at beginning of length
+ * @param p_target target point at end of length
+ * @return signed length
+ */
MOTION_COMMON_PUBLIC float64_t calcLongitudinalOffsetToSegment(
const Points & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target);
/**
- * @brief find nearest segment index to point
- * segment is straight path between two continuous points of trajectory
- * When point is on a trajectory point whose index is nearest_idx, return nearest_idx - 1
- * @param points points of trajectory
- * @param point point to which to find nearest segment index
- * @return nearest index
- */
-MOTION_COMMON_PUBLIC size_t findNearestSegmentIndex(
- const Points & points,
- const geometry_msgs::msg::Point & point);
+ * @brief find nearest segment index to point
+ * segment is straight path between two continuous points of trajectory
+ * When point is on a trajectory point whose index is nearest_idx, return nearest_idx - 1
+ * @param points points of trajectory
+ * @param point point to which to find nearest segment index
+ * @return nearest index
+ */
+MOTION_COMMON_PUBLIC size_t
+findNearestSegmentIndex(const Points & points, const geometry_msgs::msg::Point & point);
/**
- * @brief calculate arc length along points
- * @param [in] points input points
- * @param [in] src_idx source index
- * @param [in] dst_idx destination index
- * @return arc length distance from source to destination along the input points
- */
-MOTION_COMMON_PUBLIC float64_t calcSignedArcLength(
- const Points & points, const size_t src_idx,
- const size_t dst_idx);
+ * @brief calculate arc length along points
+ * @param [in] points input points
+ * @param [in] src_idx source index
+ * @param [in] dst_idx destination index
+ * @return arc length distance from source to destination along the input points
+ */
+MOTION_COMMON_PUBLIC float64_t
+calcSignedArcLength(const Points & points, const size_t src_idx, const size_t dst_idx);
/**
- * @brief calculate arc length along points
- * @param [in] points input points
- * @param [in] src_point source point
- * @param [in] dst_idx destination index
- * @return arc length distance from source to destination along the input points
- */
+ * @brief calculate arc length along points
+ * @param [in] points input points
+ * @param [in] src_point source point
+ * @param [in] dst_idx destination index
+ * @return arc length distance from source to destination along the input points
+ */
MOTION_COMMON_PUBLIC float64_t calcSignedArcLength(
const Points & points, const geometry_msgs::msg::Point & src_point, const size_t & dst_idx);
/**
- * @brief calculate arc length along points
- * @param [in] points input points
- * @param [in] src_point source point
- * @param [in] dst_point destination point
- * @return arc length distance from source to destination along the input points
- */
+ * @brief calculate arc length along points
+ * @param [in] points input points
+ * @param [in] src_point source point
+ * @param [in] dst_point destination point
+ * @return arc length distance from source to destination along the input points
+ */
MOTION_COMMON_PUBLIC float64_t calcSignedArcLength(
const Points & points, const geometry_msgs::msg::Point & src_point,
const geometry_msgs::msg::Point & dst_point);
/**
- * @brief calculate longitudinal deviation of a point relative to a pose
- * @param [in] base_pose base from which to calculate the deviation
- * @param [in] target_point point for which to calculate the deviation
- * @return longitudinal distance between the base and the target
- */
+ * @brief calculate longitudinal deviation of a point relative to a pose
+ * @param [in] base_pose base from which to calculate the deviation
+ * @param [in] target_point point for which to calculate the deviation
+ * @return longitudinal distance between the base and the target
+ */
MOTION_COMMON_PUBLIC float64_t calcLongitudinalDeviation(
const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point);
diff --git a/common/motion_common/include/motion_common/visibility_control.hpp b/common/motion_common/include/motion_common/visibility_control.hpp
index 75725a22ffa23..1379fe321b18f 100644
--- a/common/motion_common/include/motion_common/visibility_control.hpp
+++ b/common/motion_common/include/motion_common/visibility_control.hpp
@@ -16,21 +16,21 @@
#define MOTION_COMMON__VISIBILITY_CONTROL_HPP_
#if defined(__WIN32)
- #if defined(MOTION_COMMON_BUILDING_DLL) || defined(MOTION_COMMON_EXPORTS)
- #define MOTION_COMMON_PUBLIC __declspec(dllexport)
- #define MOTION_COMMON_LOCAL
- #else // defined(MOTION_COMMON_BUILDING_DLL) || defined(MOTION_COMMON_EXPORTS)
- #define MOTION_COMMON_PUBLIC __declspec(dllimport)
- #define MOTION_COMMON_LOCAL
- #endif // defined(MOTION_COMMON_BUILDING_DLL) || defined(MOTION_COMMON_EXPORTS)
+#if defined(MOTION_COMMON_BUILDING_DLL) || defined(MOTION_COMMON_EXPORTS)
+#define MOTION_COMMON_PUBLIC __declspec(dllexport)
+#define MOTION_COMMON_LOCAL
+#else // defined(MOTION_COMMON_BUILDING_DLL) || defined(MOTION_COMMON_EXPORTS)
+#define MOTION_COMMON_PUBLIC __declspec(dllimport)
+#define MOTION_COMMON_LOCAL
+#endif // defined(MOTION_COMMON_BUILDING_DLL) || defined(MOTION_COMMON_EXPORTS)
#elif defined(__linux__)
- #define MOTION_COMMON_PUBLIC __attribute__((visibility("default")))
- #define MOTION_COMMON_LOCAL __attribute__((visibility("hidden")))
+#define MOTION_COMMON_PUBLIC __attribute__((visibility("default")))
+#define MOTION_COMMON_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
- #define MOTION_COMMON_PUBLIC __attribute__((visibility("default")))
- #define MOTION_COMMON_LOCAL __attribute__((visibility("hidden")))
+#define MOTION_COMMON_PUBLIC __attribute__((visibility("default")))
+#define MOTION_COMMON_LOCAL __attribute__((visibility("hidden")))
#else // defined(_LINUX)
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif // defined(_WINDOWS)
#endif // MOTION_COMMON__VISIBILITY_CONTROL_HPP_
diff --git a/common/motion_common/package.xml b/common/motion_common/package.xml
index 2fb4751dbf872..bce90f760fac1 100644
--- a/common/motion_common/package.xml
+++ b/common/motion_common/package.xml
@@ -3,9 +3,7 @@
<package format="2">
<name>motion_common</name>
<version>1.0.0</version>
- <description>
- Helper functions and base classes to aid the development of motion controllers and planners
- </description>
+ <description>Helper functions and base classes to aid the development of motion controllers and planners</description>
<maintainer email="christopherj.ho@gmail.com">Christopher Ho</maintainer>
<license>Apache License 2.0</license>
@@ -29,5 +27,7 @@
<!-- <test_depend>ament_lint_auto</test_depend> -->
<!-- <test_depend>ament_lint_common</test_depend> -->
- <export><build_type>ament_cmake</build_type></export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/common/motion_common/scripts/autogeneration_code_snippets/kinematic_bicycle.snippet.hpp b/common/motion_common/scripts/autogeneration_code_snippets/kinematic_bicycle.snippet.hpp
index 517b34f1df6b0..872cc9595088a 100644
--- a/common/motion_common/scripts/autogeneration_code_snippets/kinematic_bicycle.snippet.hpp
+++ b/common/motion_common/scripts/autogeneration_code_snippets/kinematic_bicycle.snippet.hpp
@@ -27,7 +27,7 @@
DifferentialState x, y, yaw; // pose
DifferentialState u;
-Control ax; // acceleration
+Control ax; // acceleration
Control delta; // wheel angle
// Vehicle parameters
@@ -47,4 +47,7 @@ f << dot(x) == u * cos(yaw + beta);
f << dot(y) == u * sin(yaw + beta);
f << dot(yaw) == (u * sin(beta)) / L_r;
f << dot(u) == ax;
+
+// clang-format off
#endif // COMMON__MOTION_COMMON__SCRIPTS__AUTOGENERATION_CODE_SNIPPETS__KINEMATIC_BICYCLE_SNIPPET_HPP_ // NOLINT
+// clang-format on
diff --git a/common/motion_common/scripts/autogeneration_code_snippets/linear_tire.snippet.hpp b/common/motion_common/scripts/autogeneration_code_snippets/linear_tire.snippet.hpp
index 8f3aa4881c81c..edbdabcdbf58a 100644
--- a/common/motion_common/scripts/autogeneration_code_snippets/linear_tire.snippet.hpp
+++ b/common/motion_common/scripts/autogeneration_code_snippets/linear_tire.snippet.hpp
@@ -24,16 +24,16 @@
// Variables
//
-DifferentialState x, y, yaw; // pose
+DifferentialState x, y, yaw; // pose
DifferentialState u, v, omega; // velocities
-DifferentialState delta; // wheel angle
-DifferentialState ax; // acceleration
+DifferentialState delta; // wheel angle
+DifferentialState ax; // acceleration
Control jx, delta_dot;
// Vehicle parameters
OnlineData L_f, L_r; // front, rear wheelbase length
OnlineData C_f, C_r; // front, rear cornering stiffness
-OnlineData m, I; // mass, moment of inertia
+OnlineData m, I; // mass, moment of inertia
//
// Differential algebraic equation
@@ -56,4 +56,7 @@ f << dot(v) == ((-u * omega) + ((F_f + F_r) / m));
f << dot(omega) == (((L_f * F_f) - (L_r * F_r)) / I);
f << dot(delta) == delta_dot;
f << dot(ax) == jx;
+
+// clang-format off
#endif // COMMON__MOTION_COMMON__SCRIPTS__AUTOGENERATION_CODE_SNIPPETS__LINEAR_TIRE_SNIPPET_HPP_ // NOLINT
+// clang-format on
diff --git a/common/motion_common/src/motion_common/config.cpp b/common/motion_common/src/motion_common/config.cpp
index c6784a0bb931e..8e8377458892e 100644
--- a/common/motion_common/src/motion_common/config.cpp
+++ b/common/motion_common/src/motion_common/config.cpp
@@ -22,31 +22,20 @@ namespace motion
namespace motion_common
{
-LimitsConfig::Extremum::Extremum(Real min, Real max)
-: m_min{min}, m_max{max}
+LimitsConfig::Extremum::Extremum(Real min, Real max) : m_min{min}, m_max{max}
{
if (min >= max - std::numeric_limits<decltype(max)>::epsilon()) {
throw std::domain_error{"Extremum: min >= max - epsilon"};
}
}
-Real LimitsConfig::Extremum::min() const noexcept
-{
- return m_min;
-}
-Real LimitsConfig::Extremum::max() const noexcept
-{
- return m_max;
-}
+Real LimitsConfig::Extremum::min() const noexcept { return m_min; }
+Real LimitsConfig::Extremum::max() const noexcept { return m_max; }
////////////////////////////////////////////////////////////////////////////////
LimitsConfig::LimitsConfig(
- Extremum longitudinal_velocity_mps,
- Extremum lateral_velocity_mps,
- Extremum acceleration_mps2,
- Extremum yaw_rate_rps,
- Extremum jerk_mps3,
- Extremum steer_angle_rad,
+ Extremum longitudinal_velocity_mps, Extremum lateral_velocity_mps, Extremum acceleration_mps2,
+ Extremum yaw_rate_rps, Extremum jerk_mps3, Extremum steer_angle_rad,
Extremum steer_angle_rate_rps)
: m_longitudinal_velocity_limits_mps{longitudinal_velocity_mps},
m_lateral_velocity_limits_mps{lateral_velocity_mps},
@@ -70,10 +59,7 @@ LimitsConfig::Extremum LimitsConfig::acceleration() const noexcept
{
return m_acceleration_limits_mps2;
}
-LimitsConfig::Extremum LimitsConfig::jerk() const noexcept
-{
- return m_jerk_limits_mps3;
-}
+LimitsConfig::Extremum LimitsConfig::jerk() const noexcept { return m_jerk_limits_mps3; }
LimitsConfig::Extremum LimitsConfig::steer_angle() const noexcept
{
return m_steer_angle_limits_rad;
@@ -82,22 +68,13 @@ LimitsConfig::Extremum LimitsConfig::steer_angle_rate() const noexcept
{
return m_steer_angle_rate_limits_rps;
}
-LimitsConfig::Extremum LimitsConfig::yaw_rate() const noexcept
-{
- return m_yaw_rate_limits_rps;
-}
+LimitsConfig::Extremum LimitsConfig::yaw_rate() const noexcept { return m_yaw_rate_limits_rps; }
////////////////////////////////////////////////////////////////////////////////
VehicleConfig::VehicleConfig(
- Real length_cg_front_axel_m,
- Real length_cg_rear_axel_m,
- Real front_cornering_stiffness_N,
- Real rear_cornering_stiffness_N,
- Real mass_kg,
- Real inertia_kgm2,
- Real width_m,
- Real front_overhang_m,
- Real rear_overhang_m)
+ Real length_cg_front_axel_m, Real length_cg_rear_axel_m, Real front_cornering_stiffness_N,
+ Real rear_cornering_stiffness_N, Real mass_kg, Real inertia_kgm2, Real width_m,
+ Real front_overhang_m, Real rear_overhang_m)
: m_length_cg_to_front_axel_m{length_cg_front_axel_m},
m_length_cg_to_rear_axel_m{length_cg_rear_axel_m},
m_front_cornering_stiffness_N{front_cornering_stiffness_N},
@@ -110,14 +87,8 @@ VehicleConfig::VehicleConfig(
{
}
-Real VehicleConfig::length_cg_front_axel() const noexcept
-{
- return m_length_cg_to_front_axel_m;
-}
-Real VehicleConfig::length_cg_rear_axel() const noexcept
-{
- return m_length_cg_to_rear_axel_m;
-}
+Real VehicleConfig::length_cg_front_axel() const noexcept { return m_length_cg_to_front_axel_m; }
+Real VehicleConfig::length_cg_rear_axel() const noexcept { return m_length_cg_to_rear_axel_m; }
Real VehicleConfig::front_cornering_stiffness() const noexcept
{
return m_front_cornering_stiffness_N;
@@ -126,38 +97,16 @@ Real VehicleConfig::rear_cornering_stiffness() const noexcept
{
return m_rear_cornering_stiffness_N;
}
-Real VehicleConfig::mass() const noexcept
-{
- return m_mass_kg;
-}
-Real VehicleConfig::inertia() const noexcept
-{
- return m_inertia_kgm2;
-}
-Real VehicleConfig::width() const noexcept
-{
- return m_width_m;
-}
-Real VehicleConfig::front_overhang() const noexcept
-{
- return m_front_overhang_m;
-}
-Real VehicleConfig::rear_overhang() const noexcept
-{
- return m_rear_overhang_m;
-}
+Real VehicleConfig::mass() const noexcept { return m_mass_kg; }
+Real VehicleConfig::inertia() const noexcept { return m_inertia_kgm2; }
+Real VehicleConfig::width() const noexcept { return m_width_m; }
+Real VehicleConfig::front_overhang() const noexcept { return m_front_overhang_m; }
+Real VehicleConfig::rear_overhang() const noexcept { return m_rear_overhang_m; }
////////////////////////////////////////////////////////////////////////////////
StateWeight::StateWeight(
- Real pose,
- Real heading,
- Real longitudinal_velocity,
- Real lateral_velocity,
- Real yaw_rate,
- Real acceleration,
- Real jerk,
- Real steer_angle,
- Real steer_angle_rate)
+ Real pose, Real heading, Real longitudinal_velocity, Real lateral_velocity, Real yaw_rate,
+ Real acceleration, Real jerk, Real steer_angle, Real steer_angle_rate)
: m_pose_weight{pose},
m_heading_weight{heading},
m_longitudinal_velocity_weight{longitudinal_velocity},
@@ -197,68 +146,32 @@ StateWeight::StateWeight(
}
}
-Real StateWeight::pose() const noexcept
-{
- return m_pose_weight;
-}
+Real StateWeight::pose() const noexcept { return m_pose_weight; }
-Real StateWeight::heading() const noexcept
-{
- return m_heading_weight;
-}
+Real StateWeight::heading() const noexcept { return m_heading_weight; }
-Real StateWeight::longitudinal_velocity() const noexcept
-{
- return m_longitudinal_velocity_weight;
-}
+Real StateWeight::longitudinal_velocity() const noexcept { return m_longitudinal_velocity_weight; }
-Real StateWeight::lateral_velocity() const noexcept
-{
- return m_lateral_velocity_weight;
-}
+Real StateWeight::lateral_velocity() const noexcept { return m_lateral_velocity_weight; }
-Real StateWeight::yaw_rate() const noexcept
-{
- return m_yaw_rate_weight;
-}
+Real StateWeight::yaw_rate() const noexcept { return m_yaw_rate_weight; }
-Real StateWeight::acceleration() const noexcept
-{
- return m_acceleration_weight;
-}
+Real StateWeight::acceleration() const noexcept { return m_acceleration_weight; }
-Real StateWeight::steer_angle() const noexcept
-{
- return m_steer_angle_weight;
-}
+Real StateWeight::steer_angle() const noexcept { return m_steer_angle_weight; }
-Real StateWeight::steer_angle_rate() const noexcept
-{
- return m_steer_angle_rate_weight;
-}
+Real StateWeight::steer_angle_rate() const noexcept { return m_steer_angle_rate_weight; }
-Real StateWeight::jerk() const noexcept
-{
- return m_jerk_weight;
-}
+Real StateWeight::jerk() const noexcept { return m_jerk_weight; }
////////////////////////////////////////////////////////////////////////////////
-OptimizationConfig::OptimizationConfig(
- StateWeight nominal_weights,
- StateWeight terminal_weights)
-: m_nominal_weights{nominal_weights},
- m_terminal_weights{terminal_weights}
+OptimizationConfig::OptimizationConfig(StateWeight nominal_weights, StateWeight terminal_weights)
+: m_nominal_weights{nominal_weights}, m_terminal_weights{terminal_weights}
{
}
-StateWeight OptimizationConfig::nominal() const noexcept
-{
- return m_nominal_weights;
-}
+StateWeight OptimizationConfig::nominal() const noexcept { return m_nominal_weights; }
-StateWeight OptimizationConfig::terminal() const noexcept
-{
- return m_terminal_weights;
-}
+StateWeight OptimizationConfig::terminal() const noexcept { return m_terminal_weights; }
} // namespace motion_common
} // namespace motion
diff --git a/common/motion_common/src/motion_common/motion_common.cpp b/common/motion_common/src/motion_common/motion_common.cpp
index d89ba33d9021e..531363f4d8261 100644
--- a/common/motion_common/src/motion_common/motion_common.cpp
+++ b/common/motion_common/src/motion_common/motion_common.cpp
@@ -14,14 +14,15 @@
#include "motion_common/motion_common.hpp"
-#include <algorithm>
-#include <cmath>
-#include <limits>
-
#include "helper_functions/angle_utils.hpp"
#include "tf2/utils.h"
+
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
namespace motion
{
namespace motion_common
@@ -39,10 +40,7 @@ bool is_past_point(const Point & state, const Point & pt) noexcept
}
////////////////////////////////////////////////////////////////////////////////
-bool is_past_point(
- const Point & state,
- const Point & current_pt,
- const Point & next_pt) noexcept
+bool is_past_point(const Point & state, const Point & current_pt, const Point & next_pt) noexcept
{
const auto nx = next_pt.pose.position.x - current_pt.pose.position.x;
const auto ny = next_pt.pose.position.y - current_pt.pose.position.y;
@@ -51,11 +49,7 @@ bool is_past_point(
}
////////////////////////////////////////////////////////////////////////////////
-bool is_past_point(
- const Point & state,
- const Point & pt,
- const double nx,
- const double ny) noexcept
+bool is_past_point(const Point & state, const Point & pt, const double nx, const double ny) noexcept
{
const auto dx = (state.pose.position.x - pt.pose.position.x);
const auto dy = (state.pose.position.y - pt.pose.position.y);
@@ -81,11 +75,11 @@ bool is_aligned(const Heading a, const Heading b, const Real dot_threshold)
bool heading_ok(const Trajectory & traj)
{
const auto bad_heading = [](const auto & pt) -> bool {
- const auto real2 = static_cast<Real>(pt.pose.orientation.w * pt.pose.orientation.w);
- const auto imag2 = static_cast<Real>(pt.pose.orientation.z * pt.pose.orientation.z);
- constexpr auto TOL = 1.0E-3F;
- return std::fabs(1.0F - (real2 + imag2)) > TOL;
- };
+ const auto real2 = static_cast<Real>(pt.pose.orientation.w * pt.pose.orientation.w);
+ const auto imag2 = static_cast<Real>(pt.pose.orientation.z * pt.pose.orientation.z);
+ constexpr auto TOL = 1.0E-3F;
+ return std::fabs(1.0F - (real2 + imag2)) > TOL;
+ };
const auto bad_it = std::find_if(traj.points.begin(), traj.points.end(), bad_heading);
// True if there is no bad point
return bad_it == traj.points.end();
@@ -93,8 +87,7 @@ bool heading_ok(const Trajectory & traj)
////////////////////////////////////////////////////////////////////////////////
void doTransform(
- const Point & t_in,
- Point & t_out,
+ const Point & t_in, Point & t_out,
const geometry_msgs::msg::TransformStamped & transform) noexcept
{
geometry_msgs::msg::PoseStamped p_in;
@@ -107,8 +100,7 @@ void doTransform(
////////////////////////////////////////////////////////////////////////////////
void doTransform(
- const State & t_in,
- State & t_out,
+ const State & t_in, State & t_out,
const geometry_msgs::msg::TransformStamped & transform) noexcept
{
doTransform(t_in.state, t_out.state, transform);
@@ -127,8 +119,8 @@ Real to_angle(Heading heading) noexcept
}
// See:
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
- const auto y = Real{2.0} *heading.real * heading.imag;
- const auto x = Real{1} - (Real{2.0} *heading.imag * heading.imag);
+ const auto y = Real{2.0} * heading.real * heading.imag;
+ const auto x = Real{1} - (Real{2.0} * heading.imag * heading.imag);
// TODO(c.ho) fast atan2
return std::atan2(y, x);
}
@@ -173,10 +165,7 @@ Orientation slerp(const Orientation & a, const Orientation & b, const Real t)
}
////////////////////////////////////////////////////////////////////////////////
-Point interpolate(Point a, Point b, Real t)
-{
- return interpolate(a, b, t, slerp);
-}
+Point interpolate(Point a, Point b, Real t) { return interpolate(a, b, t, slerp); }
////////////////////////////////////////////////////////////////////////////////
void sample(const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period)
@@ -191,7 +180,7 @@ void error(const Point & state, const Point & ref, Diagnostic & out) noexcept
// compute heading normal of reference point
const auto & q = ref.pose.orientation;
const auto nx = (q.w * q.w) - (q.z * q.z);
- const auto ny = decltype(nx) {2.0} *q.w * q.z;
+ const auto ny = decltype(nx){2.0} * q.w * q.z;
// project state onto reference basis
const auto dx = state.pose.position.x - ref.pose.position.x;
const auto dy = state.pose.position.y - ref.pose.position.y;
@@ -219,7 +208,7 @@ Complex32 operator+(Complex32 a, Complex32 b) noexcept
// check dot product: if negative, reflect one quaternion (360 deg rotation)
{
const auto dot = (a.real * b.real) + (a.imag * b.imag);
- if (dot < decltype(b.real) {}) { // zero initialization
+ if (dot < decltype(b.real){}) { // zero initialization
b.real = -b.real;
b.imag = -b.imag;
}
@@ -233,12 +222,9 @@ Complex32 operator-(Complex32 a) noexcept
a.real = -a.real;
return a;
}
-Complex32 operator-(Complex32 a, Complex32 b) noexcept
-{
- return a + (-b);
-}
+Complex32 operator-(Complex32 a, Complex32 b) noexcept { return a + (-b); }
} // namespace msg
-} // namespace autoware_auto_msgs
+} // namespace autoware_auto_geometry_msgs
namespace geometry_msgs
{
diff --git a/common/motion_common/src/motion_common/trajectory_common.cpp b/common/motion_common/src/motion_common/trajectory_common.cpp
index f461bc77973a7..5693d9e268fc5 100644
--- a/common/motion_common/src/motion_common/trajectory_common.cpp
+++ b/common/motion_common/src/motion_common/trajectory_common.cpp
@@ -14,10 +14,10 @@
#include "motion_common/trajectory_common.hpp"
-#include <limits>
-
#include "motion_common/motion_common.hpp"
+#include <limits>
+
namespace autoware
{
namespace motion
@@ -75,8 +75,7 @@ size_t findNearestIndex(const Points & points, const geometry_msgs::msg::Point &
}
std::experimental::optional<size_t> findNearestIndex(
- const Points & points, const geometry_msgs::msg::Pose & pose,
- const float64_t max_dist,
+ const Points & points, const geometry_msgs::msg::Pose & pose, const float64_t max_dist,
const float64_t max_yaw)
{
validateNonEmpty(points);
@@ -107,8 +106,8 @@ std::experimental::optional<size_t> findNearestIndex(
min_idx = i;
is_nearest_found = true;
}
- return is_nearest_found ? std::experimental::optional<size_t>(min_idx) : std::experimental::
- nullopt;
+ return is_nearest_found ? std::experimental::optional<size_t>(min_idx)
+ : std::experimental::nullopt;
}
float64_t calcLongitudinalOffsetToSegment(
@@ -124,8 +123,9 @@ float64_t calcLongitudinalOffsetToSegment(
const auto y_back = static_cast<float64_t>(p_back.pose.position.y);
const Vector3f segment_vec{x_back - x_front, y_back - y_front, 0.0};
- const Vector3f target_vec{static_cast<float64_t>(p_target.x) - x_front,
- static_cast<float64_t>(p_target.y) - y_front, 0.0};
+ const Vector3f target_vec{
+ static_cast<float64_t>(p_target.x) - x_front, static_cast<float64_t>(p_target.y) - y_front,
+ 0.0};
if (segment_vec.norm() == 0.0) {
throw std::runtime_error("Same points are given.");
diff --git a/common/motion_common/test/interpolation.cpp b/common/motion_common/test/interpolation.cpp
index 15ef68b61b302..e59d9e0af9346 100644
--- a/common/motion_common/test/interpolation.cpp
+++ b/common/motion_common/test/interpolation.cpp
@@ -12,7 +12,6 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <apex_test_tools/apex_test_tools.hpp>
-
#include <motion_common/motion_common.hpp>
#include <motion_testing/motion_testing.hpp>
#include <time_utils/time_utils.hpp>
@@ -21,13 +20,13 @@
#include <limits>
using motion::motion_common::Command;
+using motion::motion_common::from_angle;
using motion::motion_common::Point;
using motion::motion_common::State;
-using motion::motion_common::Trajectory;
-using motion::motion_common::from_angle;
using motion::motion_common::to_angle;
-using motion::motion_testing::make_state;
+using motion::motion_common::Trajectory;
using motion::motion_testing::constant_velocity_trajectory;
+using motion::motion_testing::make_state;
using time_utils::from_message;
@@ -35,24 +34,22 @@ using std::chrono::milliseconds;
TEST(Interpolation, Clamp)
{
- const auto fn_floating = [](auto val, auto min, auto max, auto res, auto tol)
- {
- EXPECT_LT(std::fabs(motion::motion_common::clamp(val, min, max) - res), tol) <<
- val << ", " << min << ", " << max << ", " << res << ", " << tol;
- };
- const auto fn_suite = [ = ](auto min, auto max, auto tol)
- {
- const auto med = (min + max) / decltype(min) {2.0};
- fn_floating(med, min, max, med, tol);
- fn_floating(max, min, max, max, tol);
- fn_floating(min, min, max, min, tol);
- constexpr auto eps = std::numeric_limits<decltype(tol)>::epsilon();
- fn_floating(min - eps, min, max, min, tol);
- fn_floating(max + eps, min, max, max, tol);
- constexpr auto lim = std::numeric_limits<decltype(tol)>::max();
- fn_floating(min - lim, min, max, min, tol);
- fn_floating(max + lim, min, max, max, tol);
- };
+ const auto fn_floating = [](auto val, auto min, auto max, auto res, auto tol) {
+ EXPECT_LT(std::fabs(motion::motion_common::clamp(val, min, max) - res), tol)
+ << val << ", " << min << ", " << max << ", " << res << ", " << tol;
+ };
+ const auto fn_suite = [=](auto min, auto max, auto tol) {
+ const auto med = (min + max) / decltype(min){2.0};
+ fn_floating(med, min, max, med, tol);
+ fn_floating(max, min, max, max, tol);
+ fn_floating(min, min, max, min, tol);
+ constexpr auto eps = std::numeric_limits<decltype(tol)>::epsilon();
+ fn_floating(min - eps, min, max, min, tol);
+ fn_floating(max + eps, min, max, max, tol);
+ constexpr auto lim = std::numeric_limits<decltype(tol)>::max();
+ fn_floating(min - lim, min, max, min, tol);
+ fn_floating(max + lim, min, max, max, tol);
+ };
apex_test_tools::memory_test::start_paused();
// Just have one of these in a compilation unit to make sure everything is hunky-dory
#ifndef __aarch64__
@@ -75,11 +72,10 @@ TEST(Interpolation, Clamp)
TEST(Interpolation, Interpolation)
{
- const auto fn_floating = [](auto a, auto b, auto t, auto res, auto tol)
- {
- EXPECT_LT(std::fabs(motion::motion_common::interpolate(a, b, t) - res), tol) <<
- a << ", " << b << ", " << t << ", " << res << ", " << tol;
- };
+ const auto fn_floating = [](auto a, auto b, auto t, auto res, auto tol) {
+ EXPECT_LT(std::fabs(motion::motion_common::interpolate(a, b, t) - res), tol)
+ << a << ", " << b << ", " << t << ", " << res << ", " << tol;
+ };
constexpr auto TOL = 1.0E-5F;
constexpr auto lim = std::numeric_limits<decltype(TOL)>::max();
// Base case
@@ -106,42 +102,37 @@ TEST(Interpolation, Slerp2d)
{
using motion::motion_common::Orientation;
using motion::motion_common::Real;
- const auto angle_distance = [](Real a, Real b) -> Real
- {
- // https://stackoverflow.com/questions/1878907/the-smallest-difference-between-2-angles?rq=1
- const auto d = a - b;
- return std::atan2(std::sin(d), std::cos(d));
- };
+ const auto angle_distance = [](Real a, Real b) -> Real {
+ // https://stackoverflow.com/questions/1878907/the-smallest-difference-between-2-angles?rq=1
+ const auto d = a - b;
+ return std::atan2(std::sin(d), std::cos(d));
+ };
// Plain check
- const auto test_case = [ = ](Orientation a, Orientation b, Real t, Orientation res, Real tol)
- {
- using motion::motion_common::slerp;
- const auto ret = slerp(a, b, t);
- EXPECT_LT(
- std::fabs(to_angle(ret) - to_angle(res)),
- tol) << to_angle(ret) << ", " << to_angle(res);
- };
+ const auto test_case = [=](Orientation a, Orientation b, Real t, Orientation res, Real tol) {
+ using motion::motion_common::slerp;
+ const auto ret = slerp(a, b, t);
+ EXPECT_LT(std::fabs(to_angle(ret) - to_angle(res)), tol)
+ << to_angle(ret) << ", " << to_angle(res);
+ };
// Check using alternate computation path
- const auto test_case_dual_path = [ = ](Orientation a, Orientation b, Real t, double tol)
- {
- // Compute result using angles
- const auto th_a = to_angle(a);
- const auto th_b = to_angle(b);
- const auto ab = angle_distance(th_a, th_b);
- const auto t_ = motion::motion_common::clamp(t, 0.0F, 1.0F);
- const auto th_t = th_a + (t_ * ab);
- const auto res_th = from_angle(th_t);
- using motion::motion_common::slerp;
- test_case(a, b, t, res_th, tol);
- if (HasFailure()) {
- const auto ret = slerp(a, b, t);
- std::cout << "Angles: " << th_a << ", " << th_b << "; " << th_t << ", " << to_angle(ret) <<
- "\n";
- }
- };
+ const auto test_case_dual_path = [=](Orientation a, Orientation b, Real t, double tol) {
+ // Compute result using angles
+ const auto th_a = to_angle(a);
+ const auto th_b = to_angle(b);
+ const auto ab = angle_distance(th_a, th_b);
+ const auto t_ = motion::motion_common::clamp(t, 0.0F, 1.0F);
+ const auto th_t = th_a + (t_ * ab);
+ const auto res_th = from_angle(th_t);
+ using motion::motion_common::slerp;
+ test_case(a, b, t, res_th, tol);
+ if (HasFailure()) {
+ const auto ret = slerp(a, b, t);
+ std::cout << "Angles: " << th_a << ", " << th_b << "; " << th_t << ", " << to_angle(ret)
+ << "\n";
+ }
+ };
const auto test_suite =
- [ = ](Orientation a, Orientation b, Real tol, Real tol2, bool b_snap = false)
- {
+ [=](Orientation a, Orientation b, Real tol, Real tol2, bool b_snap = false) {
constexpr auto lim = std::numeric_limits<Real>::max();
// If you do something crazy, make b less crazy wrt a
if (!b_snap) {
@@ -181,12 +172,12 @@ TEST(Interpolation, Slerp2d)
TEST(Interpolation, AngleArithmetic)
{
const auto test_fn = [](auto a, auto b, auto res, auto tol) {
- const auto qa = from_angle(a);
- const auto qb = from_angle(b);
- const auto dq = qa - qb;
- const auto dth = to_angle(dq);
- EXPECT_LT(std::fabs(dth - res), tol) << a << ", " << b << ", " << res << ", " << dth;
- };
+ const auto qa = from_angle(a);
+ const auto qb = from_angle(b);
+ const auto dq = qa - qb;
+ const auto dth = to_angle(dq);
+ EXPECT_LT(std::fabs(dth - res), tol) << a << ", " << b << ", " << res << ", " << dth;
+ };
const auto TOL = 1.0E-3F;
apex_test_tools::memory_test::start();
test_fn(0.0F, 1.0F, -1.0F, TOL);
@@ -206,8 +197,8 @@ void generic_checks(const Point & s, const Point & p, std::chrono::nanoseconds d
EXPECT_LT(std::fabs(s.lateral_velocity_mps - p.lateral_velocity_mps), TOL);
EXPECT_LT(std::fabs(s.front_wheel_angle_rad - p.front_wheel_angle_rad), TOL);
EXPECT_LT(std::fabs(s.rear_wheel_angle_rad - p.rear_wheel_angle_rad), TOL);
- EXPECT_TRUE((dt < milliseconds(1)) && (dt > milliseconds(-1))) <<
- std::chrono::duration_cast<milliseconds>(dt).count();
+ EXPECT_TRUE((dt < milliseconds(1)) && (dt > milliseconds(-1)))
+ << std::chrono::duration_cast<milliseconds>(dt).count();
}
TEST(Interpolation, TrajectorySubsample)
{
diff --git a/common/motion_common/test/sanity_checks.cpp b/common/motion_common/test/sanity_checks.cpp
index fd21502b32fc0..ecb289b57b8e4 100644
--- a/common/motion_common/test/sanity_checks.cpp
+++ b/common/motion_common/test/sanity_checks.cpp
@@ -15,12 +15,13 @@
/// \file
/// \brief This file includes basic tests for utility functions in motion_common
-#include <gtest/gtest.h>
+#include <common/types.hpp>
+#include <motion_common/motion_common.hpp>
#include <autoware_auto_geometry_msgs/msg/complex32.hpp>
-#include <common/types.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
-#include <motion_common/motion_common.hpp>
+
+#include <gtest/gtest.h>
using autoware::common::types::float64_t;
using autoware_auto_geometry_msgs::msg::Complex32;
@@ -35,7 +36,8 @@ struct MyQuaternion
float64_t w{1.0};
};
-TEST(HeadingFuncs, FromQuat) {
+TEST(HeadingFuncs, FromQuat)
+{
Quaternion gm_quat{};
gm_quat.z = 0.5f;
gm_quat.w = 0.5f;
diff --git a/common/motion_common/test/trajectory.cpp b/common/motion_common/test/trajectory.cpp
index 7ac5ebd5d2969..7a15d5999e848 100644
--- a/common/motion_common/test/trajectory.cpp
+++ b/common/motion_common/test/trajectory.cpp
@@ -15,22 +15,23 @@
/// \file
/// \brief This file includes tests for functions in trajectory_common
-#include <experimental/optional>
-#include <cmath>
-
#include "common/types.hpp"
#include "gtest/gtest.h"
#include "helper_functions/angle_utils.hpp"
#include "motion_common/trajectory_common.hpp"
#include "tf2/LinearMath/Quaternion.h"
+#include <experimental/optional>
+
+#include <cmath>
using autoware::common::types::float64_t;
-TEST(TrajectoryCommonTests, ValidateNonEmpty) {
- using autoware::motion::motion_common::validateNonEmpty;
+TEST(TrajectoryCommonTests, ValidateNonEmpty)
+{
using autoware::motion::motion_common::Point;
using autoware::motion::motion_common::Points;
+ using autoware::motion::motion_common::validateNonEmpty;
Points points;
EXPECT_THROW(validateNonEmpty(points), std::invalid_argument);
@@ -39,7 +40,8 @@ TEST(TrajectoryCommonTests, ValidateNonEmpty) {
EXPECT_NO_THROW(validateNonEmpty(points));
}
-TEST(TrajectoryCommonTests, CalcYawDeviation) {
+TEST(TrajectoryCommonTests, CalcYawDeviation)
+{
using autoware::motion::motion_common::calcYawDeviation;
EXPECT_EQ(calcYawDeviation(0.0, 0.0), 0.0);
EXPECT_EQ(calcYawDeviation(M_PI, 0.0), -M_PI);
@@ -53,10 +55,11 @@ TEST(TrajectoryCommonTests, CalcYawDeviation) {
EXPECT_EQ(calcYawDeviation(M_PI_2, M_PI), M_PI_2);
}
-TEST(TrajectoryCommonTests, SearchZeroVelocityIndex) {
- using autoware::motion::motion_common::searchZeroVelocityIndex;
+TEST(TrajectoryCommonTests, SearchZeroVelocityIndex)
+{
using autoware::motion::motion_common::Point;
using autoware::motion::motion_common::Points;
+ using autoware::motion::motion_common::searchZeroVelocityIndex;
Points points;
EXPECT_THROW(searchZeroVelocityIndex(points), std::invalid_argument);
@@ -89,7 +92,8 @@ TEST(TrajectoryCommonTests, SearchZeroVelocityIndex) {
EXPECT_EQ(searchZeroVelocityIndex(points, 3, 4, 2.5).value(), size_t(3));
}
-TEST(TrajectoryCommonTests, FindNearestIndex) {
+TEST(TrajectoryCommonTests, FindNearestIndex)
+{
using autoware::motion::motion_common::findNearestIndex;
using autoware::motion::motion_common::Point;
using autoware::motion::motion_common::Points;
@@ -158,7 +162,8 @@ TEST(TrajectoryCommonTests, FindNearestIndex) {
EXPECT_EQ(findNearestIndex(points, pose, 100.0, M_PI_2).value(), size_t(0));
}
-TEST(TrajectoryCommonTests, FindNearestSegmentIndex) {
+TEST(TrajectoryCommonTests, FindNearestSegmentIndex)
+{
using autoware::motion::motion_common::findNearestSegmentIndex;
using autoware::motion::motion_common::Point;
using autoware::motion::motion_common::Points;
@@ -208,7 +213,8 @@ TEST(TrajectoryCommonTests, FindNearestSegmentIndex) {
EXPECT_EQ(findNearestSegmentIndex(points, target), size_t(3));
}
-TEST(TrajectoryCommonTests, CalcLongitudinalOffsetToSegment) {
+TEST(TrajectoryCommonTests, CalcLongitudinalOffsetToSegment)
+{
using autoware::motion::motion_common::calcLongitudinalOffsetToSegment;
using autoware::motion::motion_common::Point;
using autoware::motion::motion_common::Points;
@@ -250,7 +256,8 @@ TEST(TrajectoryCommonTests, CalcLongitudinalOffsetToSegment) {
EXPECT_EQ(calcLongitudinalOffsetToSegment(points, 3, target), -2.0);
}
-TEST(TrajectoryCommonTests, CalcSignedArcLengthIndexToIndex) {
+TEST(TrajectoryCommonTests, CalcSignedArcLengthIndexToIndex)
+{
using autoware::motion::motion_common::calcSignedArcLength;
using autoware::motion::motion_common::Point;
using autoware::motion::motion_common::Points;
diff --git a/common/motion_testing/include/motion_testing/motion_testing.hpp b/common/motion_testing/include/motion_testing/motion_testing.hpp
index 17318552e9982..848e4685205ba 100644
--- a/common/motion_testing/include/motion_testing/motion_testing.hpp
+++ b/common/motion_testing/include/motion_testing/motion_testing.hpp
@@ -15,6 +15,7 @@
#define MOTION_TESTING__MOTION_TESTING_HPP_
#include <motion_testing/visibility_control.hpp>
+
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
@@ -36,12 +37,7 @@ using Real = decltype(Point::longitudinal_velocity_mps);
/// \brief Makes a state, intended to make message generation more terse
MOTION_TESTING_PUBLIC State make_state(
- Real x0,
- Real y0,
- Real heading,
- Real v0,
- Real a0,
- Real turn_rate,
+ Real x0, Real y0, Real heading, Real v0, Real a0, Real turn_rate,
std::chrono::system_clock::time_point t);
/// \brief Generates a state from a normal distribution with the following bounds:
@@ -66,44 +62,23 @@ Trajectory bad_heading_trajectory(const State & start_state, std::chrono::nanose
/// \brief Generates a constant velocity trajectory with invalid heading values
MOTION_TESTING_PUBLIC Trajectory constant_velocity_trajectory(
- float x0,
- float y0,
- float heading,
- float v0,
- std::chrono::nanoseconds dt);
+ float x0, float y0, float heading, float v0, std::chrono::nanoseconds dt);
/// \brief Generates a constant acceleration trajectory
MOTION_TESTING_PUBLIC Trajectory constant_acceleration_trajectory(
- float x0,
- float y0,
- float heading,
- float v0,
- float a0,
- std::chrono::nanoseconds dt);
+ float x0, float y0, float heading, float v0, float a0, std::chrono::nanoseconds dt);
/// \brief Generates a constant velocity and constant turn rate trajectory
MOTION_TESTING_PUBLIC Trajectory constant_velocity_turn_rate_trajectory(
- float x0,
- float y0,
- float heading,
- float v0,
- float turn_rate,
- std::chrono::nanoseconds dt);
+ float x0, float y0, float heading, float v0, float turn_rate, std::chrono::nanoseconds dt);
/// \brief Generates a constant acceleration and constant turn rate trajectory
MOTION_TESTING_PUBLIC Trajectory constant_acceleration_turn_rate_trajectory(
- float x0,
- float y0,
- float heading,
- float v0,
- float a0,
- float turn_rate,
+ float x0, float y0, float heading, float v0, float a0, float turn_rate,
std::chrono::nanoseconds dt);
/// Given a trajectory, advance state to next trajectory point, with normally distributed noise
/// Note: This version takes "hint" as gospel, and doesn't try to do any time/space matching
/// Note: not implemented
MOTION_TESTING_PUBLIC void next_state(
- const Trajectory & trajectory,
- State & state,
- uint32_t hint,
+ const Trajectory & trajectory, State & state, uint32_t hint,
Generator * gen = nullptr); // TODO(c.ho) std::optional NOLINT
// TODO(c.ho) version that takes control commands
@@ -112,9 +87,7 @@ MOTION_TESTING_PUBLIC void next_state(
/// heading tolerance is in dot product space of 2d quaternion
MOTION_TESTING_PUBLIC
Index progresses_towards_target(
- const Trajectory & trajectory,
- const Point & target,
- Real heading_tolerance = Real{0.006F});
+ const Trajectory & trajectory, const Point & target, Real heading_tolerance = Real{0.006F});
/// Checks that a trajectory is more or less dynamically feasible given the derivatives;
/// tolerance is relative tolerance of trajectory, index is first point that is not dynamically
diff --git a/common/motion_testing/include/motion_testing/visibility_control.hpp b/common/motion_testing/include/motion_testing/visibility_control.hpp
index c536894b789a0..2ddfe7c1f2855 100644
--- a/common/motion_testing/include/motion_testing/visibility_control.hpp
+++ b/common/motion_testing/include/motion_testing/visibility_control.hpp
@@ -16,21 +16,21 @@
#define MOTION_TESTING__VISIBILITY_CONTROL_HPP_
#if defined(__WIN32)
- #if defined(MOTION_TESTING_BUILDING_DLL) || defined(MOTION_TESTING_EXPORTS)
- #define MOTION_TESTING_PUBLIC __declspec(dllexport)
- #define MOTION_TESTING_LOCAL
- #else // defined(MOTION_TESTING_BUILDING_DLL) || defined(MOTION_TESTING_EXPORTS)
- #define MOTION_TESTING_PUBLIC __declspec(dllimport)
- #define MOTION_TESTING_LOCAL
- #endif // defined(MOTION_TESTING_BUILDING_DLL) || defined(MOTION_TESTING_EXPORTS)
+#if defined(MOTION_TESTING_BUILDING_DLL) || defined(MOTION_TESTING_EXPORTS)
+#define MOTION_TESTING_PUBLIC __declspec(dllexport)
+#define MOTION_TESTING_LOCAL
+#else // defined(MOTION_TESTING_BUILDING_DLL) || defined(MOTION_TESTING_EXPORTS)
+#define MOTION_TESTING_PUBLIC __declspec(dllimport)
+#define MOTION_TESTING_LOCAL
+#endif // defined(MOTION_TESTING_BUILDING_DLL) || defined(MOTION_TESTING_EXPORTS)
#elif defined(__linux__)
- #define MOTION_TESTING_PUBLIC __attribute__((visibility("default")))
- #define MOTION_TESTING_LOCAL __attribute__((visibility("hidden")))
+#define MOTION_TESTING_PUBLIC __attribute__((visibility("default")))
+#define MOTION_TESTING_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
- #define MOTION_TESTING_PUBLIC __attribute__((visibility("default")))
- #define MOTION_TESTING_LOCAL __attribute__((visibility("hidden")))
+#define MOTION_TESTING_PUBLIC __attribute__((visibility("default")))
+#define MOTION_TESTING_LOCAL __attribute__((visibility("hidden")))
#else // defined(_LINUX)
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif // defined(_WINDOWS)
#endif // MOTION_TESTING__VISIBILITY_CONTROL_HPP_
diff --git a/common/motion_testing/package.xml b/common/motion_testing/package.xml
index dcfffa7164e21..f868f36cbf9da 100644
--- a/common/motion_testing/package.xml
+++ b/common/motion_testing/package.xml
@@ -3,9 +3,7 @@
<package format="2">
<name>motion_testing</name>
<version>1.0.0</version>
- <description>
- Helper functions to aid in the testing of motion planner and motion controllers
- </description>
+ <description>Helper functions to aid in the testing of motion planner and motion controllers</description>
<maintainer email="christopherj.ho@gmail.com">Christopher Ho</maintainer>
<license>Apache License 2.0</license>
@@ -18,13 +16,15 @@
<build_export_depend>autoware_auto_vehicle_msgs</build_export_depend>
<build_depend>autoware_auto_planning_msgs</build_depend>
<build_depend>autoware_auto_vehicle_msgs</build_depend>
- <build_depend>time_utils</build_depend>
<build_depend>tf2</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
+ <build_depend>time_utils</build_depend>
<test_depend>ament_cmake_gtest</test_depend>
<!-- <test_depend>ament_lint_auto</test_depend> -->
<!-- <test_depend>ament_lint_common</test_depend> -->
- <export><build_type>ament_cmake</build_type></export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/common/motion_testing/src/motion_testing/motion_testing.cpp b/common/motion_testing/src/motion_testing/motion_testing.cpp
index e3f6704c2284c..90e4a768970de 100644
--- a/common/motion_testing/src/motion_testing/motion_testing.cpp
+++ b/common/motion_testing/src/motion_testing/motion_testing.cpp
@@ -13,11 +13,12 @@
// limitations under the License.
#include "motion_testing/motion_testing.hpp"
-#include <tf2/convert.h>
+#include <time_utils/time_utils.hpp>
+
#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <time_utils/time_utils.hpp>
#include <limits>
@@ -26,12 +27,7 @@ namespace motion
namespace motion_testing
{
State make_state(
- Real x0,
- Real y0,
- Real heading,
- Real v0,
- Real a0,
- Real turn_rate,
+ Real x0, Real y0, Real heading, Real v0, Real a0, Real turn_rate,
std::chrono::system_clock::time_point t)
{
State start_state{rosidl_runtime_cpp::MessageInitialization::ALL};
@@ -66,8 +62,7 @@ State generate_state(Generator & gen)
quat.setRPY(0.0, 0.0, normal(gen));
ret.state.pose.orientation = tf2::toMsg(quat);
// Parameters with only positive supports
- std::exponential_distribution<decltype(ret.state.longitudinal_velocity_mps)>
- exponential{1.0F};
+ std::exponential_distribution<decltype(ret.state.longitudinal_velocity_mps)> exponential{1.0F};
ret.state.longitudinal_velocity_mps = 5.0F * exponential(gen);
return ret;
@@ -105,9 +100,8 @@ Trajectory constant_trajectory(const State & start_state, const std::chrono::nan
tf2::fromMsg(last_state.pose.orientation, last_orientation);
next_state.pose.orientation = tf2::toMsg(last_orientation * orientation_increment);
// pose.position update: simplified heading effects
- const auto ds =
- static_cast<double>(dt_s *
- (last_state.longitudinal_velocity_mps + (0.5F * dt_s * last_state.acceleration_mps2)));
+ const auto ds = static_cast<double>(
+ dt_s * (last_state.longitudinal_velocity_mps + (0.5F * dt_s * last_state.acceleration_mps2)));
const auto yaw = tf2::getYaw(next_state.pose.orientation);
next_state.pose.position.x += std::cos(yaw) * ds;
next_state.pose.position.y += std::sin(yaw) * ds;
@@ -153,10 +147,7 @@ Trajectory bad_heading_trajectory(const State & start_state, const std::chrono::
////////////////////////////////////////////////////////////////////////////////
Trajectory constant_velocity_trajectory(
- const Real x0,
- const Real y0,
- const Real heading,
- const Real v0,
+ const Real x0, const Real y0, const Real heading, const Real v0,
const std::chrono::nanoseconds dt)
{
return constant_acceleration_turn_rate_trajectory(x0, y0, heading, v0, 0.0F, 0.0F, dt);
@@ -164,11 +155,7 @@ Trajectory constant_velocity_trajectory(
////////////////////////////////////////////////////////////////////////////////
Trajectory constant_acceleration_trajectory(
- const Real x0,
- const Real y0,
- const Real heading,
- const Real v0,
- const Real a0,
+ const Real x0, const Real y0, const Real heading, const Real v0, const Real a0,
const std::chrono::nanoseconds dt)
{
return constant_acceleration_turn_rate_trajectory(x0, y0, heading, v0, a0, 0.0F, dt);
@@ -176,11 +163,7 @@ Trajectory constant_acceleration_trajectory(
////////////////////////////////////////////////////////////////////////////////
Trajectory constant_velocity_turn_rate_trajectory(
- const Real x0,
- const Real y0,
- const Real heading,
- const Real v0,
- const Real turn_rate,
+ const Real x0, const Real y0, const Real heading, const Real v0, const Real turn_rate,
const std::chrono::nanoseconds dt)
{
return constant_acceleration_turn_rate_trajectory(x0, y0, heading, v0, 0.0F, turn_rate, dt);
@@ -188,13 +171,8 @@ Trajectory constant_velocity_turn_rate_trajectory(
////////////////////////////////////////////////////////////////////////////////
Trajectory constant_acceleration_turn_rate_trajectory(
- const Real x0,
- const Real y0,
- const Real heading,
- const Real v0,
- const Real a0,
- const Real turn_rate,
- const std::chrono::nanoseconds dt)
+ const Real x0, const Real y0, const Real heading, const Real v0, const Real a0,
+ const Real turn_rate, const std::chrono::nanoseconds dt)
{
State start_state =
make_state(x0, y0, heading, v0, a0, turn_rate, std::chrono::system_clock::now());
@@ -204,10 +182,7 @@ Trajectory constant_acceleration_turn_rate_trajectory(
////////////////////////////////////////////////////////////////////////////////
void next_state(
- const Trajectory & trajectory,
- State & state,
- const uint32_t hint,
- Generator * const gen)
+ const Trajectory & trajectory, State & state, const uint32_t hint, Generator * const gen)
{
(void)trajectory;
(void)state;
@@ -217,9 +192,7 @@ void next_state(
////////////////////////////////////////////////////////////////////////////////
Index progresses_towards_target(
- const Trajectory & trajectory,
- const Point & target,
- const Real heading_tolerance)
+ const Trajectory & trajectory, const Point & target, const Real heading_tolerance)
{
auto last_err = std::numeric_limits<double>::max();
auto last_heading_err = -std::numeric_limits<Real>::max();
@@ -257,23 +230,22 @@ Index dynamically_feasible(const Trajectory & trajectory, const Real tolerance)
for (auto idx = Index{1}; idx < trajectory.points.size(); ++idx) {
const auto & pt = trajectory.points[idx];
const auto dt_ = time_utils::from_message(pt.time_from_start) -
- time_utils::from_message(last_pt.time_from_start);
+ time_utils::from_message(last_pt.time_from_start);
const auto dt = std::chrono::duration_cast<std::chrono::duration<Real>>(dt_).count();
const auto dv = last_pt.acceleration_mps2 * dt;
- const auto ds =
- (Real{0.5} *dv * dt) + (last_pt.longitudinal_velocity_mps * dt);
+ const auto ds = (Real{0.5} * dv * dt) + (last_pt.longitudinal_velocity_mps * dt);
const auto dn = last_pt.lateral_velocity_mps * dt;
const auto dth = last_pt.heading_rate_rps * dt;
const auto check_fn = [tolerance](auto expect, auto val, auto str) -> bool {
- bool success = true;
- if (static_cast<Real>(std::fabs(expect)) < Real{1}) {
- success = static_cast<Real>(std::fabs(expect - val)) < tolerance;
- } else {
- success = static_cast<Real>(std::fabs(expect - val) / expect) < tolerance;
- }
- (void)str;
- return success;
- };
+ bool success = true;
+ if (static_cast<Real>(std::fabs(expect)) < Real{1}) {
+ success = static_cast<Real>(std::fabs(expect - val)) < tolerance;
+ } else {
+ success = static_cast<Real>(std::fabs(expect - val) / expect) < tolerance;
+ }
+ (void)str;
+ return success;
+ };
bool ok = true;
{
const auto v = last_pt.longitudinal_velocity_mps + dv;
@@ -302,11 +274,11 @@ Index dynamically_feasible(const Trajectory & trajectory, const Real tolerance)
const tf2::Transform tf2(new_th);
const tf2::Vector3 delta2 = tf2 * ds_dn;
ok = (check_fn(last_pt.pose.position.x + delta.getX(), pt.pose.position.x, "x") ||
- check_fn(last_pt.pose.position.x + delta2.getX(), pt.pose.position.x, "x2")) &&
- ok;
+ check_fn(last_pt.pose.position.x + delta2.getX(), pt.pose.position.x, "x2")) &&
+ ok;
ok = (check_fn(last_pt.pose.position.y + delta.getY(), pt.pose.position.y, "y") ||
- check_fn(last_pt.pose.position.y + delta2.getY(), pt.pose.position.y, "y2")) &&
- ok;
+ check_fn(last_pt.pose.position.y + delta2.getY(), pt.pose.position.y, "y2")) &&
+ ok;
}
if (!ok) {
return idx;
diff --git a/common/motion_testing/test/constant_trajectory.cpp b/common/motion_testing/test/constant_trajectory.cpp
index b9546352dd25a..02563f7134d0f 100644
--- a/common/motion_testing/test/constant_trajectory.cpp
+++ b/common/motion_testing/test/constant_trajectory.cpp
@@ -11,10 +11,11 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <gtest/gtest.h>
#include <motion_testing/motion_testing.hpp>
#include <time_utils/time_utils.hpp>
+#include <gtest/gtest.h>
+
using motion::motion_testing::State;
using motion::motion_testing::Trajectory;
using time_utils::from_message;
@@ -59,8 +60,7 @@ TEST(ConstantTrajectory, ConstantVelocity)
EXPECT_FLOAT_EQ(std::sin(heading), s);
EXPECT_FLOAT_EQ(std::cos(heading), c);
const auto dt = std::chrono::milliseconds(100LL);
- const auto traj = motion::motion_testing::constant_velocity_trajectory(
- x0, y0, heading, v0, dt);
+ const auto traj = motion::motion_testing::constant_velocity_trajectory(x0, y0, heading, v0, dt);
const auto dt_s = std::chrono::duration_cast<std::chrono::duration<float_t>>(dt).count();
for (auto i = 0U; i < traj.points.size(); ++i) {
const auto & t = traj.points[i];
@@ -97,8 +97,8 @@ TEST(ConstantTrajectory, ConstantAcceleration)
EXPECT_FLOAT_EQ(std::sin(heading), s);
EXPECT_FLOAT_EQ(std::cos(heading), c);
const auto dt = std::chrono::milliseconds(100LL);
- const auto traj = motion::motion_testing::constant_acceleration_trajectory(
- x0, y0, heading, v0, a0, dt);
+ const auto traj =
+ motion::motion_testing::constant_acceleration_trajectory(x0, y0, heading, v0, a0, dt);
const auto dt_s = std::chrono::duration_cast<std::chrono::duration<float_t>>(dt).count();
for (auto i = 0U; i < traj.points.size(); ++i) {
const auto & t = traj.points[i];
diff --git a/common/motion_testing/test/trajectory_checks.cpp b/common/motion_testing/test/trajectory_checks.cpp
index 7b4498b292f96..5a63fd81b40d4 100644
--- a/common/motion_testing/test/trajectory_checks.cpp
+++ b/common/motion_testing/test/trajectory_checks.cpp
@@ -11,23 +11,24 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <gtest/gtest.h>
#include <motion_testing/motion_testing.hpp>
+#include <gtest/gtest.h>
+
#include <chrono>
+using motion::motion_testing::constant_velocity_trajectory;
+using motion::motion_testing::make_state;
using motion::motion_testing::State;
using motion::motion_testing::Trajectory;
-using motion::motion_testing::make_state;
-using motion::motion_testing::constant_velocity_trajectory;
TEST(TrajectoryChecks, Basic)
{
Trajectory traj{};
const auto target =
make_state(-100.0F, 100.0F, 2.0F, 1.0F, 0.0F, 0.0F, std::chrono::system_clock::now());
- using motion::motion_testing::progresses_towards_target;
using motion::motion_testing::dynamically_feasible;
+ using motion::motion_testing::progresses_towards_target;
// Empty case
ASSERT_TRUE(traj.points.empty());
EXPECT_EQ(dynamically_feasible(traj), {});
diff --git a/common/osqp_interface/design/osqp_interface-design.md b/common/osqp_interface/design/osqp_interface-design.md
index 5ed1c81a17e2a..fcd0f81667f2a 100644
--- a/common/osqp_interface/design/osqp_interface-design.md
+++ b/common/osqp_interface/design/osqp_interface-design.md
@@ -1,61 +1,72 @@
-Interface for the OSQP library {#osqp_interface-package-design}
-===========
+# Interface for the OSQP library {#osqp_interface-package-design}
This is the design document for the `osqp_interface` package.
+## Purpose / Use cases
-# Purpose / Use cases
<!-- Required -->
<!-- Things to consider:
- Why did we implement this feature? -->
+
This packages provides a C++ interface for the [OSQP library](https://osqp.org/docs/solver/index.html).
+## Design
-# Design
<!-- Required -->
<!-- Things to consider:
- How does it work? -->
+
The class `OSQPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into
C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the OSQP workspace dataholder, and runs the optimizer.
## Inputs / Outputs / API
+
<!-- Required -->
<!-- Things to consider:
- How do you use the package / API? -->
- The interface can be used in several ways:
- 1. Initialize the interface WITHOUT data. Load the problem formulation at the optimization call.
- ```
- osqp_interface = OSQPInterface();
- osqp_interface.optimize(P, A, q, l, u);
- ```
+The interface can be used in several ways:
+
+1. Initialize the interface WITHOUT data. Load the problem formulation at the optimization call.
- 2. Initialize the interface WITH data.
+ ```cpp
+ osqp_interface = OSQPInterface();
+ osqp_interface.optimize(P, A, q, l, u);
```
- osqp_interface = OSQPInterface(P, A, q, l, u);
- osqp_interface.optimize();
+
+2. Initialize the interface WITH data.
+
+ ```cpp
+ osqp_interface = OSQPInterface(P, A, q, l, u);
+ osqp_interface.optimize();
```
- 3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs.
+3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs.
+
+ ```cpp
+ osqp_interface = OSQPInterface(P, A, q, l, u);
+ osqp_interface.optimize();
+ osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new);
+ osqp_interface.optimize();
```
- osqp_interface = OSQPInterface(P, A, q, l, u);
- osqp_interface.optimize();
- osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new);
- osqp_interface.optimize();
+
+ The optimization results are returned as a vector by the optimization function.
+
+ ```cpp
+ std::tuple<std::vector<double>, std::vector<double>> result = osqp_interface.optimize();
+ std::vector<double> param = std::get<0>(result);
+ double x_0 = param[0];
+ double x_1 = param[1];
```
-The optimization results are returned as a vector by the optimization function.
-```
- std::tuple<std::vector<double>, std::vector<double>> result = osqp_interface.optimize();
- std::vector<double> param = std::get<0>(result);
- double x_0 = param[0];
- double x_1 = param[1];
-```
+## References / External links
-# References / External links
<!-- Optional -->
-- OSQP library: https://osqp.org/
-# Related issues
+- OSQP library: <https://osqp.org/>
+
+## Related issues
+
<!-- Required -->
-- This package was introduced as a dependency of the MPC-based lateral controller: https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057
+
+- This package was introduced as a dependency of the MPC-based lateral controller: <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057>
diff --git a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp
index 8f50bda214fd6..2301c1cb04e65 100644
--- a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp
+++ b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp
@@ -15,12 +15,12 @@
#ifndef OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_
#define OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_
-#include <vector>
-
#include "eigen3/Eigen/Core"
#include "osqp/glob_opts.h" // for 'c_int' type ('long' or 'long long')
#include "osqp_interface/visibility_control.hpp"
+#include <vector>
+
namespace autoware
{
namespace common
diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp
index 950f699b912dd..53df1c6d329c9 100644
--- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp
+++ b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp
@@ -132,8 +132,8 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface
const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector<float64_t> & q,
const std::vector<float64_t> & l, const std::vector<float64_t> & u);
int64_t initializeProblem(
- CSC_Matrix P, CSC_Matrix A, const std::vector<float64_t> & q,
- const std::vector<float64_t> & l, const std::vector<float64_t> & u);
+ CSC_Matrix P, CSC_Matrix A, const std::vector<float64_t> & q, const std::vector<float64_t> & l,
+ const std::vector<float64_t> & u);
// Updates problem parameters while keeping solution in memory.
//
diff --git a/common/osqp_interface/include/osqp_interface/visibility_control.hpp b/common/osqp_interface/include/osqp_interface/visibility_control.hpp
index 816d3c0fff82f..6ffc9814f1667 100644
--- a/common/osqp_interface/include/osqp_interface/visibility_control.hpp
+++ b/common/osqp_interface/include/osqp_interface/visibility_control.hpp
@@ -17,21 +17,21 @@
////////////////////////////////////////////////////////////////////////////////
#if defined(__WIN32)
- #if defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS)
- #define OSQP_INTERFACE_PUBLIC __declspec(dllexport)
- #define OSQP_INTERFACE_LOCAL
- #else // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS)
- #define OSQP_INTERFACE_PUBLIC __declspec(dllimport)
- #define OSQP_INTERFACE_LOCAL
- #endif // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS)
+#if defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS)
+#define OSQP_INTERFACE_PUBLIC __declspec(dllexport)
+#define OSQP_INTERFACE_LOCAL
+#else // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS)
+#define OSQP_INTERFACE_PUBLIC __declspec(dllimport)
+#define OSQP_INTERFACE_LOCAL
+#endif // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS)
#elif defined(__linux__)
- #define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default")))
- #define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden")))
+#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default")))
+#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
- #define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default")))
- #define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden")))
+#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default")))
+#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden")))
#else
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif
#endif // OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_
diff --git a/common/osqp_interface/package.xml b/common/osqp_interface/package.xml
index 34bb8a5a195d7..74458b2624890 100644
--- a/common/osqp_interface/package.xml
+++ b/common/osqp_interface/package.xml
@@ -17,9 +17,9 @@
<depend>rclcpp_components</depend>
<test_depend>ament_cmake_gtest</test_depend>
+ <test_depend>eigen</test_depend>
<!-- <test_depend>ament_lint_auto</test_depend> -->
<!-- <test_depend>ament_lint_common</test_depend> -->
- <test_depend>eigen</test_depend>
<export>
<build_type>ament_cmake</build_type>
diff --git a/common/osqp_interface/src/csc_matrix_conv.cpp b/common/osqp_interface/src/csc_matrix_conv.cpp
index 6dbe362ab277e..9ebd3f2ea2d35 100644
--- a/common/osqp_interface/src/csc_matrix_conv.cpp
+++ b/common/osqp_interface/src/csc_matrix_conv.cpp
@@ -12,13 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <exception>
-#include <iostream>
-#include <vector>
+#include "osqp_interface/csc_matrix_conv.hpp"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/SparseCore"
-#include "osqp_interface/csc_matrix_conv.hpp"
+
+#include <exception>
+#include <iostream>
+#include <vector>
namespace autoware
{
@@ -45,7 +46,7 @@ CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat)
col_idxs.push_back(0);
- for (Eigen::Index j = 0; j < cols; j++) { // col iteration
+ for (Eigen::Index j = 0; j < cols; j++) { // col iteration
for (Eigen::Index i = 0; i < rows; i++) { // row iteration
// Get values of nonzero elements
val = mat(i, j);
@@ -91,7 +92,7 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat)
col_idxs.push_back(0);
- for (Eigen::Index j = 0; j < cols; j++) { // col iteration
+ for (Eigen::Index j = 0; j < cols; j++) { // col iteration
for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration
// Get values of nonzero elements
val = mat(i, j);
diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp
index 3052844d6b6ea..4bc1c1d6f9443 100644
--- a/common/osqp_interface/src/osqp_interface.cpp
+++ b/common/osqp_interface/src/osqp_interface.cpp
@@ -62,8 +62,7 @@ OSQPInterface::OSQPInterface(
OSQPInterface::OSQPInterface(
const CSC_Matrix & P, const CSC_Matrix & A, const std::vector<float64_t> & q,
- const std::vector<float64_t> & l, const std::vector<float64_t> & u,
- const c_float eps_abs)
+ const std::vector<float64_t> & l, const std::vector<float64_t> & u, const c_float eps_abs)
: OSQPInterface(eps_abs)
{
initializeProblem(P, A, q, l, u);
@@ -88,12 +87,14 @@ void OSQPInterface::updateP(const Eigen::MatrixXd & P_new)
c_int P_elem_N = P_sparse.nonZeros();
*/
CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new);
- osqp_update_P(m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(P_csc.m_vals.size()));
+ osqp_update_P(
+ m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(P_csc.m_vals.size()));
}
void OSQPInterface::updateCscP(const CSC_Matrix & P_csc)
{
- osqp_update_P(m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(P_csc.m_vals.size()));
+ osqp_update_P(
+ m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(P_csc.m_vals.size()));
}
void OSQPInterface::updateA(const Eigen::MatrixXd & A_new)
@@ -106,13 +107,15 @@ void OSQPInterface::updateA(const Eigen::MatrixXd & A_new)
c_int A_elem_N = A_sparse.nonZeros();
*/
CSC_Matrix A_csc = calCSCMatrix(A_new);
- osqp_update_A(m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(A_csc.m_vals.size()));
+ osqp_update_A(
+ m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(A_csc.m_vals.size()));
return;
}
void OSQPInterface::updateCscA(const CSC_Matrix & A_csc)
{
- osqp_update_A(m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(A_csc.m_vals.size()));
+ osqp_update_A(
+ m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast<c_int>(A_csc.m_vals.size()));
}
void OSQPInterface::updateQ(const std::vector<double> & q_new)
@@ -277,7 +280,8 @@ int64_t OSQPInterface::initializeProblem(
return m_exitflag;
}
-std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> OSQPInterface::solve()
+std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t>
+OSQPInterface::solve()
{
// Solve Problem
osqp_solve(m_work.get());
@@ -296,7 +300,8 @@ std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int
// Result tuple
std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> result =
- std::make_tuple(sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration);
+ std::make_tuple(
+ sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration);
m_latest_work_info = *(m_work->info);
@@ -307,7 +312,8 @@ std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int
OSQPInterface::optimize()
{
// Run the solver on the stored problem representation.
- std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> result = solve();
+ std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> result =
+ solve();
return result;
}
@@ -320,7 +326,8 @@ OSQPInterface::optimize(
initializeProblem(P, A, q, l, u);
// Run the solver on the stored problem representation.
- std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> result = solve();
+ std::tuple<std::vector<float64_t>, std::vector<float64_t>, int64_t, int64_t, int64_t> result =
+ solve();
m_work.reset();
m_work_initialized = false;
diff --git a/common/osqp_interface/test/test_csc_matrix_conv.cpp b/common/osqp_interface/test/test_csc_matrix_conv.cpp
index 5e627e6f2447d..d08fce9a4e23d 100644
--- a/common/osqp_interface/test/test_csc_matrix_conv.cpp
+++ b/common/osqp_interface/test/test_csc_matrix_conv.cpp
@@ -12,17 +12,18 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <string>
-#include <tuple>
-#include <vector>
-
#include "eigen3/Eigen/Core"
#include "gtest/gtest.h"
#include "osqp_interface/csc_matrix_conv.hpp"
-TEST(TestCscMatrixConv, Nominal) {
- using autoware::common::osqp::CSC_Matrix;
+#include <string>
+#include <tuple>
+#include <vector>
+
+TEST(TestCscMatrixConv, Nominal)
+{
using autoware::common::osqp::calCSCMatrix;
+ using autoware::common::osqp::CSC_Matrix;
Eigen::MatrixXd rect1(1, 2);
rect1 << 0.0, 1.0;
@@ -38,8 +39,7 @@ TEST(TestCscMatrixConv, Nominal) {
EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1));
Eigen::MatrixXd rect2(2, 4);
- rect2 << 1.0, 0.0, 3.0, 0.0,
- 0.0, 6.0, 7.0, 0.0;
+ rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0;
const CSC_Matrix rect_m2 = calCSCMatrix(rect2);
ASSERT_EQ(rect_m2.m_vals.size(), size_t(4));
@@ -61,12 +61,8 @@ TEST(TestCscMatrixConv, Nominal) {
// Example from http://netlib.org/linalg/html_templates/node92.html
Eigen::MatrixXd square2(6, 6);
- square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0,
- 3.0, 9.0, 0.0, 0.0, 0.0, 3.0,
- 0.0, 7.0, 8.0, 7.0, 0.0, 0.0,
- 3.0, 0.0, 8.0, 7.0, 5.0, 0.0,
- 0.0, 8.0, 0.0, 9.0, 9.0, 13.0,
- 0.0, 4.0, 0.0, 0.0, 2.0, -1.0;
+ square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0,
+ 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0;
const CSC_Matrix square_m2 = calCSCMatrix(square2);
ASSERT_EQ(square_m2.m_vals.size(), size_t(19));
@@ -118,18 +114,16 @@ TEST(TestCscMatrixConv, Nominal) {
EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16));
EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19));
}
-TEST(TestCscMatrixConv, Trapezoidal) {
- using autoware::common::osqp::CSC_Matrix;
+TEST(TestCscMatrixConv, Trapezoidal)
+{
using autoware::common::osqp::calCSCMatrixTrapezoidal;
+ using autoware::common::osqp::CSC_Matrix;
Eigen::MatrixXd square1(2, 2);
Eigen::MatrixXd square2(3, 3);
Eigen::MatrixXd rect1(1, 2);
- square1 << 1.0, 2.0,
- 2.0, 4.0;
- square2 << 0.0, 2.0, 0.0,
- 4.0, 5.0, 6.0,
- 0.0, 0.0, 0.0;
+ square1 << 1.0, 2.0, 2.0, 4.0;
+ square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0;
rect1 << 0.0, 1.0;
const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1);
@@ -169,11 +163,12 @@ TEST(TestCscMatrixConv, Trapezoidal) {
EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)"));
}
}
-TEST(TestCscMatrixConv, Print) {
- using autoware::common::osqp::CSC_Matrix;
- using autoware::common::osqp::printCSCMatrix;
+TEST(TestCscMatrixConv, Print)
+{
using autoware::common::osqp::calCSCMatrix;
using autoware::common::osqp::calCSCMatrixTrapezoidal;
+ using autoware::common::osqp::CSC_Matrix;
+ using autoware::common::osqp::printCSCMatrix;
Eigen::MatrixXd square1(2, 2);
Eigen::MatrixXd rect1(1, 2);
square1 << 1.0, 2.0, 2.0, 4.0;
diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp
index e3cd601c9c6e4..cb375f8cfc566 100644
--- a/common/osqp_interface/test/test_osqp_interface.cpp
+++ b/common/osqp_interface/test/test_osqp_interface.cpp
@@ -12,13 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <tuple>
-#include <vector>
-
#include "eigen3/Eigen/Core"
#include "gtest/gtest.h"
#include "osqp_interface/osqp_interface.hpp"
+#include <tuple>
+#include <vector>
namespace
{
@@ -39,10 +38,11 @@ using autoware::common::osqp::float64_t;
// obj = 1.88
// cppcheck-suppress syntaxError
-TEST(TestOsqpInterface, BasicQp) {
- using autoware::common::osqp::CSC_Matrix;
+TEST(TestOsqpInterface, BasicQp)
+{
using autoware::common::osqp::calCSCMatrix;
using autoware::common::osqp::calCSCMatrixTrapezoidal;
+ using autoware::common::osqp::CSC_Matrix;
auto check_result =
[](const std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> & result) {
@@ -73,15 +73,16 @@ TEST(TestOsqpInterface, BasicQp) {
{
// Define problem during optimization
autoware::common::osqp::OSQPInterface osqp;
- std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result = osqp.optimize(
- P, A, q, l, u);
+ std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result =
+ osqp.optimize(P, A, q, l, u);
check_result(result);
}
{
// Define problem during initialization
autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6);
- std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result = osqp.optimize();
+ std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result =
+ osqp.optimize();
check_result(result);
}
@@ -107,7 +108,8 @@ TEST(TestOsqpInterface, BasicQp) {
CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
CSC_Matrix A_csc = calCSCMatrix(A);
autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6);
- std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result = osqp.optimize();
+ std::tuple<std::vector<float64_t>, std::vector<float64_t>, int, int, int> result =
+ osqp.optimize();
check_result(result);
}
diff --git a/common/time_utils/include/time_utils/stopwatch.hpp b/common/time_utils/include/time_utils/stopwatch.hpp
index afb3c45746161..223e7ab700ac4 100644
--- a/common/time_utils/include/time_utils/stopwatch.hpp
+++ b/common/time_utils/include/time_utils/stopwatch.hpp
@@ -32,11 +32,15 @@ namespace time_utils
namespace detail
{
-template<typename T>
-struct is_duration : public std::false_type {};
+template <typename T>
+struct is_duration : public std::false_type
+{
+};
-template<typename ScalarT, typename DurationT>
-struct is_duration<std::chrono::duration<ScalarT, DurationT>>: public std::true_type {};
+template <typename ScalarT, typename DurationT>
+struct is_duration<std::chrono::duration<ScalarT, DurationT>> : public std::true_type
+{
+};
} // namespace detail
@@ -53,7 +57,7 @@ class TIME_UTILS_PUBLIC Stopwatch
Stopwatch() = default;
/// Restart a running timer.
- inline void restart() noexcept {m_start = Clock::now();}
+ inline void restart() noexcept { m_start = Clock::now(); }
///
/// @brief Get the elapsed time in the specified duration.
@@ -63,7 +67,7 @@ class TIME_UTILS_PUBLIC Stopwatch
/// @return Time elapsed since the start (or a restart) of the timer represented in the
/// provided duration format.
///
- template<typename UnitsT>
+ template <typename UnitsT>
inline UnitsT measure() const noexcept
{
static_assert(
diff --git a/common/time_utils/include/time_utils/time_utils.hpp b/common/time_utils/include/time_utils/time_utils.hpp
index 6dee7947f9eaf..e69fcd1009db1 100644
--- a/common/time_utils/include/time_utils/time_utils.hpp
+++ b/common/time_utils/include/time_utils/time_utils.hpp
@@ -16,9 +16,9 @@
#ifndef TIME_UTILS__TIME_UTILS_HPP_
#define TIME_UTILS__TIME_UTILS_HPP_
-#include <time_utils/visibility_control.hpp>
#include <builtin_interfaces/msg/duration.hpp>
#include <builtin_interfaces/msg/time.hpp>
+#include <time_utils/visibility_control.hpp>
#include <chrono>
@@ -36,13 +36,11 @@ TIME_UTILS_PUBLIC
std::chrono::nanoseconds from_message(builtin_interfaces::msg::Duration dt) noexcept;
/// Standard interpolation
TIME_UTILS_PUBLIC std::chrono::nanoseconds interpolate(
- std::chrono::nanoseconds a,
- std::chrono::nanoseconds b,
- float t) noexcept;
+ std::chrono::nanoseconds a, std::chrono::nanoseconds b, float t) noexcept;
namespace details
{
-template<typename TimeT>
+template <typename TimeT>
TimeT duration_to_msg(std::chrono::nanoseconds dt);
} // namespace details
} // namespace time_utils
diff --git a/common/time_utils/include/time_utils/visibility_control.hpp b/common/time_utils/include/time_utils/visibility_control.hpp
index 7573c99b55c8d..ca9423d8df9ab 100644
--- a/common/time_utils/include/time_utils/visibility_control.hpp
+++ b/common/time_utils/include/time_utils/visibility_control.hpp
@@ -16,21 +16,21 @@
#define TIME_UTILS__VISIBILITY_CONTROL_HPP_
#if defined(__WIN32)
- #if defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS)
- #define TIME_UTILS_PUBLIC __declspec(dllexport)
- #define TIME_UTILS_LOCAL
- #else // defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS)
- #define TIME_UTILS_PUBLIC __declspec(dllimport)
- #define TIME_UTILS_LOCAL
- #endif // defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS)
+#if defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS)
+#define TIME_UTILS_PUBLIC __declspec(dllexport)
+#define TIME_UTILS_LOCAL
+#else // defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS)
+#define TIME_UTILS_PUBLIC __declspec(dllimport)
+#define TIME_UTILS_LOCAL
+#endif // defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS)
#elif defined(__linux__)
- #define TIME_UTILS_PUBLIC __attribute__((visibility("default")))
- #define TIME_UTILS_LOCAL __attribute__((visibility("hidden")))
+#define TIME_UTILS_PUBLIC __attribute__((visibility("default")))
+#define TIME_UTILS_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
- #define TIME_UTILS_PUBLIC __attribute__((visibility("default")))
- #define TIME_UTILS_LOCAL __attribute__((visibility("hidden")))
+#define TIME_UTILS_PUBLIC __attribute__((visibility("default")))
+#define TIME_UTILS_LOCAL __attribute__((visibility("hidden")))
#else // defined(_LINUX)
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif // defined(_WINDOWS)
#endif // TIME_UTILS__VISIBILITY_CONTROL_HPP_
diff --git a/common/time_utils/package.xml b/common/time_utils/package.xml
index 74c81b4238084..dd6e7bafb7f44 100644
--- a/common/time_utils/package.xml
+++ b/common/time_utils/package.xml
@@ -3,9 +3,7 @@
<package format="2">
<name>time_utils</name>
<version>1.0.0</version>
- <description>
- Simple conversion methods to/from std::chrono to simplify algorithm development
- </description>
+ <description>Simple conversion methods to/from std::chrono to simplify algorithm development</description>
<maintainer email="christopherj.ho@gmail.com">Christopher Ho</maintainer>
<license>Apache License 2.0</license>
@@ -19,5 +17,7 @@
<!-- <test_depend>ament_lint_auto</test_depend> -->
<!-- <test_depend>ament_lint_common</test_depend> -->
- <export><build_type>ament_cmake</build_type></export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/common/time_utils/src/time_utils/time_utils.cpp b/common/time_utils/src/time_utils/time_utils.cpp
index aa034b4078796..c8e394b98ece0 100644
--- a/common/time_utils/src/time_utils/time_utils.cpp
+++ b/common/time_utils/src/time_utils/time_utils.cpp
@@ -21,7 +21,7 @@ namespace time_utils
{
namespace details
{
-template<typename TimeT>
+template <typename TimeT>
TimeT duration_to_msg(std::chrono::nanoseconds dt)
{
// Round down seconds
@@ -94,9 +94,7 @@ std::chrono::nanoseconds from_message(builtin_interfaces::msg::Duration dt) noex
////////////////////////////////////////////////////////////////////////////////
std::chrono::nanoseconds interpolate(
- std::chrono::nanoseconds a,
- std::chrono::nanoseconds b,
- float t) noexcept
+ std::chrono::nanoseconds a, std::chrono::nanoseconds b, float t) noexcept
{
// TODO(c.ho) consider long double
const auto t_ = static_cast<double>(std::min(std::max(t, 0.0F), 1.0F));
diff --git a/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md b/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md
index 8e4a9882d5ff0..0c3dcd1fe1bff 100644
--- a/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md
+++ b/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md
@@ -1,9 +1,8 @@
-vehicle_constants_manager {#vehicle-constants-manager-package-design}
-===========
+# vehicle_constants_manager {#vehicle-constants-manager-package-design}
This is the design document for the `vehicle_constants_manager` package.
-# Purpose / Use cases
+## Purpose / Use cases
<!-- Required -->
<!-- Things to consider:
@@ -13,41 +12,42 @@ This library provides a struct for holding vehicle specific constants. It also
provides a helper method to declare vehicle specific constants which have
already been passed into a node and provide a `VehicleConstants` object.
-# Design
+## Design
<!-- Required -->
<!-- Things to consider:
- How does it work? -->
+
Provided `VehicleConstants` struct holds vehicle parameters. Its parameters can
be split in 2 categories:
(The detailed descriptions and units of the variables is in the
`vehicle_constants_manager.hpp` file.)
- Primary Constants
- - wheel_radius
- - wheel_width
- - wheel_base
- - wheel_tread
- - overhang_front
- - overhang_rear
- - overhang_left
- - overhang_right
- - vehicle_height
- - cg_to_rear
- - tire_cornering_stiffness_front_n_per_deg
- - tire_cornering_stiffness_rear_n_per_deg
- - mass_vehicle
- - inertia_yaw_kg_m_2
+ - wheel_radius
+ - wheel_width
+ - wheel_base
+ - wheel_tread
+ - overhang_front
+ - overhang_rear
+ - overhang_left
+ - overhang_right
+ - vehicle_height
+ - cg_to_rear
+ - tire_cornering_stiffness_front_n_per_deg
+ - tire_cornering_stiffness_rear_n_per_deg
+ - mass_vehicle
+ - inertia_yaw_kg_m_2
- Derived Constants
- - cg_to_front
- - vehicle_length
- - vehicle_width
- - offset_longitudinal_min
- - offset_longitudinal_max
- - offset_lateral_min
- - offset_lateral_max
- - offset_height_min
- - offset_height_max
+ - cg_to_front
+ - vehicle_length
+ - vehicle_width
+ - offset_longitudinal_min
+ - offset_longitudinal_max
+ - offset_lateral_min
+ - offset_lateral_max
+ - offset_height_min
+ - offset_height_max
The `VehicleConstants` constructor is initialized with the primary parameters.
@@ -64,6 +64,7 @@ This library assumes the vehicle is defined with Ackermann steering geometry.
`declare_and_get_vehicle_constants` method requires the passed node to have following parameters overridden:
(Pay attention to the `vehicle` namespace)
+
```yaml
vehicle:
wheel_radius:
@@ -82,7 +83,6 @@ vehicle:
inertia_yaw_kg_m_2:
```
-
## Inputs / Outputs / API {#vehicle-constants-manager-package-design-inputs}
<!-- Required -->
@@ -92,10 +92,11 @@ vehicle:
The constructor of `VehicleConstants` takes the primary vehicle constants and
generates the derived parameters.
-`declare_and_get_vehicle_constants` method takes a `rclcpp::Node` object. And
+`declare_and_get_vehicle_constants` method takes a `rclcpp::Node` object. And
returns a `VehicleConstants` object if it succeeds.
Example usage:
+
```cpp
// In the constructor of a node which received primary vehicle parameters from a
// .yaml file or run args.
@@ -105,6 +106,7 @@ auto vehicle_constants = declare_and_get_vehicle_constants(*this);
## Inner-workings / Algorithms
<!-- If applicable -->
+
Not Available.
## Error detection and handling
@@ -117,7 +119,7 @@ It will throw `std::runtime_error` in case certain parameters are negative or
cg_to_rear is larger than wheel_base (to ensure center of gravity is within
front and rear axles.)
-# Security considerations
+## Security considerations
<!-- Required -->
<!-- Things to consider:
@@ -127,19 +129,23 @@ front and rear axles.)
- Information Disclosure (Can data leak?).
- Denial of Service (How do you handle spamming?).
- Elevation of Privilege (Do you need to change permission levels during execution?) -->
+
To Be Determined.
-# References / External links
+## References / External links
<!-- Optional -->
+
Not Available.
-# Future extensions / Unimplemented parts
+## Future extensions / Unimplemented parts
<!-- Optional -->
+
Not Available.
-# Related issues
+## Related issues
<!-- Required -->
-https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1294
+
+<https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1294>
diff --git a/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp b/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp
index 72e53fff0c53f..68ea68ca41d32 100644
--- a/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp
+++ b/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp
@@ -63,13 +63,11 @@ struct VEHICLE_CONSTANTS_MANAGER_PUBLIC VehicleConstants
/// @throws std::runtime_error if cg_to_rear is larger than wheel_base (center of gravity must be
/// within front and rear axles.)
explicit VehicleConstants(
- float64_t wheel_radius, float64_t wheel_width, float64_t wheel_base,
- float64_t wheel_tread, float64_t overhang_front,
- float64_t overhang_rear, float64_t overhang_left,
- float64_t overhang_right, float64_t vehicle_height,
- float64_t cg_to_rear, float64_t tire_cornering_stiffness_front,
- float64_t tire_cornering_stiffness_rear, float64_t mass_vehicle,
- float64_t inertia_yaw_kg_m_2, float64_t maximum_turning_angle_rad);
+ float64_t wheel_radius, float64_t wheel_width, float64_t wheel_base, float64_t wheel_tread,
+ float64_t overhang_front, float64_t overhang_rear, float64_t overhang_left,
+ float64_t overhang_right, float64_t vehicle_height, float64_t cg_to_rear,
+ float64_t tire_cornering_stiffness_front, float64_t tire_cornering_stiffness_rear,
+ float64_t mass_vehicle, float64_t inertia_yaw_kg_m_2, float64_t maximum_turning_angle_rad);
// Primary Constants
@@ -127,7 +125,6 @@ struct VEHICLE_CONSTANTS_MANAGER_PUBLIC VehicleConstants
/// @brief [rad] Maximum turning angle for cars front axis
const float64_t maximum_turning_angle_rad;
-
// Derived Constants
/// @brief [m] Absolute value of longitudinal distance between Center of Gravity and center of
diff --git a/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp b/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp
index df84d51780bd2..f16c180a17173 100644
--- a/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp
+++ b/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp
@@ -17,23 +17,23 @@
////////////////////////////////////////////////////////////////////////////////
#if defined(__WIN32)
- #if defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL) || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS)
- #define VEHICLE_CONSTANTS_MANAGER_PUBLIC __declspec(dllexport)
- #define VEHICLE_CONSTANTS_MANAGER_LOCAL
- #else // defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL)
+#if defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL) || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS)
+#define VEHICLE_CONSTANTS_MANAGER_PUBLIC __declspec(dllexport)
+#define VEHICLE_CONSTANTS_MANAGER_LOCAL
+#else // defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL)
// || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS)
- #define VEHICLE_CONSTANTS_MANAGER_PUBLIC __declspec(dllimport)
- #define VEHICLE_CONSTANTS_MANAGER_LOCAL
- #endif // defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL)
+#define VEHICLE_CONSTANTS_MANAGER_PUBLIC __declspec(dllimport)
+#define VEHICLE_CONSTANTS_MANAGER_LOCAL
+#endif // defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL)
// || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS)
#elif defined(__linux__)
- #define VEHICLE_CONSTANTS_MANAGER_PUBLIC __attribute__((visibility("default")))
- #define VEHICLE_CONSTANTS_MANAGER_LOCAL __attribute__((visibility("hidden")))
+#define VEHICLE_CONSTANTS_MANAGER_PUBLIC __attribute__((visibility("default")))
+#define VEHICLE_CONSTANTS_MANAGER_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
- #define VEHICLE_CONSTANTS_MANAGER_PUBLIC __attribute__((visibility("default")))
- #define VEHICLE_CONSTANTS_MANAGER_LOCAL __attribute__((visibility("hidden")))
+#define VEHICLE_CONSTANTS_MANAGER_PUBLIC __attribute__((visibility("default")))
+#define VEHICLE_CONSTANTS_MANAGER_LOCAL __attribute__((visibility("hidden")))
#else
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif
#endif // VEHICLE_CONSTANTS_MANAGER__VISIBILITY_CONTROL_HPP_
diff --git a/common/vehicle_constants_manager/package.xml b/common/vehicle_constants_manager/package.xml
index 97068350b0927..a9a5461bb6c4f 100644
--- a/common/vehicle_constants_manager/package.xml
+++ b/common/vehicle_constants_manager/package.xml
@@ -10,13 +10,11 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_auto_cmake</buildtool_depend>
+ <depend>autoware_auto_common</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
- <depend>autoware_auto_common</depend>
<test_depend>ament_cmake_gtest</test_depend>
- <!-- <test_depend>ament_lint_auto</test_depend>
- <test_depend>ament_lint_common</test_depend> -->
<test_depend>autoware_auto_cmake</test_depend>
<export>
diff --git a/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml b/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml
index 95121d84e0f6e..ea9a6f10d5abc 100644
--- a/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml
+++ b/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml
@@ -14,7 +14,5 @@
tire_cornering_stiffness_front: 0.1 # Taken from AVP demo params, can't verify
tire_cornering_stiffness_rear: 0.1 # Taken from AVP demo params, can't verify
mass_vehicle: 2120.0 # Measured from SVL
- inertia_yaw_kg_m2: 12.0 # Taken from AVP demo params, can't verify
+ inertia_yaw_kg_m2: 12.0 # Taken from AVP demo params, can't verify
maximum_turning_angle_rad: 0.53
-
-
diff --git a/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp b/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp
index e895194e098a5..97ea7ae5dd4eb 100644
--- a/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp
+++ b/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp
@@ -13,6 +13,7 @@
// limitations under the License.
#include "vehicle_constants_manager/vehicle_constants_manager.hpp"
+
#include <rclcpp/rclcpp.hpp>
#include <map>
@@ -29,23 +30,12 @@ namespace vehicle_constants_manager
using float64_t = VehicleConstants::float64_t;
-
VehicleConstants::VehicleConstants(
- float64_t wheel_radius,
- float64_t wheel_width,
- float64_t wheel_base,
- float64_t wheel_tread,
- float64_t overhang_front,
- float64_t overhang_rear,
- float64_t overhang_left,
- float64_t overhang_right,
- float64_t vehicle_height,
- float64_t cg_to_rear,
- float64_t tire_cornering_stiffness_front,
- float64_t tire_cornering_stiffness_rear,
- float64_t mass_vehicle,
- float64_t inertia_yaw_kg_m_2,
- float64_t maximum_turning_angle_rad)
+ float64_t wheel_radius, float64_t wheel_width, float64_t wheel_base, float64_t wheel_tread,
+ float64_t overhang_front, float64_t overhang_rear, float64_t overhang_left,
+ float64_t overhang_right, float64_t vehicle_height, float64_t cg_to_rear,
+ float64_t tire_cornering_stiffness_front, float64_t tire_cornering_stiffness_rear,
+ float64_t mass_vehicle, float64_t inertia_yaw_kg_m_2, float64_t maximum_turning_angle_rad)
: wheel_radius(wheel_radius),
wheel_width(wheel_width),
wheel_base(wheel_base),
@@ -79,12 +69,10 @@ VehicleConstants::VehicleConstants(
// These values must be positive
auto throw_if_negative = [](float64_t number, const std::string & name) {
- if (number < 0.0) {
- throw std::runtime_error(
- name + " = " + std::to_string(number) +
- " shouldn't be negative.");
- }
- };
+ if (number < 0.0) {
+ throw std::runtime_error(name + " = " + std::to_string(number) + " shouldn't be negative.");
+ }
+ };
throw_if_negative(wheel_radius, "wheel_radius");
throw_if_negative(wheel_width, "wheel_width");
throw_if_negative(wheel_base, "wheel_base");
@@ -103,7 +91,7 @@ VehicleConstants::VehicleConstants(
if (!(0.0 < maximum_turning_angle_rad && maximum_turning_angle_rad < (M_PI / 2.0))) {
throw std::runtime_error(
- "maximum_turning_angle_rad must be positive and cannot be greater than 0.5*PI.");
+ "maximum_turning_angle_rad must be positive and cannot be greater than 0.5*PI.");
}
minimum_turning_radius = wheel_base / tan(maximum_turning_angle_rad);
}
@@ -111,32 +99,79 @@ VehicleConstants::VehicleConstants(
std::string VehicleConstants::str_pretty() const
{
return std::string{
- "wheel_radius: " + std::to_string(wheel_radius) + "\n"
- "wheel_width: " + std::to_string(wheel_width) + "\n"
- "wheel_base: " + std::to_string(wheel_base) + "\n"
- "wheel_tread: " + std::to_string(wheel_tread) + "\n"
- "overhang_front: " + std::to_string(overhang_front) + "\n"
- "overhang_rear: " + std::to_string(overhang_rear) + "\n"
- "overhang_left: " + std::to_string(overhang_left) + "\n"
- "overhang_right: " + std::to_string(overhang_right) + "\n"
- "vehicle_height: " + std::to_string(vehicle_height) + "\n"
- "cg_to_rear: " + std::to_string(cg_to_rear) + "\n"
- "tire_cornering_stiffness_front: " + std::to_string(tire_cornering_stiffness_front) + "\n"
- "tire_cornering_stiffness_rear: " + std::to_string(tire_cornering_stiffness_rear) + "\n"
- "mass_vehicle: " + std::to_string(mass_vehicle) + "\n"
- "inertia_yaw_kg_m2: " + std::to_string(inertia_yaw_kg_m2) + "\n"
- "maximum_turning_angle_rad: " + std::to_string(maximum_turning_angle_rad) + "\n"
- "cg_to_front: " + std::to_string(cg_to_front) + "\n"
- "vehicle_length: " + std::to_string(vehicle_length) + "\n"
- "vehicle_width: " + std::to_string(vehicle_width) + "\n"
- "offset_longitudinal_min: " + std::to_string(offset_longitudinal_min) + "\n"
- "offset_longitudinal_max: " + std::to_string(offset_longitudinal_max) + "\n"
- "offset_lateral_min: " + std::to_string(offset_lateral_min) + "\n"
- "offset_lateral_max: " + std::to_string(offset_lateral_max) + "\n"
- "offset_height_min: " + std::to_string(offset_height_min) + "\n"
- "offset_height_max: " + std::to_string(offset_height_max) + "\n"
- "minimum_turning_radius: " + std::to_string(minimum_turning_radius) + "\n"
- };
+ "wheel_radius: " + std::to_string(wheel_radius) +
+ "\n"
+ "wheel_width: " +
+ std::to_string(wheel_width) +
+ "\n"
+ "wheel_base: " +
+ std::to_string(wheel_base) +
+ "\n"
+ "wheel_tread: " +
+ std::to_string(wheel_tread) +
+ "\n"
+ "overhang_front: " +
+ std::to_string(overhang_front) +
+ "\n"
+ "overhang_rear: " +
+ std::to_string(overhang_rear) +
+ "\n"
+ "overhang_left: " +
+ std::to_string(overhang_left) +
+ "\n"
+ "overhang_right: " +
+ std::to_string(overhang_right) +
+ "\n"
+ "vehicle_height: " +
+ std::to_string(vehicle_height) +
+ "\n"
+ "cg_to_rear: " +
+ std::to_string(cg_to_rear) +
+ "\n"
+ "tire_cornering_stiffness_front: " +
+ std::to_string(tire_cornering_stiffness_front) +
+ "\n"
+ "tire_cornering_stiffness_rear: " +
+ std::to_string(tire_cornering_stiffness_rear) +
+ "\n"
+ "mass_vehicle: " +
+ std::to_string(mass_vehicle) +
+ "\n"
+ "inertia_yaw_kg_m2: " +
+ std::to_string(inertia_yaw_kg_m2) +
+ "\n"
+ "maximum_turning_angle_rad: " +
+ std::to_string(maximum_turning_angle_rad) +
+ "\n"
+ "cg_to_front: " +
+ std::to_string(cg_to_front) +
+ "\n"
+ "vehicle_length: " +
+ std::to_string(vehicle_length) +
+ "\n"
+ "vehicle_width: " +
+ std::to_string(vehicle_width) +
+ "\n"
+ "offset_longitudinal_min: " +
+ std::to_string(offset_longitudinal_min) +
+ "\n"
+ "offset_longitudinal_max: " +
+ std::to_string(offset_longitudinal_max) +
+ "\n"
+ "offset_lateral_min: " +
+ std::to_string(offset_lateral_min) +
+ "\n"
+ "offset_lateral_max: " +
+ std::to_string(offset_lateral_max) +
+ "\n"
+ "offset_height_min: " +
+ std::to_string(offset_height_min) +
+ "\n"
+ "offset_height_max: " +
+ std::to_string(offset_height_max) +
+ "\n"
+ "minimum_turning_radius: " +
+ std::to_string(minimum_turning_radius) + "\n"};
}
VehicleConstants declare_and_get_vehicle_constants(rclcpp::Node & node)
@@ -158,8 +193,7 @@ VehicleConstants declare_and_get_vehicle_constants(rclcpp::Node & node)
std::make_pair(ns + "tire_cornering_stiffness_rear", -1.0),
std::make_pair(ns + "mass_vehicle", -1.0),
std::make_pair(ns + "inertia_yaw_kg_m2", -1.0),
- std::make_pair(ns + "maximum_turning_angle_rad", -1.0)
- };
+ std::make_pair(ns + "maximum_turning_angle_rad", -1.0)};
// Try to get parameter values from parameter_overrides set either from .yaml
// or with args.
@@ -170,26 +204,18 @@ VehicleConstants declare_and_get_vehicle_constants(rclcpp::Node & node)
continue;
}
- pair.second = node.declare_parameter(pair.first, rclcpp::ParameterType::PARAMETER_DOUBLE).get<float64_t>();
+ pair.second =
+ node.declare_parameter(pair.first, rclcpp::ParameterType::PARAMETER_DOUBLE).get<float64_t>();
}
return VehicleConstants(
- params.at(ns + "wheel_radius"),
- params.at(ns + "wheel_width"),
- params.at(ns + "wheel_base"),
- params.at(ns + "wheel_tread"),
- params.at(ns + "overhang_front"),
- params.at(ns + "overhang_rear"),
- params.at(ns + "overhang_left"),
- params.at(ns + "overhang_right"),
- params.at(ns + "vehicle_height"),
- params.at(ns + "cg_to_rear"),
- params.at(ns + "tire_cornering_stiffness_front"),
- params.at(ns + "tire_cornering_stiffness_rear"),
- params.at(ns + "mass_vehicle"),
- params.at(ns + "inertia_yaw_kg_m2"),
- params.at(ns + "maximum_turning_angle_rad")
- );
+ params.at(ns + "wheel_radius"), params.at(ns + "wheel_width"), params.at(ns + "wheel_base"),
+ params.at(ns + "wheel_tread"), params.at(ns + "overhang_front"),
+ params.at(ns + "overhang_rear"), params.at(ns + "overhang_left"),
+ params.at(ns + "overhang_right"), params.at(ns + "vehicle_height"),
+ params.at(ns + "cg_to_rear"), params.at(ns + "tire_cornering_stiffness_front"),
+ params.at(ns + "tire_cornering_stiffness_rear"), params.at(ns + "mass_vehicle"),
+ params.at(ns + "inertia_yaw_kg_m2"), params.at(ns + "maximum_turning_angle_rad"));
}
} // namespace vehicle_constants_manager
} // namespace common
diff --git a/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp b/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp
index b57c362c9d588..00f34b0bfeb81 100644
--- a/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp
+++ b/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp
@@ -12,18 +12,18 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "gtest/gtest.h"
+#include "vehicle_constants_manager/vehicle_constants_manager.hpp"
+
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>
-#include "vehicle_constants_manager/vehicle_constants_manager.hpp"
-#include "gtest/gtest.h"
-
-TEST(TestVehicleConstantsManager, TestInitializationConstructor) {
+TEST(TestVehicleConstantsManager, TestInitializationConstructor)
+{
using float64_t = autoware::common::types::float64_t;
- using VehicleConstants =
- autoware::common::vehicle_constants_manager::VehicleConstants;
+ using VehicleConstants = autoware::common::vehicle_constants_manager::VehicleConstants;
const float64_t wheel_radius = 0.37;
const float64_t wheel_width = 0.27;
@@ -42,54 +42,45 @@ TEST(TestVehicleConstantsManager, TestInitializationConstructor) {
const float64_t maximum_turning_angle_rad = 0.53;
// Well set parameters
- EXPECT_NO_THROW(
- VehicleConstants vc(
- wheel_radius, wheel_width, wheel_base, wheel_tread, overhang_front,
- overhang_rear, overhang_left, overhang_right, vehicle_height, cg_to_rear,
- tire_cornering_stiffness_front_n_per_deg,
- tire_cornering_stiffness_rear_n_per_deg, mass_vehicle,
- inertia_yaw_kg_m_2, maximum_turning_angle_rad));
+ EXPECT_NO_THROW(VehicleConstants vc(
+ wheel_radius, wheel_width, wheel_base, wheel_tread, overhang_front, overhang_rear,
+ overhang_left, overhang_right, vehicle_height, cg_to_rear,
+ tire_cornering_stiffness_front_n_per_deg, tire_cornering_stiffness_rear_n_per_deg, mass_vehicle,
+ inertia_yaw_kg_m_2, maximum_turning_angle_rad));
// Center of gravity is not within wheel_base
const float64_t bad_cg_to_rear = wheel_base + 1.0;
EXPECT_THROW(
VehicleConstants vc(
- wheel_radius, wheel_width, wheel_base,
- wheel_tread, overhang_front, overhang_rear,
- overhang_left, overhang_right,
- vehicle_height, bad_cg_to_rear,
- tire_cornering_stiffness_front_n_per_deg,
- tire_cornering_stiffness_rear_n_per_deg,
- mass_vehicle, inertia_yaw_kg_m_2,
- maximum_turning_angle_rad),
+ wheel_radius, wheel_width, wheel_base, wheel_tread, overhang_front, overhang_rear,
+ overhang_left, overhang_right, vehicle_height, bad_cg_to_rear,
+ tire_cornering_stiffness_front_n_per_deg, tire_cornering_stiffness_rear_n_per_deg,
+ mass_vehicle, inertia_yaw_kg_m_2, maximum_turning_angle_rad),
std::runtime_error);
// Some supposedly absolute parameters are negative
EXPECT_THROW(
VehicleConstants vc(
- wheel_radius, wheel_width, -wheel_base,
- wheel_tread, -overhang_front, overhang_rear,
- overhang_left, -overhang_right,
- vehicle_height, cg_to_rear,
- tire_cornering_stiffness_front_n_per_deg,
- tire_cornering_stiffness_rear_n_per_deg,
- mass_vehicle, inertia_yaw_kg_m_2,
- maximum_turning_angle_rad),
+ wheel_radius, wheel_width, -wheel_base, wheel_tread, -overhang_front, overhang_rear,
+ overhang_left, -overhang_right, vehicle_height, cg_to_rear,
+ tire_cornering_stiffness_front_n_per_deg, tire_cornering_stiffness_rear_n_per_deg,
+ mass_vehicle, inertia_yaw_kg_m_2, maximum_turning_angle_rad),
std::runtime_error);
}
-TEST(TestVehicleConstantsManager, TestGetVehicleConstantsMissing) {
+TEST(TestVehicleConstantsManager, TestGetVehicleConstantsMissing)
+{
rclcpp::init(0, nullptr);
const std::string ns_node = "TestGetVehicleConstantsMissing";
rclcpp::Node node("some_node", ns_node);
EXPECT_THROW(
- autoware::common::vehicle_constants_manager::
- declare_and_get_vehicle_constants(node),
+ autoware::common::vehicle_constants_manager::declare_and_get_vehicle_constants(node),
std::runtime_error);
rclcpp::shutdown();
}
-TEST(TestVehicleConstantsManager, TestGetVehicleConstants) {
+TEST(TestVehicleConstantsManager, TestGetVehicleConstants)
+{
rclcpp::init(0, nullptr);
const std::string ns_node = "TestGetVehicleConstants";
std::vector<rclcpp::Parameter> params;
@@ -117,8 +108,7 @@ TEST(TestVehicleConstantsManager, TestGetVehicleConstants) {
std::make_shared<rclcpp::Node>("some_node", ns_node, node_options);
EXPECT_NO_THROW(
- autoware::common::vehicle_constants_manager::
- declare_and_get_vehicle_constants(*node));
+ autoware::common::vehicle_constants_manager::declare_and_get_vehicle_constants(*node));
rclcpp::shutdown();
}
diff --git a/control/trajectory_follower/design/trajectory_follower-design.md b/control/trajectory_follower/design/trajectory_follower-design.md
index 93a27f168ace8..2a68a6cfe483d 100644
--- a/control/trajectory_follower/design/trajectory_follower-design.md
+++ b/control/trajectory_follower/design/trajectory_follower-design.md
@@ -1,21 +1,24 @@
-Trajectory Follower {#trajectory_follower-package-design}
-===========
+# Trajectory Follower {#trajectory_follower-package-design}
This is the design document for the `trajectory_follower` package.
+## Purpose / Use cases
-# Purpose / Use cases
<!-- Required -->
<!-- Things to consider:
- Why did we implement this feature? -->
+
This package provides the library code used by the nodes of the `trajectory_follower_nodes` package.
Mainly, it implements two algorithms:
+
- Model-Predictive Control (MPC) for the computation of lateral steering commands.
- - @subpage trajectory_follower-mpc-design
+ - @subpage trajectory_follower-mpc-design
- PID control for the computation of velocity and acceleration commands.
- - @subpage trajectory_follower-pid-design
+ - @subpage trajectory_follower-pid-design
+
+## Related issues
-# Related issues
<!-- Required -->
-- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057
-- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058
+
+- <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057>
+- <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058>
diff --git a/control/trajectory_follower/design/trajectory_follower-mpc-design.md b/control/trajectory_follower/design/trajectory_follower-mpc-design.md
index b0dc982fe099e..8ed633037d1b1 100644
--- a/control/trajectory_follower/design/trajectory_follower-mpc-design.md
+++ b/control/trajectory_follower/design/trajectory_follower-mpc-design.md
@@ -1,37 +1,42 @@
-MPC (Trajectory Follower) {#trajectory_follower-mpc-design}
-===========
+# MPC (Trajectory Follower) {#trajectory_follower-mpc-design}
This is the design document for the MPC implemented in the `trajectory_follower` package.
-# Purpose / Use cases
+## Purpose / Use cases
+
<!-- Required -->
<!-- Things to consider:
- Why did we implement this feature? -->
+
Model Predictive Control (MPC) is used by the `trajectory_follower`
to calculate the lateral commands corresponding to a steering angle and a steering rate.
This implementation differs from the one in the `mpc_controller` package in several aspects.
+
- This is a linear MPC that only computes the steering command whereas
-the `mpc_controller` uses a non-linear MPC which calculates coupled steering and velocity commands.
+ the `mpc_controller` uses a non-linear MPC which calculates coupled steering and velocity commands.
- The optimization problem solved by the linear MPC is simpler, making it less likely to fail.
- Tuning of the linear MPC is easier.
-# Design
+## Design
+
<!-- Required -->
<!-- Things to consider:
- How does it work? -->
+
MPC uses predictions of the vehicle's motion to optimize the immediate control command.
Different vehicle models are implemented:
+
- `kinematics` : bicycle kinematics model with steering 1st-order delay.
- `kinematics_no_delay` : bicycle kinematics model without steering delay.
- `dynamics` : bicycle dynamics model considering slip angle.
The `kinematics` model is being used by default. Please see the reference [1] for more details.
-
For the optimization, a Quadratric Programming (QP) solver is used
with two options are currently implemented:
+
- `unconstraint` : use least square method to solve unconstraint QP with eigen.
- `unconstraint_fast` : similar to unconstraint. This is faster, but lower accuracy for optimization.
@@ -46,20 +51,26 @@ The moving average filter for example is not suited and can yield worse results
filtering.
## Inputs / Outputs / API
+
<!-- Required -->
<!-- Things to consider:
- How do you use the package / API? -->
+
The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm.
Once a vehicle model, a QP solver, and the reference trajectory to follow have been set
(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command
can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`.
-# References / External links
+## References / External links
+
<!-- Optional -->
+
- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking",
-Robotics Institute, Carnegie Mellon University, February 2009.
+ Robotics Institute, Carnegie Mellon University, February 2009.
+
+## Related issues
-# Related issues
<!-- Required -->
-- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057
-- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058
+
+- <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057>
+- <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058>
diff --git a/control/trajectory_follower/design/trajectory_follower-pid-design.md b/control/trajectory_follower/design/trajectory_follower-pid-design.md
index de6df3a4ed527..869747bde2879 100644
--- a/control/trajectory_follower/design/trajectory_follower-pid-design.md
+++ b/control/trajectory_follower/design/trajectory_follower-pid-design.md
@@ -1,19 +1,22 @@
-PID (Trajectory Follower) {#trajectory_follower-pid-design}
-===========
+# PID (Trajectory Follower) {#trajectory_follower-pid-design}
This is the design document for the PID implemented in the `trajectory_follower` package.
-# Purpose / Use cases
+## Purpose / Use cases
+
<!-- Required -->
<!-- Things to consider:
- Why did we implement this feature? -->
+
PID control is used by the `trajectory_follower`
to calculate longitudinal commands corresponding to a velocity and an acceleration.
-# Design
+## Design
+
<!-- Required -->
<!-- Things to consider:
- How does it work? -->
+
This PID control calculates the target acceleration from the deviation between the current ego-velocity and the target velocity.
This PID logic has a maximum value for the output of each term. This is to prevent the following:
@@ -29,7 +32,7 @@ This may be replaced by a higher performance controller (adaptive control or rob
@image html images/trajectory_follower-pid-diagram.svg "PID controller diagram"
-## States
+### States
This module has four state transitions as shown below in order to handle special processing in a specific situation.
@@ -49,7 +52,7 @@ The state transition diagram is shown below.
@image html images/trajectory_follower-pid-states-diagram.svg "State Transitions"
-## Time delay compensation
+### Time delay compensation
At high speeds, the delay of actuator systems such as gas pedals and brakes has a significant impact on driving accuracy.
Depending on the actuating principle of the vehicle,
@@ -58,7 +61,7 @@ the mechanism that physically controls the gas pedal and brake typically has a d
In this controller,
the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem.
-## Slope compensation
+### Slope compensation
Based on the slope information, a compensation term is added to the target acceleration.
@@ -75,14 +78,18 @@ There are two sources of the slope information, which can be switched by a param
- Cons: z-coordinates of high-precision map is needed.
- Cons: Does not support free space planning (for now)
-## Inputs / Outputs / API
+### Inputs / Outputs / API
+
<!-- Required -->
<!-- Things to consider:
- How do you use the package / API? -->
+
The `PIDController` class is straightforward to use.
First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components.
Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function.
-# Related issues
+## Related issues
+
<!-- Required -->
-- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058
+
+- <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058>
diff --git a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp b/control/trajectory_follower/include/trajectory_follower/debug_values.hpp
index 61ba471b0c8bd..ab6e7e04998e7 100644
--- a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/debug_values.hpp
@@ -15,11 +15,11 @@
#ifndef TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_
#define TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_
-#include <array>
-
#include "common/types.hpp"
#include "trajectory_follower/visibility_control.hpp"
+#include <array>
+
namespace autoware
{
namespace motion
@@ -34,8 +34,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues
{
public:
/// Types of debug values
- enum class TYPE
- {
+ enum class TYPE {
DT = 0,
CURRENT_VEL = 1,
TARGET_VEL = 2,
@@ -73,12 +72,12 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues
* @param [in] type the TYPE enum for which to get the index
* @return index of the type
*/
- size_t getValuesIdx(const TYPE type) const {return static_cast<size_t>(type);}
+ size_t getValuesIdx(const TYPE type) const { return static_cast<size_t>(type); }
/**
* @brief get all the debug values as an std::array
* @return array of all debug values
*/
- std::array<float64_t, static_cast<size_t>(TYPE::SIZE)> getValues() const {return m_values;}
+ std::array<float64_t, static_cast<size_t>(TYPE::SIZE)> getValues() const { return m_values; }
/**
* @brief set the given type to the given value
* @param [in] type TYPE of the value
@@ -93,7 +92,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues
* @param [in] type index of the type
* @param [in] value value to set
*/
- void setValues(const size_t type, const float64_t value) {m_values.at(type) = value;}
+ void setValues(const size_t type, const float64_t value) { m_values.at(type) = value; }
private:
std::array<float64_t, static_cast<size_t>(TYPE::SIZE)> m_values;
diff --git a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp b/control/trajectory_follower/include/trajectory_follower/interpolate.hpp
index c01177cbe7581..45277f912502c 100644
--- a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/interpolate.hpp
@@ -15,13 +15,13 @@
#ifndef TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_
#define TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_
+#include "common/types.hpp"
+#include "trajectory_follower/visibility_control.hpp"
+
#include <cmath>
#include <iostream>
#include <vector>
-#include "common/types.hpp"
-#include "trajectory_follower/visibility_control.hpp"
-
namespace autoware
{
namespace motion
@@ -30,8 +30,8 @@ namespace control
{
namespace trajectory_follower
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
/**
* @brief linearly interpolate the given values assuming a base indexing and a new desired indexing
* @param [in] base_index indexes for each base value
diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp
index 854f85b758f74..010fba8272cea 100644
--- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp
@@ -15,22 +15,23 @@
#ifndef TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_
#define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_
-#include <cmath>
-#include <experimental/optional> // NOLINT
-#include <limits>
-
+#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Geometry"
-
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "common/types.hpp"
-#include "geometry_msgs/msg/pose.hpp"
#include "geometry/common_2d.hpp"
#include "motion_common/motion_common.hpp"
#include "motion_common/trajectory_common.hpp"
#include "tf2/utils.h"
#include "trajectory_follower/visibility_control.hpp"
+#include <experimental/optional> // NOLINT
+
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+
+#include <cmath>
+#include <limits>
+
namespace autoware
{
namespace motion
@@ -41,8 +42,8 @@ namespace trajectory_follower
{
namespace longitudinal_utils
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using geometry_msgs::msg::Point;
@@ -59,9 +60,8 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj);
/**
* @brief calculate distance to stopline from current vehicle position where velocity is 0
*/
-TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance(
- const Point & current_pos,
- const Trajectory & traj);
+TRAJECTORY_FOLLOWER_PUBLIC float64_t
+calcStopDistance(const Point & current_pos, const Trajectory & traj);
/**
* @brief calculate pitch angle from estimated current pose
@@ -75,19 +75,18 @@ TRAJECTORY_FOLLOWER_PUBLIC float64_t getPitchByPose(const Quaternion & quaternio
* @param [in] closest_idx nearest index to current vehicle position
* @param [in] wheel_base length of wheel base
*/
-TRAJECTORY_FOLLOWER_PUBLIC float64_t getPitchByTraj(
- const Trajectory & trajectory, const size_t closest_idx,
- const float64_t wheel_base);
+TRAJECTORY_FOLLOWER_PUBLIC float64_t
+getPitchByTraj(const Trajectory & trajectory, const size_t closest_idx, const float64_t wheel_base);
/**
* @brief calculate elevation angle
*/
-TRAJECTORY_FOLLOWER_PUBLIC float64_t calcElevationAngle(
- const TrajectoryPoint & p_from,
- const TrajectoryPoint & p_to);
+TRAJECTORY_FOLLOWER_PUBLIC float64_t
+calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to);
/**
- * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for delayed time
+ * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for
+ * delayed time
*/
TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay(
const Pose & current_pose, const float64_t delay_time, const float64_t current_vel);
@@ -98,19 +97,17 @@ TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay(
* @param [in] o_to second orientation
* @param [in] ratio ratio between o_from and o_to for interpolation
*/
-TRAJECTORY_FOLLOWER_PUBLIC Quaternion lerpOrientation(
- const Quaternion & o_from,
- const Quaternion & o_to,
- const float64_t ratio);
+TRAJECTORY_FOLLOWER_PUBLIC Quaternion
+lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float64_t ratio);
/**
* @brief apply linear interpolation to trajectory point that is nearest to a certain point
* @param [in] points trajectory points
* @param [in] point Interpolated point is nearest to this point.
*/
-template<class T>
-TRAJECTORY_FOLLOWER_PUBLIC
-TrajectoryPoint lerpTrajectoryPoint(const T & points, const Point & point)
+template <class T>
+TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint
+lerpTrajectoryPoint(const T & points, const Point & point)
{
TrajectoryPoint interpolated_point;
@@ -126,29 +123,20 @@ TrajectoryPoint lerpTrajectoryPoint(const T & points, const Point & point)
const size_t i = nearest_seg_idx;
interpolated_point.pose.position.x = motion_common::interpolate(
- points.at(i).pose.position.x, points.at(
- i + 1).pose.position.x, interpolate_ratio);
+ points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);
interpolated_point.pose.position.y = motion_common::interpolate(
- points.at(i).pose.position.y, points.at(
- i + 1).pose.position.y, interpolate_ratio);
+ points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio);
interpolated_point.pose.orientation = lerpOrientation(
points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
- interpolated_point.longitudinal_velocity_mps =
- motion_common::interpolate(
- points.at(i).longitudinal_velocity_mps, points.at(
- i + 1).longitudinal_velocity_mps, interpolate_ratio);
- interpolated_point.lateral_velocity_mps =
- motion_common::interpolate(
- points.at(i).lateral_velocity_mps, points.at(
- i + 1).lateral_velocity_mps, interpolate_ratio);
- interpolated_point.acceleration_mps2 =
- motion_common::interpolate(
- points.at(i).acceleration_mps2, points.at(
- i + 1).acceleration_mps2, interpolate_ratio);
- interpolated_point.heading_rate_rps =
- motion_common::interpolate(
- points.at(i).heading_rate_rps, points.at(
- i + 1).heading_rate_rps, interpolate_ratio);
+ interpolated_point.longitudinal_velocity_mps = motion_common::interpolate(
+ points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps,
+ interpolate_ratio);
+ interpolated_point.lateral_velocity_mps = motion_common::interpolate(
+ points.at(i).lateral_velocity_mps, points.at(i + 1).lateral_velocity_mps, interpolate_ratio);
+ interpolated_point.acceleration_mps2 = motion_common::interpolate(
+ points.at(i).acceleration_mps2, points.at(i + 1).acceleration_mps2, interpolate_ratio);
+ interpolated_point.heading_rate_rps = motion_common::interpolate(
+ points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio);
}
return interpolated_point;
diff --git a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp b/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp
index a717ddb4bb36b..5e2173158f090 100644
--- a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp
@@ -15,14 +15,14 @@
#ifndef TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_
#define TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_
+#include "common/types.hpp"
+#include "trajectory_follower/visibility_control.hpp"
+
#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>
-#include "common/types.hpp"
-#include "trajectory_follower/visibility_control.hpp"
-
namespace autoware
{
namespace motion
@@ -31,8 +31,8 @@ namespace control
{
namespace trajectory_follower
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
/**
* @brief Simple filter with gain on the first derivative of the value
@@ -49,19 +49,18 @@ class LowpassFilter1d
* @param [in] x initial value
* @param [in] gain first-order gain
*/
- LowpassFilter1d(const float64_t x, const float64_t gain)
- : m_x(x), m_gain(gain) {}
+ LowpassFilter1d(const float64_t x, const float64_t gain) : m_x(x), m_gain(gain) {}
/**
* @brief set the current value of the filter
* @param [in] x new value
*/
- void reset(const float64_t x) {m_x = x;}
+ void reset(const float64_t x) { m_x = x; }
/**
* @brief get the current value of the filter
*/
- float64_t getValue() const {return m_x;}
+ float64_t getValue() const { return m_x; }
/**
* @brief filter a new value
@@ -75,7 +74,6 @@ class LowpassFilter1d
}
};
-
/**
* @brief 2nd-order Butterworth Filter
* reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930.
diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp
index 91a4a2e39552e..4d8ee20db8c1b 100644
--- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/mpc.hpp
@@ -15,14 +15,15 @@
#ifndef TRAJECTORY_FOLLOWER__MPC_HPP_
#define TRAJECTORY_FOLLOWER__MPC_HPP_
-#include <deque>
-#include <memory>
-#include <string>
-#include <vector>
-
+#include "common/types.hpp"
+#include "geometry/common_2d.hpp"
+#include "helper_functions/angle_utils.hpp"
+#include "motion_common/motion_common.hpp"
+#include "osqp_interface/osqp_interface.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
-
#include "trajectory_follower/interpolate.hpp"
#include "trajectory_follower/lowpass_filter.hpp"
#include "trajectory_follower/mpc_trajectory.hpp"
@@ -35,17 +36,15 @@
#include "trajectory_follower/visibility_control.hpp"
#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
-#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
-#include "common/types.hpp"
-#include "geometry/common_2d.hpp"
#include "geometry_msgs/msg/pose.hpp"
-#include "helper_functions/angle_utils.hpp"
-#include "motion_common/motion_common.hpp"
-#include "osqp_interface/osqp_interface.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_components/register_node_macro.hpp"
+
+#include <deque>
+#include <memory>
+#include <string>
+#include <vector>
namespace autoware
{
@@ -55,63 +54,63 @@ namespace control
{
namespace trajectory_follower
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
struct MPCParam
{
-//!< @brief prediction horizon step
+ //!< @brief prediction horizon step
int64_t prediction_horizon;
-//!< @brief prediction horizon sampling time
+ //!< @brief prediction horizon sampling time
float64_t prediction_dt;
-//!< @brief threshold that feed-forward angle becomes zero
+ //!< @brief threshold that feed-forward angle becomes zero
float64_t zero_ff_steer_deg;
-//!< @brief delay time for steering input to be compensated
+ //!< @brief delay time for steering input to be compensated
float64_t input_delay;
-//!< @brief for trajectory velocity calculation
+ //!< @brief for trajectory velocity calculation
float64_t acceleration_limit;
-//!< @brief for trajectory velocity calculation
+ //!< @brief for trajectory velocity calculation
float64_t velocity_time_constant;
-//!< @brief time constant for steer model
+ //!< @brief time constant for steer model
float64_t steer_tau;
-// for weight matrix Q
-//!< @brief lateral error weight
+ // for weight matrix Q
+ //!< @brief lateral error weight
float64_t weight_lat_error;
-//!< @brief heading error weight
+ //!< @brief heading error weight
float64_t weight_heading_error;
-//!< @brief heading error * velocity weight
+ //!< @brief heading error * velocity weight
float64_t weight_heading_error_squared_vel;
-//!< @brief terminal lateral error weight
+ //!< @brief terminal lateral error weight
float64_t weight_terminal_lat_error;
-//!< @brief terminal heading error weight
+ //!< @brief terminal heading error weight
float64_t weight_terminal_heading_error;
-//!< @brief lateral error weight in matrix Q in low curvature point
+ //!< @brief lateral error weight in matrix Q in low curvature point
float64_t low_curvature_weight_lat_error;
-//!< @brief heading error weight in matrix Q in low curvature point
+ //!< @brief heading error weight in matrix Q in low curvature point
float64_t low_curvature_weight_heading_error;
-//!< @brief heading error * velocity weight in matrix Q in low curvature point
+ //!< @brief heading error * velocity weight in matrix Q in low curvature point
float64_t low_curvature_weight_heading_error_squared_vel;
-// for weight matrix R
-//!< @brief steering error weight
+ // for weight matrix R
+ //!< @brief steering error weight
float64_t weight_steering_input;
-//!< @brief steering error * velocity weight
+ //!< @brief steering error * velocity weight
float64_t weight_steering_input_squared_vel;
-//!< @brief lateral jerk weight
+ //!< @brief lateral jerk weight
float64_t weight_lat_jerk;
-//!< @brief steering rate weight
+ //!< @brief steering rate weight
float64_t weight_steer_rate;
-//!< @brief steering angle acceleration weight
+ //!< @brief steering angle acceleration weight
float64_t weight_steer_acc;
-//!< @brief steering error weight in matrix R in low curvature point
+ //!< @brief steering error weight in matrix R in low curvature point
float64_t low_curvature_weight_steering_input;
-//!< @brief steering error * velocity weight in matrix R in low curvature point
+ //!< @brief steering error * velocity weight in matrix R in low curvature point
float64_t low_curvature_weight_steering_input_squared_vel;
-//!< @brief lateral jerk weight in matrix R in low curvature point
+ //!< @brief lateral jerk weight in matrix R in low curvature point
float64_t low_curvature_weight_lat_jerk;
-//!< @brief steering rate weight in matrix R in low curvature point
+ //!< @brief steering rate weight in matrix R in low curvature point
float64_t low_curvature_weight_steer_rate;
-//!< @brief steering angle acceleration weight in matrix R in low curvature
+ //!< @brief steering angle acceleration weight in matrix R in low curvature
float64_t low_curvature_weight_steer_acc;
-//!< @brief threshold of curvature to use "low curvature" parameter
+ //!< @brief threshold of curvature to use "low curvature" parameter
float64_t low_curvature_thresh_curvature;
};
/**
@@ -187,8 +186,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
bool8_t getData(
const trajectory_follower::MPCTrajectory & traj,
const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer,
- const geometry_msgs::msg::Pose & current_pose,
- MPCData * data);
+ const geometry_msgs::msg::Pose & current_pose, MPCData * data);
/**
* @brief calculate predicted steering
*/
@@ -197,8 +195,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
* @brief get the sum of all steering commands over the given time range
*/
float64_t getSteerCmdSum(
- const rclcpp::Time & t_start, const rclcpp::Time & t_end,
- const float64_t time_constant) const;
+ const rclcpp::Time & t_start, const rclcpp::Time & t_end, const float64_t time_constant) const;
/**
* @brief set the reference trajectory to follow
*/
@@ -274,64 +271,64 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
*/
inline float64_t getWeightLatError(const float64_t curvature)
{
- return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_error :
- m_param.weight_lat_error;
+ return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_error
+ : m_param.weight_lat_error;
}
/**
* @brief return the weight of the heading error for the given curvature
*/
inline float64_t getWeightHeadingError(const float64_t curvature)
{
- return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error :
- m_param.weight_heading_error;
+ return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error
+ : m_param.weight_heading_error;
}
/**
* @brief return the squared velocity weight of the heading error for the given curvature
*/
inline float64_t getWeightHeadingErrorSqVel(const float64_t curvature)
{
- return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error_squared_vel :
- m_param.weight_heading_error_squared_vel;
+ return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error_squared_vel
+ : m_param.weight_heading_error_squared_vel;
}
/**
* @brief return the weight of the steer input for the given curvature
*/
inline float64_t getWeightSteerInput(const float64_t curvature)
{
- return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input :
- m_param.weight_steering_input;
+ return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input
+ : m_param.weight_steering_input;
}
/**
* @brief return the squared velocity weight of the steer input for the given curvature
*/
inline float64_t getWeightSteerInputSqVel(const float64_t curvature)
{
- return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input_squared_vel :
- m_param.weight_steering_input_squared_vel;
+ return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input_squared_vel
+ : m_param.weight_steering_input_squared_vel;
}
/**
* @brief return the weight of the lateral jerk for the given curvature
*/
inline float64_t getWeightLatJerk(const float64_t curvature)
{
- return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_jerk :
- m_param.weight_lat_jerk;
+ return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_jerk
+ : m_param.weight_lat_jerk;
}
/**
* @brief return the weight of the steering rate for the given curvature
*/
inline float64_t getWeightSteerRate(const float64_t curvature)
{
- return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_rate :
- m_param.weight_steer_rate;
+ return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_rate
+ : m_param.weight_steer_rate;
}
/**
* @brief return the weight of the steering acceleration for the given curvature
*/
inline float64_t getWeightSteerAcc(const float64_t curvature)
{
- return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_acc :
- m_param.weight_steer_acc;
+ return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_acc
+ : m_param.weight_steer_acc;
}
public:
@@ -373,21 +370,17 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
*/
bool8_t calculateMPC(
const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer,
- const float64_t current_velocity,
- const geometry_msgs::msg::Pose & current_pose,
+ const float64_t current_velocity, const geometry_msgs::msg::Pose & current_pose,
autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd,
autoware_auto_planning_msgs::msg::Trajectory & predicted_traj,
- autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic
- );
+ autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic);
/**
* @brief set the reference trajectory to follow
*/
void setReferenceTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
- const float64_t traj_resample_dist,
- const bool8_t enable_path_smoothing,
- const int64_t path_filter_moving_ave_num,
- const int64_t curvature_smoothing_num_traj,
+ const float64_t traj_resample_dist, const bool8_t enable_path_smoothing,
+ const int64_t path_filter_moving_ave_num, const int64_t curvature_smoothing_num_traj,
const int64_t curvature_smoothing_num_ref_steer,
const geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr);
/**
@@ -411,8 +404,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
* @brief initialize low pass filters
*/
inline void initializeLowPassFilters(
- const float64_t steering_lpf_cutoff_hz,
- const float64_t error_deriv_lpf_cutoff_hz)
+ const float64_t steering_lpf_cutoff_hz, const float64_t error_deriv_lpf_cutoff_hz)
{
m_lpf_steering_cmd.initialize(m_ctrl_period, steering_lpf_cutoff_hz);
m_lpf_lateral_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz);
@@ -421,31 +413,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
/**
* @brief return true if the vehicle model of this MPC is set
*/
- inline bool8_t hasVehicleModel() const
- {
- return m_vehicle_model_ptr != nullptr;
- }
+ inline bool8_t hasVehicleModel() const { return m_vehicle_model_ptr != nullptr; }
/**
* @brief return true if the QP solver of this MPC is set
*/
- inline bool8_t hasQPSolver() const
- {
- return m_qpsolver_ptr != nullptr;
- }
+ inline bool8_t hasQPSolver() const { return m_qpsolver_ptr != nullptr; }
/**
* @brief set the RCLCPP logger to use for logging
*/
- inline void setLogger(rclcpp::Logger logger)
- {
- m_logger = logger;
- }
+ inline void setLogger(rclcpp::Logger logger) { m_logger = logger; }
/**
* @brief set the RCLCPP clock to use for time keeping
*/
- inline void setClock(rclcpp::Clock::SharedPtr clock)
- {
- m_clock = clock;
- }
+ inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; }
}; // class MPC
} // namespace trajectory_follower
} // namespace control
diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp
index 6b11e1085ad17..2e37536ddb129 100644
--- a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp
@@ -15,12 +15,12 @@
#ifndef TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_
#define TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_
-#include <iostream>
-#include <vector>
-
#include "common/types.hpp"
#include "trajectory_follower/visibility_control.hpp"
+#include <iostream>
+#include <vector>
+
namespace autoware
{
namespace motion
@@ -65,10 +65,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory
/**
* @return true if the compensents sizes are all 0 or are inconsistent
*/
- inline bool empty() const
- {
- return size() == 0;
- }
+ inline bool empty() const { return size() == 0; }
};
} // namespace trajectory_follower
} // namespace control
diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp
index ec00273225e3a..37e7a56d7cee2 100644
--- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp
@@ -15,29 +15,28 @@
#ifndef TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_
#define TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_
-#include <cmath>
-#include <string>
-#include <vector>
-
-#include "trajectory_follower/interpolate.hpp"
-#include "trajectory_follower/mpc_trajectory.hpp"
-#include "trajectory_follower/visibility_control.hpp"
-
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "geometry/common_2d.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/twist_stamped.hpp"
#include "helper_functions/angle_utils.hpp"
-#include "interpolation/spline_interpolation.hpp"
#include "interpolation/linear_interpolation.hpp"
+#include "interpolation/spline_interpolation.hpp"
#include "motion_common/motion_common.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/utils.h"
+#include "trajectory_follower/interpolate.hpp"
+#include "trajectory_follower/mpc_trajectory.hpp"
+#include "trajectory_follower/visibility_control.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include <cmath>
+#include <string>
+#include <vector>
namespace autoware
{
@@ -49,8 +48,8 @@ namespace trajectory_follower
{
namespace MPCUtils
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
/**
* @brief convert from yaw to ros-Quaternion
* @param [in] yaw input yaw angle
@@ -103,7 +102,8 @@ TRAJECTORY_FOLLOWER_PUBLIC void calcMPCTrajectoryArclength(
TRAJECTORY_FOLLOWER_PUBLIC bool8_t resampleMPCTrajectoryByDistance(
const MPCTrajectory & input, const float64_t resample_interval_dist, MPCTrajectory * output);
/**
- * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired indexing
+ * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired
+ * indexing
* @param [in] in_index indexes for each trajectory point
* @param [in] in_traj MPCTrajectory to interpolate
* @param [in] out_index desired interpolated indexes
@@ -138,17 +138,19 @@ TRAJECTORY_FOLLOWER_PUBLIC void dynamicSmoothingVelocity(
TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY(
MPCTrajectory * traj, const int64_t nearest_idx, const float64_t ego_yaw);
/**
- * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 points when num = 1)
+ * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3
+ * points when num = 1)
* @param [in] curvature_smoothing_num_traj index distance for 3 points for curvature calculation
- * @param [in] curvature_smoothing_num_ref_steer index distance for 3 points for smoothed curvature calculation
+ * @param [in] curvature_smoothing_num_ref_steer index distance for 3 points for smoothed curvature
+ * calculation
* @param [inout] traj object trajectory
*/
TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcTrajectoryCurvature(
- const size_t curvature_smoothing_num_traj,
- const size_t curvature_smoothing_num_ref_steer,
+ const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer,
MPCTrajectory * traj);
/**
- * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 points when num = 1)
+ * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3
+ * points when num = 1)
* @param [in] curvature_smoothing_num index distance for 3 points for curvature calculation
* @param [in] traj input trajectory
* @return vector of curvatures at each point of the given trajectory
@@ -176,9 +178,8 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcNearestPoseInterp(
* @param [in] self_pose pose for which to search the nearest trajectory point
* @return index of the input trajectory nearest to the pose
*/
-TRAJECTORY_FOLLOWER_PUBLIC int64_t calcNearestIndex(
- const MPCTrajectory & traj,
- const geometry_msgs::msg::Pose & self_pose);
+TRAJECTORY_FOLLOWER_PUBLIC int64_t
+calcNearestIndex(const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose);
/**
* @brief calculate the index of the trajectory point nearest to the given pose
* @param [in] traj trajectory to search for the point nearest to the pose
@@ -192,8 +193,7 @@ TRAJECTORY_FOLLOWER_PUBLIC int64_t calcNearestIndex(
* @brief calculate distance to stopped point
*/
TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance(
- const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory,
- const int64_t origin);
+ const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int64_t origin);
} // namespace MPCUtils
} // namespace trajectory_follower
} // namespace control
diff --git a/control/trajectory_follower/include/trajectory_follower/pid.hpp b/control/trajectory_follower/include/trajectory_follower/pid.hpp
index 1260016da84bf..ca64614c9411f 100644
--- a/control/trajectory_follower/include/trajectory_follower/pid.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/pid.hpp
@@ -15,11 +15,11 @@
#ifndef TRAJECTORY_FOLLOWER__PID_HPP_
#define TRAJECTORY_FOLLOWER__PID_HPP_
-#include <vector>
-
#include "common/types.hpp"
#include "trajectory_follower/visibility_control.hpp"
+#include <vector>
+
namespace autoware
{
namespace motion
@@ -28,8 +28,8 @@ namespace control
{
namespace trajectory_follower
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
/// @brief implementation of a PID controller
class TRAJECTORY_FOLLOWER_PUBLIC PIDController
{
@@ -68,9 +68,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC PIDController
*/
void setLimits(
const float64_t max_ret, const float64_t min_ret, const float64_t max_ret_p,
- const float64_t min_ret_p,
- const float64_t max_ret_i, const float64_t min_ret_i, const float64_t max_ret_d,
- const float64_t min_ret_d);
+ const float64_t min_ret_p, const float64_t max_ret_i, const float64_t min_ret_i,
+ const float64_t max_ret_d, const float64_t min_ret_d);
/**
* @brief reset this PID to its initial state
*/
diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp
index 6d9eef7821f17..ed3365fc89570 100644
--- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp
@@ -15,12 +15,11 @@
#ifndef TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
#define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
-#include "trajectory_follower/qp_solver/qp_solver_interface.hpp"
-
#include "common/types.hpp"
#include "eigen3/Eigen/Dense"
#include "osqp_interface/osqp_interface.hpp"
#include "rclcpp/rclcpp.hpp"
+#include "trajectory_follower/qp_solver/qp_solver_interface.hpp"
#include "trajectory_follower/visibility_control.hpp"
namespace autoware
@@ -31,8 +30,8 @@ namespace control
{
namespace trajectory_follower
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
/// Solver for QP problems using the OSQP library
class TRAJECTORY_FOLLOWER_PUBLIC QPSolverOSQP : public QPSolverInterface
{
diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp
index c52c25dbf2879..f6aa35d514460 100644
--- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp
@@ -15,16 +15,15 @@
#ifndef TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_
#define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_
-#include "trajectory_follower/qp_solver/qp_solver_interface.hpp"
-
-#include <cmath>
-
#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Dense"
#include "eigen3/Eigen/LU"
+#include "trajectory_follower/qp_solver/qp_solver_interface.hpp"
#include "trajectory_follower/visibility_control.hpp"
+#include <cmath>
+
namespace autoware
{
namespace motion
diff --git a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp b/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp
index 5d639cbe23696..ad40de50a35aa 100644
--- a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp
@@ -15,18 +15,18 @@
#ifndef TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_
#define TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_
+#include "common/types.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "trajectory_follower/visibility_control.hpp"
+
+#include <experimental/optional> // NOLINT
+
#include <algorithm>
#include <cmath>
-#include <experimental/optional> // NOLINT
#include <limits>
#include <utility>
#include <vector>
-#include "common/types.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "trajectory_follower/visibility_control.hpp"
-
-
namespace autoware
{
namespace motion
@@ -35,8 +35,8 @@ namespace control
{
namespace trajectory_follower
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
/**
* @brief Smooth stop class to implement vehicle specific deceleration profiles
*/
@@ -62,13 +62,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop
* @param [in] min_running_acc minimum acceleration to consider ego to be running [m/s]
* @param [in] weak_stop_time time allowed for stopping with a weak acceleration [s]
* @param [in] weak_stop_dist distance to the stop point bellow which a weak accel is applied [m]
- * @param [in] strong_stop_dist distance to the stop point bellow which a strong accel is applied [m]
+ * @param [in] strong_stop_dist distance to the stop point bellow which a strong accel is applied
+ * [m]
*/
void setParams(
float64_t max_strong_acc, float64_t min_strong_acc, float64_t weak_acc, float64_t weak_stop_acc,
float64_t strong_stop_acc, float64_t min_fast_vel, float64_t min_running_vel,
- float64_t min_running_acc,
- float64_t weak_stop_time, float64_t weak_stop_dist, float64_t strong_stop_dist);
+ float64_t min_running_acc, float64_t weak_stop_time, float64_t weak_stop_dist,
+ float64_t strong_stop_dist);
/**
* @brief predict time when car stops by fitting some latest observed velocity history
diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp
index 7313361572ac7..23f3ca8e288fa 100644
--- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp
@@ -33,23 +33,24 @@
* u = steer
*
* Linearized model around reference point (v=vr)
- * [0, 1, 0, 0] [ 0] [ 0] dx/dt = [0,
- * -(cf+cr)/m/vr, (cf+cr)/m, (lr*cr-lf*cf)/m/vr] * x + [ cf/m] * u + [(lr*cr-lf*cf)/m/vr*k - vr*k] [0,
- * 0, 0, 1] [ 0] [ 0] [0,
- * (lr*cr-lf*cf)/Iz/vr, (lf*cf-lr*cr)/Iz, -(lf^2*cf+lr^2*cr)/Iz/vr] [lf*cf/Iz] [ -(lf^2*cf+lr^2*cr)/Iz/vr]
+ * [0, 1, 0, 0] [ 0] [
+ * 0] dx/dt = [0,
+ * -(cf+cr)/m/vr, (cf+cr)/m, (lr*cr-lf*cf)/m/vr] * x + [ cf/m] * u +
+ * [(lr*cr-lf*cf)/m/vr*k - vr*k] [0, 0, 0, 1] [ 0]
+ * [ 0] [0, (lr*cr-lf*cf)/Iz/vr, (lf*cf-lr*cr)/Iz,
+ * -(lf^2*cf+lr^2*cr)/Iz/vr] [lf*cf/Iz] [ -(lf^2*cf+lr^2*cr)/Iz/vr]
*
- * Reference : Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", Robotics
- * Institute, Carnegie Mellon University, February 2009.
+ * Reference : Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path
+ * Tracking", Robotics Institute, Carnegie Mellon University, February 2009.
*/
#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_
#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_
-#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp"
-
#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/LU"
+#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp"
#include "trajectory_follower/visibility_control.hpp"
namespace autoware
@@ -80,8 +81,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInter
*/
DynamicsBicycleModel(
const float64_t wheelbase, const float64_t mass_fl, const float64_t mass_fr,
- const float64_t mass_rl, const float64_t mass_rr,
- const float64_t cf, const float64_t cr);
+ const float64_t mass_rl, const float64_t mass_rr, const float64_t cf, const float64_t cr);
/**
* @brief destructor
@@ -107,12 +107,12 @@ class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInter
void calculateReferenceInput(Eigen::MatrixXd & u_ref) override;
private:
- float64_t m_lf; //!< @brief length from center of mass to front wheel [m]
- float64_t m_lr; //!< @brief length from center of mass to rear wheel [m]
- float64_t m_mass; //!< @brief total mass of vehicle [kg]
- float64_t m_iz; //!< @brief moment of inertia [kg * m2]
- float64_t m_cf; //!< @brief front cornering power [N/rad]
- float64_t m_cr; //!< @brief rear cornering power [N/rad]
+ float64_t m_lf; //!< @brief length from center of mass to front wheel [m]
+ float64_t m_lr; //!< @brief length from center of mass to rear wheel [m]
+ float64_t m_mass; //!< @brief total mass of vehicle [kg]
+ float64_t m_iz; //!< @brief moment of inertia [kg * m2]
+ float64_t m_cf; //!< @brief front cornering power [N/rad]
+ float64_t m_cr; //!< @brief rear cornering power [N/rad]
};
} // namespace trajectory_follower
} // namespace control
diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp
index 3470c9ad583c9..525461f3a9e5d 100644
--- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp
@@ -41,13 +41,11 @@
#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_
#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_
-#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp"
-
-#include "trajectory_follower/visibility_control.hpp"
-
#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/LU"
+#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp"
+#include "trajectory_follower/visibility_control.hpp"
namespace autoware
{
diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp
index 9436b22096b7b..8656f1ab7afb8 100644
--- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp
@@ -38,11 +38,10 @@
#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_
#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_
-#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp"
-
#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/LU"
+#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp"
#include "trajectory_follower/visibility_control.hpp"
namespace autoware
diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp
index 6ad22852c2e8a..1c3856246f719 100644
--- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp
@@ -35,9 +35,9 @@ using autoware::common::types::float64_t;
class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface
{
protected:
- const int64_t m_dim_x; //!< @brief dimension of state x
- const int64_t m_dim_u; //!< @brief dimension of input u
- const int64_t m_dim_y; //!< @brief dimension of output y
+ const int64_t m_dim_x; //!< @brief dimension of state x
+ const int64_t m_dim_u; //!< @brief dimension of input u
+ const int64_t m_dim_y; //!< @brief dimension of output y
float64_t m_velocity; //!< @brief vehicle velocity [m/s]
float64_t m_curvature; //!< @brief curvature on the linearized point on path
float64_t m_wheelbase; //!< @brief wheelbase of the vehicle [m]
diff --git a/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp b/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp
index 655d7b5160c9c..f7ede1ea32c96 100644
--- a/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp
+++ b/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp
@@ -17,21 +17,21 @@
////////////////////////////////////////////////////////////////////////////////
#if defined(__WIN32)
- #if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
- #define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport)
- #define TRAJECTORY_FOLLOWER_LOCAL
- #else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
- #define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport)
- #define TRAJECTORY_FOLLOWER_LOCAL
- #endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
+#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
+#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport)
+#define TRAJECTORY_FOLLOWER_LOCAL
+#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
+#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport)
+#define TRAJECTORY_FOLLOWER_LOCAL
+#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
#elif defined(__linux__)
- #define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default")))
- #define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden")))
+#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default")))
+#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
- #define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default")))
- #define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden")))
+#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default")))
+#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden")))
#else
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif
#endif // TRAJECTORY_FOLLOWER__VISIBILITY_CONTROL_HPP_
diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml
index f94336c6e31a5..6df4d34cfcc89 100644
--- a/control/trajectory_follower/package.xml
+++ b/control/trajectory_follower/package.xml
@@ -11,11 +11,11 @@
<buildtool_depend>autoware_auto_cmake</buildtool_depend>
<depend>autoware_auto_common</depend>
- <depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_control_msgs</depend>
- <depend>autoware_auto_vehicle_msgs</depend>
- <depend>autoware_auto_system_msgs</depend>
<depend>autoware_auto_geometry</depend>
+ <depend>autoware_auto_planning_msgs</depend>
+ <depend>autoware_auto_system_msgs</depend>
+ <depend>autoware_auto_vehicle_msgs</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>interpolation</depend>
@@ -28,9 +28,9 @@
<depend>tf2_ros</depend>
<test_depend>ament_cmake_gtest</test_depend>
+ <test_depend>eigen</test_depend>
<!-- <test_depend>ament_lint_auto</test_depend> -->
<!-- <test_depend>ament_lint_common</test_depend> -->
- <test_depend>eigen</test_depend>
<export>
<build_type>ament_cmake</build_type>
diff --git a/control/trajectory_follower/src/interpolate.cpp b/control/trajectory_follower/src/interpolate.cpp
index 4abff5c4ddfe3..4087f7b6d3fc3 100644
--- a/control/trajectory_follower/src/interpolate.cpp
+++ b/control/trajectory_follower/src/interpolate.cpp
@@ -35,7 +35,9 @@ namespace
bool8_t isIncrease(const std::vector<float64_t> & x)
{
for (size_t i = 0; i < x.size() - 1; ++i) {
- if (x.at(i) > x.at(i + 1)) {return false;}
+ if (x.at(i) > x.at(i + 1)) {
+ return false;
+ }
}
return true;
}
@@ -45,19 +47,19 @@ bool8_t isValidInput(
const std::vector<float64_t> & return_index)
{
if (base_index.empty() || base_value.empty() || return_index.empty()) {
- std::cerr << "mpc bad index : some vector is empty. base_index: " << base_index.size() <<
- ", base_value: " << base_value.size() << ", return_index: " << return_index.size() <<
- std::endl;
+ std::cerr << "mpc bad index : some vector is empty. base_index: " << base_index.size()
+ << ", base_value: " << base_value.size() << ", return_index: " << return_index.size()
+ << std::endl;
return false;
}
if (!isIncrease(base_index)) {
- std::cerr << "mpc bad index : base_index is not monotonically increasing. base_index = [" <<
- base_index.front() << ", " << base_index.back() << "]" << std::endl;
+ std::cerr << "mpc bad index : base_index is not monotonically increasing. base_index = ["
+ << base_index.front() << ", " << base_index.back() << "]" << std::endl;
return false;
}
if (!isIncrease(return_index)) {
- std::cerr << "mpc bad index : base_index is not monotonically increasing. return_index = [" <<
- return_index.front() << ", " << return_index.back() << "]" << std::endl;
+ std::cerr << "mpc bad index : base_index is not monotonically increasing. return_index = ["
+ << return_index.front() << ", " << return_index.back() << "]" << std::endl;
return false;
}
if (return_index.front() < base_index.front()) {
@@ -97,13 +99,15 @@ bool8_t linearInterpolate(
}
while (base_index.at(i) < idx) {
++i;
- if (i <= 0 || base_size - 1 < i) {break;}
+ if (i <= 0 || base_size - 1 < i) {
+ break;
+ }
}
if (i <= 0 || base_size - 1 < i) {
- std::cerr << "mpc LinearInterpolate : undesired condition. skip index. (i = " << i <<
- ", base_size = " << base_size << "), idx = " << idx <<
- ", base_index.at(i) = " << base_index.at(i) << std::endl;
+ std::cerr << "mpc LinearInterpolate : undesired condition. skip index. (i = " << i
+ << ", base_size = " << base_size << "), idx = " << idx
+ << ", base_index.at(i) = " << base_index.at(i) << std::endl;
continue;
}
diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp
index 18d3cbe047aee..7e82740017cd3 100644
--- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp
+++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp
@@ -14,15 +14,16 @@
#include "trajectory_follower/longitudinal_controller_utils.hpp"
-#include <algorithm>
-#include <experimental/optional> // NOLINT
-#include <limits>
-
#include "motion_common/motion_common.hpp"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Quaternion.h"
+
+#include <experimental/optional> // NOLINT
+
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include <algorithm>
+#include <limits>
namespace autoware
{
@@ -44,8 +45,7 @@ bool isValidTrajectory(const Trajectory & traj)
!isfinite(p.pose.orientation.x) || !isfinite(p.pose.orientation.y) ||
!isfinite(p.pose.orientation.z) || !isfinite(p.longitudinal_velocity_mps) ||
!isfinite(p.lateral_velocity_mps) || !isfinite(p.acceleration_mps2) ||
- !isfinite(p.heading_rate_rps))
- {
+ !isfinite(p.heading_rate_rps)) {
return false;
}
}
@@ -58,8 +58,7 @@ bool isValidTrajectory(const Trajectory & traj)
return true;
}
-float64_t calcStopDistance(
- const Point & current_pos, const Trajectory & traj)
+float64_t calcStopDistance(const Point & current_pos, const Trajectory & traj)
{
const std::experimental::optional<size_t> stop_idx_opt =
trajectory_common::searchZeroVelocityIndex(traj.points);
@@ -96,8 +95,7 @@ float64_t getPitchByTraj(
distance_2d<float64_t>(trajectory.points.at(nearest_idx), trajectory.points.at(i));
if (dist > wheel_base) {
// calculate pitch from trajectory between rear wheel (nearest) and front center (i)
- return calcElevationAngle(
- trajectory.points.at(nearest_idx), trajectory.points.at(i));
+ return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i));
}
}
@@ -109,15 +107,12 @@ float64_t getPitchByTraj(
if (dist > wheel_base) {
// calculate pitch from trajectory
// between wheelbase behind the end of trajectory (i) and the end of trajectory (back)
- return calcElevationAngle(
- trajectory.points.at(i), trajectory.points.back());
+ return calcElevationAngle(trajectory.points.at(i), trajectory.points.back());
}
}
// calculate pitch from trajectory between the beginning and end of trajectory
- return calcElevationAngle(
- trajectory.points.at(0),
- trajectory.points.back());
+ return calcElevationAngle(trajectory.points.at(0), trajectory.points.back());
}
float64_t calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to)
diff --git a/control/trajectory_follower/src/lowpass_filter.cpp b/control/trajectory_follower/src/lowpass_filter.cpp
index 4098a16a61eba..399f5c21b3208 100644
--- a/control/trajectory_follower/src/lowpass_filter.cpp
+++ b/control/trajectory_follower/src/lowpass_filter.cpp
@@ -60,8 +60,7 @@ float64_t Butterworth2dFilter::filter(const float64_t & u0)
}
void Butterworth2dFilter::filt_vector(
- const std::vector<float64_t> & t,
- std::vector<float64_t> & u) const
+ const std::vector<float64_t> & t, std::vector<float64_t> & u) const
{
u.resize(t.size());
float64_t y1 = t.at(0);
@@ -83,8 +82,7 @@ void Butterworth2dFilter::filt_vector(
// filtering forward and backward direction
void Butterworth2dFilter::filtfilt_vector(
- const std::vector<float64_t> & t,
- std::vector<float64_t> & u) const
+ const std::vector<float64_t> & t, std::vector<float64_t> & u) const
{
std::vector<float64_t> t_fwd(t);
std::vector<float64_t> t_rev(t);
diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp
index 3a34cef476f98..f71bd4e7b65ca 100644
--- a/control/trajectory_follower/src/mpc.cpp
+++ b/control/trajectory_follower/src/mpc.cpp
@@ -12,6 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "trajectory_follower/mpc.hpp"
+
#include <algorithm>
#include <deque>
#include <limits>
@@ -20,8 +22,6 @@
#include <utility>
#include <vector>
-#include "trajectory_follower/mpc.hpp"
-
#define DEG2RAD 3.1415926535 / 180.0
#define RAD2DEG 180.0 / 3.1415926535
@@ -33,13 +33,12 @@ namespace control
{
namespace trajectory_follower
{
-using namespace std::chrono_literals;
+using namespace std::literals::chrono_literals;
using ::motion::motion_common::to_angle;
bool8_t MPC::calculateMPC(
const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer,
- const float64_t current_velocity,
- const geometry_msgs::msg::Pose & current_pose,
+ const float64_t current_velocity, const geometry_msgs::msg::Pose & current_pose,
autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd,
autoware_auto_planning_msgs::msg::Trajectory & predicted_traj,
autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic)
@@ -61,8 +60,7 @@ bool8_t MPC::calculateMPC(
if (!updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, &x0)) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(
- m_logger, *m_clock, 1000 /*ms*/,
- "updateStateForDelayCompensation failed. stop computation.");
+ m_logger, *m_clock, 1000 /*ms*/, "updateStateForDelayCompensation failed. stop computation.");
return false;
}
@@ -70,9 +68,7 @@ bool8_t MPC::calculateMPC(
trajectory_follower::MPCTrajectory mpc_resampled_ref_traj;
const float64_t mpc_start_time = mpc_data.nearest_time + m_param.input_delay;
if (!resampleMPCTrajectoryByTime(mpc_start_time, reference_trajectory, &mpc_resampled_ref_traj)) {
- RCLCPP_WARN_THROTTLE(
- m_logger, *m_clock,
- 1000 /*ms*/, "trajectory resampling failed.");
+ RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "trajectory resampling failed.");
return false;
}
@@ -132,10 +128,10 @@ bool8_t MPC::calculateMPC(
const float64_t steer_cmd = ctrl_cmd.steering_tire_angle;
const float64_t wb = m_vehicle_model_ptr->getWheelbase();
- typedef decltype (diagnostic.diag_array.data) ::value_type DiagnosticValueType;
+ typedef decltype(diagnostic.diag_array.data)::value_type DiagnosticValueType;
auto append_diag_data = [&](const auto & val) -> void {
- diagnostic.diag_array.data.push_back(static_cast<DiagnosticValueType>(val));
- };
+ diagnostic.diag_array.data.push_back(static_cast<DiagnosticValueType>(val));
+ };
// [0] final steering command (MPC + LPF)
append_diag_data(steer_cmd);
// [1] mpc calculation result
@@ -178,10 +174,8 @@ bool8_t MPC::calculateMPC(
void MPC::setReferenceTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
- const float64_t traj_resample_dist,
- const bool8_t enable_path_smoothing,
- const int64_t path_filter_moving_ave_num,
- const int64_t curvature_smoothing_num_traj,
+ const float64_t traj_resample_dist, const bool8_t enable_path_smoothing,
+ const int64_t path_filter_moving_ave_num, const int64_t curvature_smoothing_num_traj,
const int64_t curvature_smoothing_num_ref_steer,
const geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr)
{
@@ -192,8 +186,7 @@ void MPC::setReferenceTrajectory(
/* resampling */
trajectory_follower::MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw);
if (!trajectory_follower::MPCUtils::resampleMPCTrajectoryByDistance(
- mpc_traj_raw, traj_resample_dist, &mpc_traj_resampled))
- {
+ mpc_traj_raw, traj_resample_dist, &mpc_traj_resampled)) {
RCLCPP_WARN(m_logger, "[setReferenceTrajectory] spline error when resampling by distance");
return;
}
@@ -204,18 +197,13 @@ void MPC::setReferenceTrajectory(
if (enable_path_smoothing && mpc_traj_resampled_size > 2 * path_filter_moving_ave_num) {
if (
!trajectory_follower::MoveAverageFilter::filt_vector(
- path_filter_moving_ave_num,
- mpc_traj_smoothed.x) ||
+ path_filter_moving_ave_num, mpc_traj_smoothed.x) ||
!trajectory_follower::MoveAverageFilter::filt_vector(
- path_filter_moving_ave_num,
- mpc_traj_smoothed.y) ||
+ path_filter_moving_ave_num, mpc_traj_smoothed.y) ||
!trajectory_follower::MoveAverageFilter::filt_vector(
- path_filter_moving_ave_num,
- mpc_traj_smoothed.yaw) ||
+ path_filter_moving_ave_num, mpc_traj_smoothed.yaw) ||
!trajectory_follower::MoveAverageFilter::filt_vector(
- path_filter_moving_ave_num,
- mpc_traj_smoothed.vx))
- {
+ path_filter_moving_ave_num, mpc_traj_smoothed.vx)) {
RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering.");
mpc_traj_smoothed = mpc_traj_resampled;
}
@@ -227,18 +215,14 @@ void MPC::setReferenceTrajectory(
MPCUtils::calcNearestIndex(mpc_traj_smoothed, current_pose_ptr->pose);
const float64_t ego_yaw = tf2::getYaw(current_pose_ptr->pose.orientation);
trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(
- &mpc_traj_smoothed, nearest_idx,
- ego_yaw);
+ &mpc_traj_smoothed, nearest_idx, ego_yaw);
trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw);
}
/* calculate curvature */
trajectory_follower::MPCUtils::calcTrajectoryCurvature(
- static_cast<size_t>(
- curvature_smoothing_num_traj),
- static_cast<size_t>(
- curvature_smoothing_num_ref_steer),
- &mpc_traj_smoothed);
+ static_cast<size_t>(curvature_smoothing_num_traj),
+ static_cast<size_t>(curvature_smoothing_num_ref_steer), &mpc_traj_smoothed);
/* add end point with vel=0 on traj for mpc prediction */
{
@@ -269,15 +253,13 @@ void MPC::resetPrevResult(const autoware_auto_vehicle_msgs::msg::SteeringReport
bool8_t MPC::getData(
const trajectory_follower::MPCTrajectory & traj,
const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer,
- const geometry_msgs::msg::Pose & current_pose,
- MPCData * data)
+ const geometry_msgs::msg::Pose & current_pose, MPCData * data)
{
static constexpr auto duration = 5000 /*ms*/;
size_t nearest_idx;
if (!trajectory_follower::MPCUtils::calcNearestPoseInterp(
- traj, current_pose, &(data->nearest_pose), &(nearest_idx),
- &(data->nearest_time), m_logger, *m_clock))
- {
+ traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time), m_logger,
+ *m_clock)) {
// reset previous MPC result
// Note: When a large deviation from the trajectory occurs, the optimization stops and
// the vehicle will return to the path by re-planning the trajectory or external operation.
@@ -285,20 +267,17 @@ bool8_t MPC::getData(
// the actual steer angle, and it may make the optimization result unstable.
resetPrevResult(current_steer);
RCLCPP_WARN_SKIPFIRST_THROTTLE(
- m_logger, *m_clock, duration,
- "calculateMPC: error in calculating nearest pose. stop mpc.");
+ m_logger, *m_clock, duration, "calculateMPC: error in calculating nearest pose. stop mpc.");
return false;
}
/* get data */
data->nearest_idx = static_cast<int64_t>(nearest_idx);
data->steer = static_cast<float64_t>(current_steer.steering_tire_angle);
- data->lateral_err = trajectory_follower::MPCUtils::calcLateralError(
- current_pose,
- data->nearest_pose);
+ data->lateral_err =
+ trajectory_follower::MPCUtils::calcLateralError(current_pose, data->nearest_pose);
data->yaw_err = autoware::common::helper_functions::wrap_angle(
- to_angle(current_pose.orientation) -
- to_angle(data->nearest_pose.orientation));
+ to_angle(current_pose.orientation) - to_angle(data->nearest_pose.orientation));
/* get predicted steer */
if (!m_steer_prediction_prev) {
@@ -309,8 +288,7 @@ bool8_t MPC::getData(
/* check error limit */
const float64_t dist_err = autoware::common::geometry::distance_2d<float64_t>(
- current_pose.position,
- data->nearest_pose.position);
+ current_pose.position, data->nearest_pose.position);
if (dist_err > m_admissible_position_error) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(
m_logger, *m_clock, duration, "position error is over limit. error = %fm, limit: %fm",
@@ -345,10 +323,12 @@ float64_t MPC::calcSteerPrediction()
const float64_t duration = (t_end - t_start).seconds();
const float64_t time_constant = m_param.steer_tau;
- const float64_t initial_response = std::exp(-duration / time_constant) *
- (*m_steer_prediction_prev);
+ const float64_t initial_response =
+ std::exp(-duration / time_constant) * (*m_steer_prediction_prev);
- if (m_ctrl_cmd_vec.size() <= 2) {return initial_response;}
+ if (m_ctrl_cmd_vec.size() <= 2) {
+ return initial_response;
+ }
return initial_response + getSteerCmdSum(t_start, t_end, time_constant);
}
@@ -356,12 +336,16 @@ float64_t MPC::calcSteerPrediction()
float64_t MPC::getSteerCmdSum(
const rclcpp::Time & t_start, const rclcpp::Time & t_end, const float64_t time_constant) const
{
- if (m_ctrl_cmd_vec.size() <= 2) {return 0.0;}
+ if (m_ctrl_cmd_vec.size() <= 2) {
+ return 0.0;
+ }
// Find first index of control command container
size_t idx = 1;
while (t_start > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) {
- if ((idx + 1) >= m_ctrl_cmd_vec.size()) {return 0.0;}
+ if ((idx + 1) >= m_ctrl_cmd_vec.size()) {
+ return 0.0;
+ }
++idx;
}
@@ -371,17 +355,17 @@ float64_t MPC::getSteerCmdSum(
while (t_end > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) {
const float64_t duration = (rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp) - t).seconds();
t = rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp);
- steer_sum +=
- (1 - std::exp(-duration / time_constant)) *
- static_cast<float64_t>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);
+ steer_sum += (1 - std::exp(-duration / time_constant)) *
+ static_cast<float64_t>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);
++idx;
- if (idx >= m_ctrl_cmd_vec.size()) {break;}
+ if (idx >= m_ctrl_cmd_vec.size()) {
+ break;
+ }
}
const float64_t duration = (t_end - t).seconds();
- steer_sum +=
- (1 - std::exp(-duration / time_constant)) *
- static_cast<float64_t>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);
+ steer_sum += (1 - std::exp(-duration / time_constant)) *
+ static_cast<float64_t>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);
return steer_sum;
}
@@ -402,10 +386,7 @@ void MPC::storeSteerCmd(const float64_t steer)
// remove unused ctrl cmd
constexpr float64_t store_time = 0.3;
- if (
- (time_delayed - m_ctrl_cmd_vec.at(1).stamp).seconds() >
- m_param.input_delay + store_time)
- {
+ if ((time_delayed - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_param.input_delay + store_time) {
m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin());
}
}
@@ -419,12 +400,9 @@ bool8_t MPC::resampleMPCTrajectoryByTime(
mpc_time_v.push_back(ts + i * m_param.prediction_dt);
}
if (!trajectory_follower::MPCUtils::linearInterpMPCTrajectory(
- input.relative_time, input,
- mpc_time_v, output))
- {
+ input.relative_time, input, mpc_time_v, output)) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(
- m_logger, *m_clock,
- 1000 /*ms*/,
+ m_logger, *m_clock, 1000 /*ms*/,
"calculateMPC: mpc resample error. stop mpc calculation. check code!");
return false;
}
@@ -479,16 +457,10 @@ bool8_t MPC::updateStateForDelayCompensation(
float64_t k = 0.0;
float64_t v = 0.0;
if (
- !trajectory_follower::linearInterpolate(
- traj.relative_time, traj.k,
- mpc_curr_time, k) ||
- !trajectory_follower::linearInterpolate(
- traj.relative_time, traj.vx,
- mpc_curr_time, v))
- {
+ !trajectory_follower::linearInterpolate(traj.relative_time, traj.k, mpc_curr_time, k) ||
+ !trajectory_follower::linearInterpolate(traj.relative_time, traj.vx, mpc_curr_time, v)) {
RCLCPP_ERROR(
- m_logger,
- "mpc resample error at delay compensation, stop mpc calculation. check code!");
+ m_logger, "mpc resample error at delay compensation, stop mpc calculation. check code!");
return false;
}
@@ -506,21 +478,20 @@ bool8_t MPC::updateStateForDelayCompensation(
}
trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter(
- const trajectory_follower::MPCTrajectory & input,
- const geometry_msgs::msg::Pose & current_pose,
+ const trajectory_follower::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose,
const float64_t v0) const
{
- int64_t nearest_idx =
- trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose);
- if (nearest_idx < 0) {return input;}
+ int64_t nearest_idx = trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose);
+ if (nearest_idx < 0) {
+ return input;
+ }
const float64_t acc_lim = m_param.acceleration_limit;
const float64_t tau = m_param.velocity_time_constant;
trajectory_follower::MPCTrajectory output = input;
trajectory_follower::MPCUtils::dynamicSmoothingVelocity(
- static_cast<size_t>(nearest_idx), v0,
- acc_lim, tau, output);
+ static_cast<size_t>(nearest_idx), v0, acc_lim, tau, output);
const float64_t t_ext = 100.0; // extra time to prevent mpc calculation failure due to short time
const float64_t t_end = output.relative_time.back() + getPredictionTime() + t_ext;
const float64_t v_end = 0.0;
@@ -536,8 +507,7 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter(
* cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex
* Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
*/
-MPCMatrix MPC::generateMPCMatrix(
- const trajectory_follower::MPCTrajectory & reference_trajectory)
+MPCMatrix MPC::generateMPCMatrix(const trajectory_follower::MPCTrajectory & reference_trajectory)
{
using Eigen::MatrixXd;
@@ -578,8 +548,8 @@ MPCMatrix MPC::generateMPCMatrix(
// curvature will be 0 when vehicle stops
const float64_t ref_k = reference_trajectory.k[static_cast<size_t>(i)] * m_sign_vx;
- const float64_t ref_smooth_k = reference_trajectory.smooth_k[static_cast<size_t>(i)] *
- m_sign_vx;
+ const float64_t ref_smooth_k =
+ reference_trajectory.smooth_k[static_cast<size_t>(i)] * m_sign_vx;
/* get discrete state matrix A, B, C, W */
m_vehicle_model_ptr->setVelocity(ref_vx);
@@ -687,7 +657,8 @@ bool8_t MPC::executeOptimization(
// cost function: 1/2 * Uex' * H * Uex + f' * Uex, H = B' * C' * Q * C * B + R
const MatrixXd CB = m.Cex * m.Bex;
const MatrixXd QCB = m.Qex * CB;
- // MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; // This calculation is heavy. looking for a good way. //NOLINT
+ // MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; // This calculation is heavy. looking for
+ // a good way. //NOLINT
MatrixXd H = MatrixXd::Zero(DIM_U_N, DIM_U_N);
H.triangularView<Eigen::Upper>() = CB.transpose() * QCB;
H.triangularView<Eigen::Upper>() += m.R1ex + m.R2ex;
@@ -717,8 +688,7 @@ bool8_t MPC::executeOptimization(
{
auto t = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
- RCLCPP_DEBUG(
- m_logger, "qp solver calculation time = %ld [ms]", t);
+ RCLCPP_DEBUG(m_logger, "qp solver calculation time = %ld [ms]", t);
}
if (Uex->array().isNaN().any()) {
@@ -803,8 +773,7 @@ void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr) const
float64_t MPC::getPredictionTime() const
{
return static_cast<float64_t>(m_param.prediction_horizon - 1) * m_param.prediction_dt +
- m_param.input_delay +
- m_ctrl_period;
+ m_param.input_delay + m_ctrl_period;
}
bool8_t MPC::isValid(const MPCMatrix & m) const
@@ -812,16 +781,14 @@ bool8_t MPC::isValid(const MPCMatrix & m) const
if (
m.Aex.array().isNaN().any() || m.Bex.array().isNaN().any() || m.Cex.array().isNaN().any() ||
m.Wex.array().isNaN().any() || m.Qex.array().isNaN().any() || m.R1ex.array().isNaN().any() ||
- m.R2ex.array().isNaN().any() || m.Uref_ex.array().isNaN().any())
- {
+ m.R2ex.array().isNaN().any() || m.Uref_ex.array().isNaN().any()) {
return false;
}
if (
m.Aex.array().isInf().any() || m.Bex.array().isInf().any() || m.Cex.array().isInf().any() ||
m.Wex.array().isInf().any() || m.Qex.array().isInf().any() || m.R1ex.array().isInf().any() ||
- m.R2ex.array().isInf().any() || m.Uref_ex.array().isInf().any())
- {
+ m.R2ex.array().isInf().any() || m.Uref_ex.array().isInf().any()) {
return false;
}
diff --git a/control/trajectory_follower/src/mpc_trajectory.cpp b/control/trajectory_follower/src/mpc_trajectory.cpp
index cc1c51130c489..82a11f077085e 100644
--- a/control/trajectory_follower/src/mpc_trajectory.cpp
+++ b/control/trajectory_follower/src/mpc_trajectory.cpp
@@ -24,8 +24,7 @@ namespace trajectory_follower
{
void MPCTrajectory::push_back(
const float64_t & xp, const float64_t & yp, const float64_t & zp, const float64_t & yawp,
- const float64_t & vxp,
- const float64_t & kp, const float64_t & smooth_kp, const float64_t & tp)
+ const float64_t & vxp, const float64_t & kp, const float64_t & smooth_kp, const float64_t & tp)
{
x.push_back(xp);
y.push_back(yp);
@@ -54,8 +53,7 @@ size_t MPCTrajectory::size() const
if (
x.size() == y.size() && x.size() == z.size() && x.size() == yaw.size() &&
x.size() == vx.size() && x.size() == k.size() && x.size() == smooth_k.size() &&
- x.size() == relative_time.size())
- {
+ x.size() == relative_time.size()) {
return x.size();
} else {
std::cerr << "[MPC trajectory] trajectory size is inappropriate" << std::endl;
diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp
index 3d2cdacd6ddd5..6909679e1900a 100644
--- a/control/trajectory_follower/src/mpc_utils.cpp
+++ b/control/trajectory_follower/src/mpc_utils.cpp
@@ -105,9 +105,8 @@ bool8_t resampleMPCTrajectoryByDistance(
output->vx = interpolation::lerp(input_arclength, input.vx, output_arclength);
output->k = interpolation::slerp(input_arclength, input.k, output_arclength);
output->smooth_k = interpolation::slerp(input_arclength, input.smooth_k, output_arclength);
- output->relative_time = interpolation::lerp(
- input_arclength, input.relative_time,
- output_arclength);
+ output->relative_time =
+ interpolation::lerp(input_arclength, input.relative_time, output_arclength);
return true;
}
@@ -136,9 +135,7 @@ bool8_t linearInterpMPCTrajectory(
!linearInterpolate(in_index, in_traj.vx, out_index, out_traj->vx) ||
!linearInterpolate(in_index, in_traj.k, out_index, out_traj->k) ||
!linearInterpolate(in_index, in_traj.smooth_k, out_index, out_traj->smooth_k) ||
- !linearInterpolate(
- in_index, in_traj.relative_time, out_index, out_traj->relative_time))
- {
+ !linearInterpolate(in_index, in_traj.relative_time, out_index, out_traj->relative_time)) {
std::cerr << "linearInterpMPCTrajectory error!" << std::endl;
return false;
}
@@ -152,8 +149,7 @@ bool8_t linearInterpMPCTrajectory(
}
void calcTrajectoryYawFromXY(
- MPCTrajectory * traj, const int64_t nearest_idx,
- const float64_t ego_yaw)
+ MPCTrajectory * traj, const int64_t nearest_idx, const float64_t ego_yaw)
{
if (traj->yaw.size() < 3) { // at least 3 points are required to calculate yaw
return;
@@ -167,9 +163,9 @@ void calcTrajectoryYawFromXY(
const int64_t upper_nearest_idx =
(static_cast<int64_t>(traj->x.size()) - 1 == nearest_idx) ? nearest_idx : nearest_idx + 1;
const float64_t dx = traj->x[static_cast<size_t>(upper_nearest_idx)] -
- traj->x[static_cast<size_t>(upper_nearest_idx - 1)];
+ traj->x[static_cast<size_t>(upper_nearest_idx - 1)];
const float64_t dy = traj->y[static_cast<size_t>(upper_nearest_idx)] -
- traj->y[static_cast<size_t>(upper_nearest_idx - 1)];
+ traj->y[static_cast<size_t>(upper_nearest_idx - 1)];
const bool forward_shift =
std::abs(autoware::common::helper_functions::wrap_angle(std::atan2(dy, dx) - ego_yaw)) <
M_PI / 2.0;
@@ -188,8 +184,7 @@ void calcTrajectoryYawFromXY(
}
bool8_t calcTrajectoryCurvature(
- const size_t curvature_smoothing_num_traj,
- const size_t curvature_smoothing_num_ref_steer,
+ const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer,
MPCTrajectory * traj)
{
if (!traj) {
@@ -208,8 +203,8 @@ std::vector<float64_t> calcTrajectoryCurvature(
/* calculate curvature by circle fitting from three points */
geometry_msgs::msg::Point p1, p2, p3;
- const size_t max_smoothing_num = static_cast<size_t>(
- std::floor(0.5 * (static_cast<float64_t>(traj.x.size() - 1))));
+ const size_t max_smoothing_num =
+ static_cast<size_t>(std::floor(0.5 * (static_cast<float64_t>(traj.x.size() - 1))));
const size_t L = std::min(curvature_smoothing_num, max_smoothing_num);
for (size_t i = L; i < traj.x.size() - L; ++i) {
const size_t curr_idx = i;
@@ -222,9 +217,8 @@ std::vector<float64_t> calcTrajectoryCurvature(
p2.y = traj.y[curr_idx];
p3.y = traj.y[next_idx];
const float64_t den = std::max(
- distance_2d<float64_t>(
- p1,
- p2) * distance_2d<float64_t>(p2, p3) * distance_2d<float64_t>(p3, p1),
+ distance_2d<float64_t>(p1, p2) * distance_2d<float64_t>(p2, p3) *
+ distance_2d<float64_t>(p3, p1),
std::numeric_limits<float64_t>::epsilon());
const float64_t curvature =
2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den;
@@ -234,9 +228,8 @@ std::vector<float64_t> calcTrajectoryCurvature(
/* first and last curvature is copied from next value */
for (size_t i = 0; i < std::min(L, traj.x.size()); ++i) {
curvature_vec.at(i) = curvature_vec.at(std::min(L, traj.x.size() - 1));
- curvature_vec.at(
- traj.x.size() - i -
- 1) = curvature_vec.at(std::max(traj.x.size() - L - 1, size_t(0)));
+ curvature_vec.at(traj.x.size() - i - 1) =
+ curvature_vec.at(std::max(traj.x.size() - L - 1, size_t(0)));
}
return curvature_vec;
}
@@ -304,8 +297,8 @@ void dynamicSmoothingVelocity(
for (size_t i = start_idx + 1; i < traj.size(); ++i) {
const float64_t ds =
std::hypot(traj.x.at(i) - traj.x.at(i - 1), traj.y.at(i) - traj.y.at(i - 1));
- const float64_t dt = ds /
- std::max(std::fabs(curr_v), std::numeric_limits<float64_t>::epsilon());
+ const float64_t dt =
+ ds / std::max(std::fabs(curr_v), std::numeric_limits<float64_t>::epsilon());
const float64_t a = tau / std::max(tau + dt, std::numeric_limits<float64_t>::epsilon());
const float64_t updated_v = a * curr_v + (1.0 - a) * traj.vx.at(i);
const float64_t dv = std::max(-acc_lim * dt, std::min(acc_lim * dt, updated_v - curr_v));
@@ -315,8 +308,7 @@ void dynamicSmoothingVelocity(
calcMPCTrajectoryTime(traj);
}
-int64_t calcNearestIndex(
- const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose)
+int64_t calcNearestIndex(const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose)
{
if (traj.empty()) {
return -1;
@@ -437,22 +429,18 @@ bool8_t calcNearestPoseInterp(
alpha * traj.x[*nearest_index] + (1 - alpha) * traj.x[second_nearest_index];
nearest_pose->position.y =
alpha * traj.y[*nearest_index] + (1 - alpha) * traj.y[second_nearest_index];
- const float64_t tmp_yaw_err =
- autoware::common::helper_functions::wrap_angle(
- traj.yaw[*nearest_index] -
- traj.yaw[second_nearest_index]);
- const float64_t nearest_yaw =
- autoware::common::helper_functions::wrap_angle(
+ const float64_t tmp_yaw_err = autoware::common::helper_functions::wrap_angle(
+ traj.yaw[*nearest_index] - traj.yaw[second_nearest_index]);
+ const float64_t nearest_yaw = autoware::common::helper_functions::wrap_angle(
traj.yaw[second_nearest_index] + alpha * tmp_yaw_err);
nearest_pose->orientation = getQuaternionFromYaw(nearest_yaw);
*nearest_time = alpha * traj.relative_time[*nearest_index] +
- (1 - alpha) * traj.relative_time[second_nearest_index];
+ (1 - alpha) * traj.relative_time[second_nearest_index];
return true;
}
float64_t calcStopDistance(
- const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory,
- const int64_t origin)
+ const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int64_t origin)
{
constexpr float zero_velocity = std::numeric_limits<float>::epsilon();
const float origin_velocity =
@@ -462,8 +450,7 @@ float64_t calcStopDistance(
// search forward
if (std::fabs(origin_velocity) > zero_velocity) {
for (int64_t i = origin + 1; i < static_cast<int64_t>(current_trajectory.points.size()) - 1;
- ++i)
- {
+ ++i) {
const auto & p0 = current_trajectory.points.at(static_cast<size_t>(i));
const auto & p1 = current_trajectory.points.at(static_cast<size_t>(i - 1));
stop_dist += distance_2d<float64_t>(p0, p1);
diff --git a/control/trajectory_follower/src/pid.cpp b/control/trajectory_follower/src/pid.cpp
index 1d61114b05972..d79f5f5e4f47c 100644
--- a/control/trajectory_follower/src/pid.cpp
+++ b/control/trajectory_follower/src/pid.cpp
@@ -12,13 +12,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "trajectory_follower/pid.hpp"
+
#include <algorithm>
#include <memory>
#include <utility>
#include <vector>
-#include "trajectory_follower/pid.hpp"
-
namespace autoware
{
namespace motion
@@ -28,8 +28,13 @@ namespace control
namespace trajectory_follower
{
PIDController::PIDController()
-: m_error_integral(0.0), m_prev_error(0.0), m_is_first_time(true), m_is_gains_set(false),
- m_is_limits_set(false) {}
+: m_error_integral(0.0),
+ m_prev_error(0.0),
+ m_is_first_time(true),
+ m_is_gains_set(false),
+ m_is_limits_set(false)
+{
+}
float64_t PIDController::calculate(
const float64_t error, const float64_t dt, const bool8_t enable_integration,
@@ -83,9 +88,8 @@ void PIDController::setGains(const float64_t kp, const float64_t ki, const float
void PIDController::setLimits(
const float64_t max_ret, const float64_t min_ret, const float64_t max_ret_p,
- const float64_t min_ret_p,
- const float64_t max_ret_i, const float64_t min_ret_i, const float64_t max_ret_d,
- const float64_t min_ret_d)
+ const float64_t min_ret_p, const float64_t max_ret_i, const float64_t min_ret_i,
+ const float64_t max_ret_d, const float64_t min_ret_d)
{
m_params.max_ret = max_ret;
m_params.min_ret = min_ret;
diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp b/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp
index 609e28b62c568..4fbd57084053b 100644
--- a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp
+++ b/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp
@@ -25,8 +25,7 @@ namespace control
{
namespace trajectory_follower
{
-QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger)
-: logger_{logger} {}
+QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger} {}
bool8_t QPSolverOSQP::solve(
const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a,
const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
@@ -60,14 +59,12 @@ bool8_t QPSolverOSQP::solve(
auto result = osqpsolver_.optimize(h_mat, osqpA, f, lower_bound, upper_bound);
std::vector<float64_t> U_osqp = std::get<0>(result);
- u =
- Eigen::Map<Eigen::Matrix<float64_t, Eigen::Dynamic, 1>>(
- &U_osqp[0],
- static_cast<Eigen::Index>(U_osqp.size()), 1);
+ u = Eigen::Map<Eigen::Matrix<float64_t, Eigen::Dynamic, 1>>(
+ &U_osqp[0], static_cast<Eigen::Index>(U_osqp.size()), 1);
const int64_t status_val = std::get<3>(result);
if (status_val != 1) {
- // TODO(Horibe): Should return false and the failure must be handled in an appropriate way.
+ // TODO(Horibe): Should return false and the failure must be handled in an appropriate way.
RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str());
}
diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp
index 1e2c8b114c1f0..df81256f15338 100644
--- a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp
+++ b/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp
@@ -28,7 +28,9 @@ bool8_t QPSolverEigenLeastSquareLLT::solve(
const Eigen::VectorXd & /*lb*/, const Eigen::VectorXd & /*ub*/, const Eigen::VectorXd & /*lb_a*/,
const Eigen::VectorXd & /*ub_a*/, Eigen::VectorXd & u)
{
- if (std::fabs(h_mat.determinant()) < 1.0E-9) {return false;}
+ if (std::fabs(h_mat.determinant()) < 1.0E-9) {
+ return false;
+ }
u = -h_mat.llt().solve(f_vec);
diff --git a/control/trajectory_follower/src/smooth_stop.cpp b/control/trajectory_follower/src/smooth_stop.cpp
index 3864860eca3eb..fec11d6d71d18 100644
--- a/control/trajectory_follower/src/smooth_stop.cpp
+++ b/control/trajectory_follower/src/smooth_stop.cpp
@@ -12,16 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "trajectory_follower/smooth_stop.hpp"
+
+#include <experimental/optional> // NOLINT
+
#include <algorithm>
#include <cmath>
-#include <experimental/optional> // NOLINT
#include <limits>
#include <utility>
#include <vector>
-#include "trajectory_follower/smooth_stop.hpp"
-
-
namespace autoware
{
namespace motion
@@ -41,15 +41,14 @@ void SmoothStop::init(const float64_t pred_vel_in_target, const float64_t pred_s
}
m_strong_acc = -std::pow(pred_vel_in_target, 2) / (2 * pred_stop_dist);
- m_strong_acc =
- std::max(std::min(m_strong_acc, m_params.max_strong_acc), m_params.min_strong_acc);
+ m_strong_acc = std::max(std::min(m_strong_acc, m_params.max_strong_acc), m_params.min_strong_acc);
}
void SmoothStop::setParams(
float64_t max_strong_acc, float64_t min_strong_acc, float64_t weak_acc, float64_t weak_stop_acc,
float64_t strong_stop_acc, float64_t min_fast_vel, float64_t min_running_vel,
- float64_t min_running_acc,
- float64_t weak_stop_time, float64_t weak_stop_dist, float64_t strong_stop_dist)
+ float64_t min_running_acc, float64_t weak_stop_time, float64_t weak_stop_dist,
+ float64_t strong_stop_dist)
{
m_params.max_strong_acc = max_strong_acc;
m_params.min_strong_acc = min_strong_acc;
@@ -99,9 +98,9 @@ std::experimental::optional<float64_t> SmoothStop::calcTimeToStop(
// return when gradient a (of v = at + b) cannot be calculated.
// See the following calculation of a
- if (std::abs(vel_hist_size * mean_t * mean_t - sum_tt) <
- std::numeric_limits<float64_t>::epsilon())
- {
+ if (
+ std::abs(vel_hist_size * mean_t * mean_t - sum_tt) <
+ std::numeric_limits<float64_t>::epsilon()) {
return {};
}
@@ -138,12 +137,12 @@ float64_t SmoothStop::calculate(
// calculate some flags
const bool8_t is_fast_vel = std::abs(current_vel) > m_params.min_fast_vel;
const bool8_t is_running = std::abs(current_vel) > m_params.min_running_vel ||
- std::abs(current_acc) > m_params.min_running_acc;
+ std::abs(current_acc) > m_params.min_running_acc;
// when exceeding the stopline (stop_dist is negative in these cases.)
- if (stop_dist < m_params.strong_stop_dist) { // when exceeding the stopline much
+ if (stop_dist < m_params.strong_stop_dist) { // when exceeding the stopline much
return m_params.strong_stop_acc;
- } else if (stop_dist < m_params.weak_stop_dist) { // when exceeding the stopline a bit
+ } else if (stop_dist < m_params.weak_stop_dist) { // when exceeding the stopline a bit
return m_params.weak_stop_acc;
}
diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp
index a16d5694d0a89..9576952034b1d 100644
--- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp
+++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp
@@ -25,9 +25,8 @@ namespace control
namespace trajectory_follower
{
DynamicsBicycleModel::DynamicsBicycleModel(
- const float64_t wheelbase, const float64_t mass_fl,
- const float64_t mass_fr, const float64_t mass_rl,
- const float64_t mass_rr, const float64_t cf, const float64_t cr)
+ const float64_t wheelbase, const float64_t mass_fl, const float64_t mass_fr,
+ const float64_t mass_rl, const float64_t mass_rr, const float64_t cf, const float64_t cr)
: VehicleModelInterface(/* dim_x */ 4, /* dim_u */ 1, /* dim_y */ 2, wheelbase)
{
const float64_t mass_front = mass_fl + mass_fr;
@@ -89,8 +88,8 @@ void DynamicsBicycleModel::calculateDiscreteMatrix(
void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref)
{
const float64_t vel = std::max(m_velocity, 0.01);
- const float64_t Kv = m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass /
- (2 * m_cr * m_wheelbase);
+ const float64_t Kv =
+ m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase);
u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature;
}
} // namespace trajectory_follower
diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
index b02ce12d4cca2..90b9151d5e074 100644
--- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
+++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
@@ -36,7 +36,7 @@ void KinematicsBicycleModel::calculateDiscreteMatrix(
Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
const float64_t dt)
{
- auto sign = [](float64_t x) {return (x > 0.0) - (x < 0.0);};
+ auto sign = [](float64_t x) { return (x > 0.0) - (x < 0.0); };
/* Linearize delta around delta_r (reference delta) */
float64_t delta_r = atan(m_wheelbase * m_curvature);
@@ -45,7 +45,9 @@ void KinematicsBicycleModel::calculateDiscreteMatrix(
}
float64_t cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r));
float64_t velocity = m_velocity;
- if (std::abs(m_velocity) < 1e-04) {velocity = 1e-04 * (m_velocity >= 0 ? 1 : -1);}
+ if (std::abs(m_velocity) < 1e-04) {
+ velocity = 1e-04 * (m_velocity >= 0 ? 1 : -1);
+ }
a_d << 0.0, velocity, 0.0, 0.0, 0.0, velocity / m_wheelbase * cos_delta_r_squared_inv, 0.0, 0.0,
-1.0 / m_steer_tau;
@@ -59,7 +61,7 @@ void KinematicsBicycleModel::calculateDiscreteMatrix(
w_d << 0.0,
-velocity * m_curvature +
- velocity / m_wheelbase * (tan(delta_r) - delta_r * cos_delta_r_squared_inv),
+ velocity / m_wheelbase * (tan(delta_r) - delta_r * cos_delta_r_squared_inv),
0.0;
w_d *= dt;
}
diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
index e9399953cce5c..1d6661c9ad8d4 100644
--- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
+++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
@@ -35,7 +35,7 @@ void KinematicsBicycleModelNoDelay::calculateDiscreteMatrix(
Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
const float64_t dt)
{
- auto sign = [](float64_t x) {return (x > 0.0) - (x < 0.0);};
+ auto sign = [](float64_t x) { return (x > 0.0) - (x < 0.0); };
/* Linearize delta around delta_r (reference delta) */
float64_t delta_r = atan(m_wheelbase * m_curvature);
diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp b/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp
index 8344bd0d6f799..122040be315db 100644
--- a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp
+++ b/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp
@@ -23,17 +23,16 @@ namespace control
namespace trajectory_follower
{
VehicleModelInterface::VehicleModelInterface(
- int64_t dim_x, int64_t dim_u, int64_t dim_y,
- float64_t wheelbase)
+ int64_t dim_x, int64_t dim_u, int64_t dim_y, float64_t wheelbase)
: m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase)
{
}
-int64_t VehicleModelInterface::getDimX() {return m_dim_x;}
-int64_t VehicleModelInterface::getDimU() {return m_dim_u;}
-int64_t VehicleModelInterface::getDimY() {return m_dim_y;}
-float64_t VehicleModelInterface::getWheelbase() {return m_wheelbase;}
-void VehicleModelInterface::setVelocity(const float64_t velocity) {m_velocity = velocity;}
-void VehicleModelInterface::setCurvature(const float64_t curvature) {m_curvature = curvature;}
+int64_t VehicleModelInterface::getDimX() { return m_dim_x; }
+int64_t VehicleModelInterface::getDimU() { return m_dim_u; }
+int64_t VehicleModelInterface::getDimY() { return m_dim_y; }
+float64_t VehicleModelInterface::getWheelbase() { return m_wheelbase; }
+void VehicleModelInterface::setVelocity(const float64_t velocity) { m_velocity = velocity; }
+void VehicleModelInterface::setCurvature(const float64_t curvature) { m_curvature = curvature; }
} // namespace trajectory_follower
} // namespace control
} // namespace motion
diff --git a/control/trajectory_follower/test/test_debug_values.cpp b/control/trajectory_follower/test/test_debug_values.cpp
index 35eb7c248e305..b56d4685a76a4 100644
--- a/control/trajectory_follower/test/test_debug_values.cpp
+++ b/control/trajectory_follower/test/test_debug_values.cpp
@@ -15,7 +15,8 @@
#include "gtest/gtest.h"
#include "trajectory_follower/debug_values.hpp"
-TEST(TestDebugValues, assign_and_get) {
+TEST(TestDebugValues, assign_and_get)
+{
using ::autoware::motion::control::trajectory_follower::DebugValues;
DebugValues debug;
diff --git a/control/trajectory_follower/test/test_interpolate.cpp b/control/trajectory_follower/test/test_interpolate.cpp
index 59906bb21c751..e8f9db9ae52f7 100644
--- a/control/trajectory_follower/test/test_interpolate.cpp
+++ b/control/trajectory_follower/test/test_interpolate.cpp
@@ -12,14 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <vector>
-
#include "common/types.hpp"
#include "gtest/gtest.h"
#include "trajectory_follower/interpolate.hpp"
+#include <vector>
+
using autoware::common::types::float64_t;
-TEST(TestInterpolate, Nominal) {
+TEST(TestInterpolate, Nominal)
+{
using autoware::motion::control::trajectory_follower::linearInterpolate;
// Simple case
@@ -30,9 +31,7 @@ TEST(TestInterpolate, Nominal) {
std::vector<float64_t> target_values;
ASSERT_TRUE(
- linearInterpolate(
- original_indexes, original_values, target_indexes,
- target_values));
+ linearInterpolate(original_indexes, original_values, target_indexes, target_values));
ASSERT_EQ(target_values.size(), target_indexes.size());
for (size_t i = 0; i < target_values.size(); ++i) {
EXPECT_EQ(target_values[i], target_indexes[i]);
@@ -46,9 +45,7 @@ TEST(TestInterpolate, Nominal) {
std::vector<float64_t> target_values;
ASSERT_TRUE(
- linearInterpolate(
- original_indexes, original_values, target_indexes,
- target_values));
+ linearInterpolate(original_indexes, original_values, target_indexes, target_values));
ASSERT_EQ(target_values.size(), target_indexes.size());
EXPECT_EQ(target_values[0], 1.5);
EXPECT_EQ(target_values[1], 3.0);
@@ -61,47 +58,31 @@ TEST(TestInterpolate, Nominal) {
float64_t target_index = 1.25;
float64_t target_value;
- ASSERT_TRUE(
- linearInterpolate(
- original_indexes, original_values, target_index,
- target_value));
+ ASSERT_TRUE(linearInterpolate(original_indexes, original_values, target_index, target_value));
EXPECT_EQ(target_value, 1.5);
}
}
-TEST(TestInterpolate, Failure) {
+TEST(TestInterpolate, Failure)
+{
using autoware::motion::control::trajectory_follower::linearInterpolate;
std::vector<float64_t> target_values;
// Non increasing indexes
ASSERT_FALSE(
- linearInterpolate(
- {1.0, 2.0, 1.5, 3.0}, {1.0, 2.0, 3.0, 4.0},
- {1.0, 3.0}, target_values));
+ linearInterpolate({1.0, 2.0, 1.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, {1.0, 3.0}, target_values));
ASSERT_FALSE(
- linearInterpolate(
- {1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0},
- {3.0, 1.0}, target_values));
+ linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, {3.0, 1.0}, target_values));
// Target indexes out of range
ASSERT_FALSE(
- linearInterpolate(
- {1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0},
- {0.0, 3.0}, target_values));
+ linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, {0.0, 3.0}, target_values));
ASSERT_FALSE(
- linearInterpolate(
- {1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0},
- {1.0, 3.5}, target_values));
+ linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, {1.0, 3.5}, target_values));
// Missmatched inputs
- ASSERT_FALSE(
- linearInterpolate(
- {1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0},
- {1.0, 1.5}, target_values));
- ASSERT_FALSE(
- linearInterpolate(
- {1.0, 2.0, 3.0}, {1.0, 2.0, 3.0, 4.0},
- {1.0, 1.5}, target_values));
+ ASSERT_FALSE(linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0}, {1.0, 1.5}, target_values));
+ ASSERT_FALSE(linearInterpolate({1.0, 2.0, 3.0}, {1.0, 2.0, 3.0, 4.0}, {1.0, 1.5}, target_values));
// Input size 0
ASSERT_FALSE(linearInterpolate({}, {}, {1.0, 3.5}, target_values));
diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp
index 54d065eeacd78..35957855a74fa 100644
--- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp
+++ b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp
@@ -12,21 +12,23 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <limits>
-
#include "gtest/gtest.h"
+#include "tf2/LinearMath/Quaternion.h"
#include "trajectory_follower/longitudinal_controller_utils.hpp"
+
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
-#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include <limits>
+
namespace longitudinal_utils = ::autoware::motion::control::trajectory_follower::longitudinal_utils;
-TEST(TestLongitudinalControllerUtils, isValidTrajectory) {
+TEST(TestLongitudinalControllerUtils, isValidTrajectory)
+{
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
Trajectory traj;
@@ -39,7 +41,8 @@ TEST(TestLongitudinalControllerUtils, isValidTrajectory) {
EXPECT_FALSE(longitudinal_utils::isValidTrajectory(traj));
}
-TEST(TestLongitudinalControllerUtils, calcStopDistance) {
+TEST(TestLongitudinalControllerUtils, calcStopDistance)
+{
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using geometry_msgs::msg::Point;
@@ -87,7 +90,8 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) {
EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, traj), 3.0);
}
-TEST(TestLongitudinalControllerUtils, getPitchByPose) {
+TEST(TestLongitudinalControllerUtils, getPitchByPose)
+{
tf2::Quaternion quaternion_tf;
quaternion_tf.setRPY(0.0, 0.0, 0.0);
EXPECT_EQ(longitudinal_utils::getPitchByPose(tf2::toMsg(quaternion_tf)), 0.0);
@@ -95,7 +99,8 @@ TEST(TestLongitudinalControllerUtils, getPitchByPose) {
EXPECT_EQ(longitudinal_utils::getPitchByPose(tf2::toMsg(quaternion_tf)), 1.0);
}
-TEST(TestLongitudinalControllerUtils, getPitchByTraj) {
+TEST(TestLongitudinalControllerUtils, getPitchByTraj)
+{
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
const double wheel_base = 0.9;
@@ -127,16 +132,10 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj) {
traj.points.push_back(point);
size_t closest_idx = 0;
EXPECT_DOUBLE_EQ(
- std::abs(
- longitudinal_utils::getPitchByTraj(
- traj, closest_idx,
- wheel_base)), M_PI_4);
+ std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4);
closest_idx = 1;
EXPECT_DOUBLE_EQ(
- std::abs(
- longitudinal_utils::getPitchByTraj(
- traj, closest_idx,
- wheel_base)), M_PI_4);
+ std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4);
closest_idx = 2;
EXPECT_DOUBLE_EQ(
std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)),
@@ -147,7 +146,8 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj) {
std::atan2(0.5, 1));
}
-TEST(TestLongitudinalControllerUtils, calcElevationAngle) {
+TEST(TestLongitudinalControllerUtils, calcElevationAngle)
+{
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
TrajectoryPoint p_from;
p_from.pose.position.x = 0.0;
@@ -173,7 +173,8 @@ TEST(TestLongitudinalControllerUtils, calcElevationAngle) {
EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4);
}
-TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) {
+TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay)
+{
using geometry_msgs::msg::Pose;
const double abs_err = 1e-7;
Pose current_pose;
@@ -187,9 +188,8 @@ TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) {
// With a delay and/or a velocity of 0.0 there is no change of position
double delay_time = 0.0;
double current_vel = 0.0;
- Pose delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(
- current_pose, delay_time,
- current_vel);
+ Pose delayed_pose =
+ longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
@@ -255,7 +255,8 @@ TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) {
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
}
-TEST(TestLongitudinalControllerUtils, lerpOrientation) {
+TEST(TestLongitudinalControllerUtils, lerpOrientation)
+{
geometry_msgs::msg::Quaternion result;
tf2::Quaternion o_from;
tf2::Quaternion o_to;
@@ -310,7 +311,8 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) {
EXPECT_DOUBLE_EQ(yaw, M_PI_4 / 2);
}
-TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) {
+TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint)
+{
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using geometry_msgs::msg::Point;
const double abs_err = 1e-15;
@@ -408,7 +410,8 @@ TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) {
EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);
}
-TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter) {
+TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter)
+{
double dt = 1.0;
double max_val = 0.0; // cannot increase
double min_val = 0.0; // cannot decrease
@@ -416,46 +419,33 @@ TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter) {
double input_val = 10.0;
EXPECT_DOUBLE_EQ(
- longitudinal_utils::applyDiffLimitFilter(
- input_val, prev_val, dt, max_val,
- min_val), 0.0);
+ longitudinal_utils::applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val), 0.0);
max_val = 1.0; // can only increase by up to 1.0 at a time
EXPECT_DOUBLE_EQ(
- longitudinal_utils::applyDiffLimitFilter(
- input_val, prev_val, dt, max_val,
- min_val), 1.0);
+ longitudinal_utils::applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val), 1.0);
input_val = -10.0;
EXPECT_DOUBLE_EQ(
- longitudinal_utils::applyDiffLimitFilter(
- input_val, prev_val, dt, max_val,
- min_val), 0.0);
+ longitudinal_utils::applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val), 0.0);
min_val = -1.0; // can decrease by up to -1.0 at a time
EXPECT_DOUBLE_EQ(
- longitudinal_utils::applyDiffLimitFilter(
- input_val, prev_val, dt, max_val,
- min_val), -1.0);
+ longitudinal_utils::applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val), -1.0);
dt = 5.0; // can now increase/decrease 5 times more
input_val = 10.0;
EXPECT_DOUBLE_EQ(
- longitudinal_utils::applyDiffLimitFilter(
- input_val, prev_val, dt, max_val,
- min_val), 5.0);
+ longitudinal_utils::applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val), 5.0);
input_val = -10.0;
EXPECT_DOUBLE_EQ(
- longitudinal_utils::applyDiffLimitFilter(
- input_val, prev_val, dt, max_val,
- min_val), -5.0);
+ longitudinal_utils::applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val), -5.0);
dt = 1.0;
input_val = 100.0;
for (double prev = 0.0; prev < 100.0; ++prev) {
- const double new_val = longitudinal_utils::applyDiffLimitFilter(
- input_val, prev, dt, max_val,
- min_val);
+ const double new_val =
+ longitudinal_utils::applyDiffLimitFilter(input_val, prev, dt, max_val, min_val);
EXPECT_DOUBLE_EQ(new_val, prev + max_val);
prev = new_val;
}
diff --git a/control/trajectory_follower/test/test_lowpass_filter.cpp b/control/trajectory_follower/test/test_lowpass_filter.cpp
index ca1f1f9896b08..4432d06d28100 100644
--- a/control/trajectory_follower/test/test_lowpass_filter.cpp
+++ b/control/trajectory_follower/test/test_lowpass_filter.cpp
@@ -12,12 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <vector>
-
#include "common/types.hpp"
#include "gtest/gtest.h"
#include "trajectory_follower/lowpass_filter.hpp"
+#include <vector>
+
using autoware::common::types::float64_t;
TEST(TestLowpassFilter, LowpassFilter1d)
@@ -42,14 +42,15 @@ TEST(TestLowpassFilter, LowpassFilter1d)
EXPECT_NEAR(lowpass_filter_1d.filter(0.0), -0.11, epsilon);
EXPECT_NEAR(lowpass_filter_1d.getValue(), -0.11, epsilon);
}
-TEST(TestLowpassFilter, MoveAverageFilter) {
+TEST(TestLowpassFilter, MoveAverageFilter)
+{
namespace MoveAverageFilter = autoware::motion::control::trajectory_follower::MoveAverageFilter;
{ // Fail case: window size higher than the vector size
const int64_t window_size = 5;
std::vector<float64_t> vec = {1.0, 2.0, 3.0, 4.0};
EXPECT_FALSE(MoveAverageFilter::filt_vector(window_size, vec));
- }
+ } // namespace autoware::motion::control::trajectory_follower::MoveAverageFilter;
{
const int64_t window_size = 0;
const std::vector<float64_t> original_vec = {1.0, 3.0, 4.0, 6.0};
@@ -85,7 +86,8 @@ TEST(TestLowpassFilter, MoveAverageFilter) {
EXPECT_EQ(filtered_vec[5], original_vec[5]);
}
}
-TEST(TestLowpassFilter, Butterworth2dFilter) {
+TEST(TestLowpassFilter, Butterworth2dFilter)
+{
using autoware::motion::control::trajectory_follower::Butterworth2dFilter;
const float64_t dt = 1.0;
const float64_t cutoff_hz = 1.0;
diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/trajectory_follower/test/test_mpc.cpp
index 52c9b60613082..a9cfde53073f2 100644
--- a/control/trajectory_follower/test/test_mpc.cpp
+++ b/control/trajectory_follower/test/test_mpc.cpp
@@ -12,30 +12,29 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-
-#include <memory>
-#include <string>
-#include <vector>
-
+#include "common/types.hpp"
+#include "gtest/gtest.h"
#include "trajectory_follower/mpc.hpp"
-#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp"
#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp"
+#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp"
#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
-#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
-#include "common/types.hpp"
#include "geometry_msgs/msg/pose.hpp"
-#include "gtest/gtest.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include <memory>
+#include <string>
+#include <vector>
+
namespace
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory;
typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint;
@@ -73,7 +72,7 @@ class MPCTest : public ::testing::Test
// Test Parameters
float64_t admissible_position_error = 5.0;
float64_t admissible_yaw_error_rad = M_PI_2;
- float64_t steer_lim = 0.610865; // 35 degrees
+ float64_t steer_lim = 0.610865; // 35 degrees
float64_t steer_rate_lim = 2.61799; // 150 degrees
float64_t ctrl_period = 0.03;
float64_t traj_resample_dist = 0.1;
@@ -178,7 +177,8 @@ class MPCTest : public ::testing::Test
}; // class MPCTest
/* cppcheck-suppress syntaxError */
-TEST_F(MPCTest, InitializeAndCalculate) {
+TEST_F(MPCTest, InitializeAndCalculate)
+{
trajectory_follower::MPC mpc;
EXPECT_FALSE(mpc.hasVehicleModel());
EXPECT_FALSE(mpc.hasQPSolver());
@@ -186,7 +186,7 @@ TEST_F(MPCTest, InitializeAndCalculate) {
const std::string vehicle_model_type = "kinematics";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
std::make_shared<trajectory_follower::KinematicsBicycleModel>(
- wheelbase, steer_limit, steer_tau);
+ wheelbase, steer_limit, steer_tau);
mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type);
ASSERT_TRUE(mpc.hasVehicleModel());
@@ -203,14 +203,13 @@ TEST_F(MPCTest, InitializeAndCalculate) {
Trajectory pred_traj;
Float32MultiArrayDiagnostic diag;
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
}
-TEST_F(MPCTest, InitializeAndCalculateRightTurn) {
+TEST_F(MPCTest, InitializeAndCalculateRightTurn)
+{
trajectory_follower::MPC mpc;
EXPECT_FALSE(mpc.hasVehicleModel());
EXPECT_FALSE(mpc.hasQPSolver());
@@ -218,7 +217,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) {
const std::string vehicle_model_type = "kinematics";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
std::make_shared<trajectory_follower::KinematicsBicycleModel>(
- wheelbase, steer_limit, steer_tau);
+ wheelbase, steer_limit, steer_tau);
mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type);
ASSERT_TRUE(mpc.hasVehicleModel());
@@ -239,14 +238,13 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) {
Trajectory pred_traj;
Float32MultiArrayDiagnostic diag;
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
}
-TEST_F(MPCTest, OsqpCalculate) {
+TEST_F(MPCTest, OsqpCalculate)
+{
trajectory_follower::MPC mpc;
initializeMPC(mpc);
mpc.setReferenceTrajectory(
@@ -257,7 +255,7 @@ TEST_F(MPCTest, OsqpCalculate) {
const std::string vehicle_model_type = "kinematics";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
std::make_shared<trajectory_follower::KinematicsBicycleModel>(
- wheelbase, steer_limit, steer_tau);
+ wheelbase, steer_limit, steer_tau);
mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type);
ASSERT_TRUE(mpc.hasVehicleModel());
@@ -272,14 +270,13 @@ TEST_F(MPCTest, OsqpCalculate) {
Float32MultiArrayDiagnostic diag;
// with OSQP this function returns false despite finding correct solutions
EXPECT_FALSE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
}
-TEST_F(MPCTest, OsqpCalculateRightTurn) {
+TEST_F(MPCTest, OsqpCalculateRightTurn)
+{
trajectory_follower::MPC mpc;
initializeMPC(mpc);
mpc.setReferenceTrajectory(
@@ -290,7 +287,7 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) {
const std::string vehicle_model_type = "kinematics";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
std::make_shared<trajectory_follower::KinematicsBicycleModel>(
- wheelbase, steer_limit, steer_tau);
+ wheelbase, steer_limit, steer_tau);
mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type);
ASSERT_TRUE(mpc.hasVehicleModel());
@@ -304,14 +301,13 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) {
Trajectory pred_traj;
Float32MultiArrayDiagnostic diag;
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
}
-TEST_F(MPCTest, KinematicsNoDelayCalculate) {
+TEST_F(MPCTest, KinematicsNoDelayCalculate)
+{
trajectory_follower::MPC mpc;
initializeMPC(mpc);
@@ -338,14 +334,13 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) {
Trajectory pred_traj;
Float32MultiArrayDiagnostic diag;
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
}
-TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) {
+TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
+{
trajectory_follower::MPC mpc;
initializeMPC(mpc);
mpc.setReferenceTrajectory(
@@ -372,22 +367,20 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) {
Trajectory pred_traj;
Float32MultiArrayDiagnostic diag;
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
}
-TEST_F(MPCTest, DynamicCalculate) {
+TEST_F(MPCTest, DynamicCalculate)
+{
trajectory_follower::MPC mpc;
initializeMPC(mpc);
const std::string vehicle_model_type = "dynamics";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
std::make_shared<trajectory_follower::DynamicsBicycleModel>(
- wheelbase, mass_fl, mass_fr,
- mass_rl, mass_rr, cf, cr);
+ wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr);
mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type);
ASSERT_TRUE(mpc.hasVehicleModel());
@@ -401,19 +394,18 @@ TEST_F(MPCTest, DynamicCalculate) {
Trajectory pred_traj;
Float32MultiArrayDiagnostic diag;
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
}
-TEST_F(MPCTest, MultiSolveWithBuffer) {
+TEST_F(MPCTest, MultiSolveWithBuffer)
+{
trajectory_follower::MPC mpc;
const std::string vehicle_model_type = "kinematics";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
std::make_shared<trajectory_follower::KinematicsBicycleModel>(
- wheelbase, steer_limit, steer_tau);
+ wheelbase, steer_limit, steer_tau);
mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type);
std::shared_ptr<trajectory_follower::QPSolverInterface> qpsolver_ptr =
std::make_shared<trajectory_follower::QPSolverEigenLeastSquareLLT>();
@@ -428,41 +420,34 @@ TEST_F(MPCTest, MultiSolveWithBuffer) {
Trajectory pred_traj;
Float32MultiArrayDiagnostic diag;
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3));
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3));
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3));
ASSERT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3));
}
-TEST_F(MPCTest, FailureCases) {
+TEST_F(MPCTest, FailureCases)
+{
trajectory_follower::MPC mpc;
const std::string vehicle_model_type = "kinematics";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
std::make_shared<trajectory_follower::KinematicsBicycleModel>(
- wheelbase, steer_limit, steer_tau);
+ wheelbase, steer_limit, steer_tau);
mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type);
std::shared_ptr<trajectory_follower::QPSolverInterface> qpsolver_ptr =
std::make_shared<trajectory_follower::QPSolverEigenLeastSquareLLT>();
@@ -479,9 +464,7 @@ TEST_F(MPCTest, FailureCases) {
Trajectory pred_traj;
Float32MultiArrayDiagnostic diag;
EXPECT_FALSE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_far, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_far, ctrl_cmd, pred_traj, diag));
// Calculate MPC with a fast velocity to make the prediction go further than the reference path
EXPECT_FALSE(
@@ -493,8 +476,6 @@ TEST_F(MPCTest, FailureCases) {
wheelbase, steer_limit, steer_tau);
mpc.setVehicleModel(vehicle_model_ptr, wrong_vehicle_model_type);
EXPECT_TRUE(
- mpc.calculateMPC(
- neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj,
- diag));
+ mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag));
}
} // namespace
diff --git a/control/trajectory_follower/test/test_mpc_trajectory.cpp b/control/trajectory_follower/test/test_mpc_trajectory.cpp
index 95835077567c8..6fa874f38bee0 100644
--- a/control/trajectory_follower/test/test_mpc_trajectory.cpp
+++ b/control/trajectory_follower/test/test_mpc_trajectory.cpp
@@ -12,14 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <vector>
-
#include "common/types.hpp"
#include "gtest/gtest.h"
#include "trajectory_follower/mpc_trajectory.hpp"
+#include <vector>
+
using autoware::common::types::float64_t;
-TEST(TestMPCTrajectory, Nominal) {
+TEST(TestMPCTrajectory, Nominal)
+{
typedef autoware::motion::control::trajectory_follower::MPCTrajectory MPCTrajectory;
MPCTrajectory traj;
diff --git a/control/trajectory_follower/test/test_mpc_utils.cpp b/control/trajectory_follower/test/test_mpc_utils.cpp
index d01b26d958e24..cc0c94cb6710e 100644
--- a/control/trajectory_follower/test/test_mpc_utils.cpp
+++ b/control/trajectory_follower/test/test_mpc_utils.cpp
@@ -12,17 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-
-#include <memory>
-#include <vector>
-
-#include "trajectory_follower/mpc_utils.hpp"
+#include "gtest/gtest.h"
#include "trajectory_follower/mpc_trajectory.hpp"
+#include "trajectory_follower/mpc_utils.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
#include "geometry_msgs/msg/pose.hpp"
-#include "gtest/gtest.h"
+
+#include <memory>
+#include <vector>
namespace
{
@@ -31,7 +30,8 @@ typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory;
typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint;
typedef geometry_msgs::msg::Pose Pose;
-TEST(TestMPCUtils, CalcNearestIndex) {
+TEST(TestMPCUtils, CalcNearestIndex)
+{
Pose pose;
pose.position.x = 0.0;
pose.position.y = 0.0;
@@ -53,7 +53,8 @@ TEST(TestMPCUtils, CalcNearestIndex) {
}
/* cppcheck-suppress syntaxError */
-TEST(TestMPC, CalcStopDistance) {
+TEST(TestMPC, CalcStopDistance)
+{
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
diff --git a/control/trajectory_follower/test/test_pid.cpp b/control/trajectory_follower/test/test_pid.cpp
index dc92912d2bb7a..8c7db8f6af255 100644
--- a/control/trajectory_follower/test/test_pid.cpp
+++ b/control/trajectory_follower/test/test_pid.cpp
@@ -12,12 +12,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <vector>
-
#include "gtest/gtest.h"
#include "trajectory_follower/pid.hpp"
-TEST(TestPID, calculate_pid_output) {
+#include <vector>
+
+TEST(TestPID, calculate_pid_output)
+{
using ::autoware::motion::control::trajectory_follower::PIDController;
std::vector<double> contributions;
const double dt = 1.0;
diff --git a/control/trajectory_follower/test/test_smooth_stop.cpp b/control/trajectory_follower/test/test_smooth_stop.cpp
index 43d289d08b77d..d03ee02c7d109 100644
--- a/control/trajectory_follower/test/test_smooth_stop.cpp
+++ b/control/trajectory_follower/test/test_smooth_stop.cpp
@@ -12,17 +12,18 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <utility>
-#include <vector>
-
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "trajectory_follower/smooth_stop.hpp"
-TEST(TestSmoothStop, calculate_stopping_acceleration) {
+#include <utility>
+#include <vector>
+
+TEST(TestSmoothStop, calculate_stopping_acceleration)
+{
using ::autoware::motion::control::trajectory_follower::SmoothStop;
- using rclcpp::Time;
using rclcpp::Duration;
+ using rclcpp::Time;
const double max_strong_acc = -0.5;
const double min_strong_acc = -1.0;
@@ -97,16 +98,14 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) {
vel_in_target = 1.0;
ss.init(vel_in_target, stop_dist);
EXPECT_EQ(
- ss.calculate(
- stop_dist, current_vel, current_acc, velocity_history,
- delay_time), max_strong_acc);
+ ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
+ max_strong_acc);
vel_in_target = std::sqrt(2.0);
ss.init(vel_in_target, stop_dist);
EXPECT_EQ(
- ss.calculate(
- stop_dist, current_vel, current_acc, velocity_history,
- delay_time), min_strong_acc);
+ ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
+ min_strong_acc);
for (double vel_in_target = 1.1; vel_in_target < std::sqrt(2.0); vel_in_target += 0.1) {
ss.init(vel_in_target, stop_dist);
diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt
index 4ec4da305ca35..9912f72f36510 100644
--- a/control/trajectory_follower_nodes/CMakeLists.txt
+++ b/control/trajectory_follower_nodes/CMakeLists.txt
@@ -79,9 +79,9 @@ if(BUILD_TESTING)
target_link_libraries(${TRAJECTORY_FOLLOWER_NODES_TEST} ${LATLON_MUXER_NODE} ${LATERAL_CONTROLLER_NODE} ${LONGITUDINAL_CONTROLLER_NODE})
#find_package(autoware_testing REQUIRED)
- #add_smoke_test(${PROJECT_NAME} ${LATLON_MUXER_NODE}_exe PARAM_FILENAME latlon_muxer_defaults.yaml)
- #add_smoke_test(${PROJECT_NAME} ${LATERAL_CONTROLLER_NODE}_exe PARAM_FILENAME lateral_controller_defaults.yaml)
- #add_smoke_test(${PROJECT_NAME} ${LONGITUDINAL_CONTROLLER_NODE}_exe PARAM_FILENAME longitudinal_controller_defaults.yaml)
+ #add_smoke_test(${PROJECT_NAME} ${LATLON_MUXER_NODE}_exe PARAM_FILENAME latlon_muxer_defaults.param.yaml)
+ #add_smoke_test(${PROJECT_NAME} ${LATERAL_CONTROLLER_NODE}_exe PARAM_FILENAME lateral_controller_defaults.param.yaml)
+ #add_smoke_test(${PROJECT_NAME} ${LONGITUDINAL_CONTROLLER_NODE}_exe PARAM_FILENAME longitudinal_controller_defaults.param.yaml)
endif()
ament_auto_package(
diff --git a/control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml b/control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml
index 289ffd8dddacc..06e34a91c4050 100644
--- a/control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml
+++ b/control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml
@@ -273,4 +273,3 @@
<snippets/>
<!-- - - - - - - - - - - - - - - -->
</root>
-
diff --git a/control/trajectory_follower_nodes/design/lateral_controller-design.md b/control/trajectory_follower_nodes/design/lateral_controller-design.md
index b4ce2c3831b6c..d4955e2d26f10 100644
--- a/control/trajectory_follower_nodes/design/lateral_controller-design.md
+++ b/control/trajectory_follower_nodes/design/lateral_controller-design.md
@@ -1,20 +1,23 @@
-Lateral Controller {#lateral-controller-design}
-=============================================
+# Lateral Controller {#lateral-controller-design}
This is the design document for the lateral controller node
in the `trajectory_follower_nodes` package.
-# Purpose / Use cases
+## Purpose / Use cases
+
<!-- Required -->
<!-- Things to consider:
- Why did we implement this feature? -->
+
This node is used to general lateral control commands (steering angle and steering rate)
when following a path.
-# Design
+## Design
+
<!-- Required -->
<!-- Things to consider:
- How does it work? -->
+
The node uses an implementation of linear model predictive control (MPC) for accurate path tracking.
The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command.
The optimization of the control command is formulated as a Quadratic Program (QP).
@@ -22,24 +25,30 @@ The optimization of the control command is formulated as a Quadratic Program (QP
These functionalities are implemented in the `trajectory_follower` package
(see @subpage trajectory_follower-mpc-design)
-## Assumptions / Known limits
+### Assumptions / Known limits
+
<!-- Required -->
+
The tracking is not accurate if the first point of the reference trajectory is at or in front of the current ego pose.
- - Issue to add points behind ego: https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1273
-## Inputs / Outputs / API
+- Issue to add points behind ego: <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1273>
+
+### Inputs / Outputs / API
+
<!-- Required -->
<!-- Things to consider:
- How do you use the package / API? -->
+
Inputs
- - `input/reference_trajectory` : reference trajectory to follow.
- - `input/current_kinematic_state`: current state of the vehicle (position, velocity, etc).
-Output
- - `output/lateral_control_cmd`: generated lateral control command.
-## Parameter description
+- `input/reference_trajectory` : reference trajectory to follow.
+- `input/current_kinematic_state`: current state of the vehicle (position, velocity, etc).
+ Output
+- `output/lateral_control_cmd`: generated lateral control command.
-The default parameters defined in `param/lateral_controller_defaults.yaml` are adjusted to the
+### Parameter description
+
+The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the
AutonomouStuff Lexus RX 450h for under 40 km/h driving.
| Name | Type | Description | Default value |
@@ -60,7 +69,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state.
-### MPC algorithm
+#### MPC algorithm
| Name | Type | Description | Default value |
| :-------------------------------------- | :----- | :---------------------------------------------------------------------------------------------- | :---------------- |
@@ -78,7 +87,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
| weight_terminal_heading_error | double | terminal cost weight for heading error | 0.1 |
| zero_ff_steer_deg | double | threshold of feedforward angle [deg]. feedforward angle smaller than this value is set to zero. | 2.0 |
-### Vehicle
+#### Vehicle
| Name | Type | Description | Default value |
| :------------ | :----- | :--------------------------------------------------------------------------------- | :------------ |
@@ -93,24 +102,24 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
| steering_tau | double | steering dynamics time constant (1d approximation) for vehicle model [s] | 0.3 |
| steer_lim_deg | double | steering angle limit for vehicle model [deg]. This is also used for QP constraint. | 35.0 |
-## How to tune MPC parameters
+### How to tune MPC parameters
1. Set appropriate vehicle kinematics parameters for distance to front and rear axle, and `steer_lim_deg`.
-Also check that the input `VehicleKinematicState` has appropriate values (speed: vehicle rear-wheels-center velocity [km/h], angle: steering (tire) angle [rad]).
-These values give a vehicle information to the controller for path following.
-Errors in these values cause fundamental tracking error.
+ Also check that the input `VehicleKinematicState` has appropriate values (speed: vehicle rear-wheels-center velocity [km/h], angle: steering (tire) angle [rad]).
+ These values give a vehicle information to the controller for path following.
+ Errors in these values cause fundamental tracking error.
2. Set appropriate vehicle dynamics parameters of `steering_tau`, which is the approximated delay from steering angle command to actual steering angle.
3. Set `weight_steering_input` = 1.0, `weight_lat_error` = 0.1, and other weights to 0.
-If the vehicle oscillates when driving with low speed, set `weight_lat_error` smaller.
+ If the vehicle oscillates when driving with low speed, set `weight_lat_error` smaller.
4. Adjust other weights.
-One of the simple way for tuning is to increase `weight_lat_error` until oscillation occurs.
-If the vehicle is unstable with very small `weight_lat_error`, increase terminal weight :
-`weight_terminal_lat_error` and `weight_terminal_heading_error` to improve tracking stability.
-Larger `prediction_horizon` and smaller `prediction_sampling_time` is effective for tracking performance, but it is a trade-off between computational costs.
-Other parameters can be adjusted like below.
+ One of the simple way for tuning is to increase `weight_lat_error` until oscillation occurs.
+ If the vehicle is unstable with very small `weight_lat_error`, increase terminal weight :
+ `weight_terminal_lat_error` and `weight_terminal_heading_error` to improve tracking stability.
+ Larger `prediction_horizon` and smaller `prediction_sampling_time` is effective for tracking performance, but it is a trade-off between computational costs.
+ Other parameters can be adjusted like below.
- `weight_lat_error`: Reduce lateral tracking error. This acts like P gain in PID.
- `weight_heading_error`: Make a drive straight. This acts like D gain in PID.
@@ -121,6 +130,8 @@ Other parameters can be adjusted like below.
- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability.
- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability.
-# Related issues
+## Related issues
+
<!-- Required -->
-- https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057
+
+- <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057>
diff --git a/control/trajectory_follower_nodes/design/latlon_muxer-design.md b/control/trajectory_follower_nodes/design/latlon_muxer-design.md
index 31458fe29b802..2d64b36324d1d 100644
--- a/control/trajectory_follower_nodes/design/latlon_muxer-design.md
+++ b/control/trajectory_follower_nodes/design/latlon_muxer-design.md
@@ -1,29 +1,32 @@
-Lateral/Longitudinal Control Muxer {#latlon-muxer-design}
-=============================================
+# Lateral/Longitudinal Control Muxer {#latlon-muxer-design}
-# Purpose
+## Purpose
When using controllers that independently compute lateral and longitudinal commands,
this node combines the resulting messages into a single control command message.
-# Design
+## Design
Inputs.
+
- `AckermannLateralCommand`: lateral command.
- `LongitudinalCommand`: longitudinal command.
Output.
+
- `AckermannControlCommand`: message containing both lateral and longitudinal commands.
Parameter.
+
- `timeout_thr_sec`: duration in second after which input messages are discarded.
Each time the node receives an input message it publishes an `AckermannControlCommand`
if the following two conditions are met.
+
1. Both inputs have been received.
2. The last received input messages are not older than defined by `timeout_thr_sec`.
-# Implementation Details
+## Implementation Details
Callbacks `latCtrlCmdCallback` and `lonCtrlCmdCallback` are defined for each input message.
Upon reception, the message is stored and function `publishCmd` is called.
diff --git a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md b/control/trajectory_follower_nodes/design/longitudinal_controller-design.md
index 76d4f19bca2e0..3348257181ca9 100644
--- a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md
+++ b/control/trajectory_follower_nodes/design/longitudinal_controller-design.md
@@ -1,6 +1,6 @@
# Longitudinal Controller {#longitudinal-controller-design}
-# Purpose / Use cases
+## Purpose / Use cases
The longitudinal_controller computes the target acceleration to achieve the target velocity set at each point of the target trajectory using a feed-forward/back control.
@@ -32,7 +32,7 @@ The lat-lon mixed control problem is very complex and uses nonlinear optimizatio
Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed.
-# Design
+## Design
## Assumptions / Known limits
@@ -58,9 +58,9 @@ Also, the benefits of simultaneous longitudinal and lateral control are small if
- current_state [`autoware_auto_msgs/VehicleKinematicState`] : Current ego state including the current pose and velocity.
- current_trajectory [`autoware_auto_msgs/Trajectory`] : Current target trajectory for the desired velocity on the each trajectory points.
-# Inner-workings / Algorithms
+## Inner-workings / Algorithms
-## States
+### States
This module has four state transitions as shown below in order to handle special processing in a specific situation.
@@ -80,19 +80,19 @@ The state transition diagram is shown below.
-## Logics
+### Logics
-### Control Block Diagram
+#### Control Block Diagram
-### FeedForward (FF)
+#### FeedForward (FF)
The reference acceleration set in the trajectory and slope compensation terms are output as a feedforward. Under ideal conditions with no modeling error, this FF term alone should be sufficient for velocity tracking.
Tracking errors causing modeling or discretization errors are removed by the feedback control (now using PID).
-#### Brake keeping
+##### Brake keeping
From the viewpoint of ride comfort, stopping with 0 acceleration is important because it reduces the impact of braking. However, if the target acceleration when stopping is 0, the vehicle may cross over the stop line or accelerate a little in front of the stop line due to vehicle model error or gradient estimation error.
@@ -100,7 +100,7 @@ For reliable stopping, the target acceleration calculated by the FeedForward sys
-### Slope compensation
+#### Slope compensation
Based on the slope information, a compensation term is added to the target acceleration.
@@ -117,7 +117,7 @@ There are two sources of the slope information, which can be switched by a param
- Cons: z-coordinates of high-precision map is needed.
- Cons: Does not support free space planning (for now)
-### PID control
+#### PID control
For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system.
@@ -134,16 +134,16 @@ On the other hand, if the vehicle gets stuck in a depression in the road surface
At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance.
This may be replaced by a higher performance controller (adaptive control or robust control) in future development.
-### Time delay compensation
+#### Time delay compensation
At high speeds, the delay of actuator systems such as gas pedals and brakes has a significant impact on driving accuracy.
Depending on the actuating principle of the vehicle, the mechanism that physically controls the gas pedal and brake typically has a delay of about a hundred millisecond.
In this controller, the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem.
-## Parameter description
+### Parameter description
-The default parameters defined in `param/lateral_controller_defaults.yaml` are adjusted to the
+The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the
AutonomouStuff Lexus RX 450h for under 40 km/h driving.
| Name | Type | Description | Default value |
@@ -163,7 +163,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
| max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 |
| min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 |
-### State transition
+#### State transition
| Name | Type | Description | Default value |
| :---------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
@@ -176,7 +176,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
| emergency_state_traj_trans_dev | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m] | 3.0 |
| emergency_state_traj_rot_dev | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad] | 0.784 |
-### DRIVE Parameter
+#### DRIVE Parameter
| Name | Type | Description | Default value |
| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
@@ -195,7 +195,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | 0.5 |
| brake_keeping_acc | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2 |
-### STOPPING Parameter (smooth stop)
+#### STOPPING Parameter (smooth stop)
Smooth stop is enabled if `enable_smooth_stop` is true.
In smooth stop, strong acceleration (`strong_acc`) will be output first to decrease the ego velocity.
@@ -217,7 +217,7 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig
| smooth_stop_weak_stop_dist | double | Weak acceleration will be output when the ego is `smooth_stop_weak_stop_dist`-meter before the stop point. [m] | -0.3 |
| smooth_stop_strong_stop_dist | double | Strong acceleration will be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m] | -0.5 |
-### STOPPED Parameter
+#### STOPPED Parameter
| Name | Type | Description | Default value |
| :----------- | :----- | :------------------------------------------- | :------------ |
@@ -225,7 +225,7 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig
| stopped_acc | double | target acceleration in STOPPED state [m/s^2] | -3.4 |
| stopped_jerk | double | target jerk in STOPPED state [m/s^3] | -5.0 |
-### EMERGENCY Parameter
+#### EMERGENCY Parameter
| Name | Type | Description | Default value |
| :------------- | :----- | :------------------------------------------------ | :------------ |
@@ -233,10 +233,10 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig
| emergency_acc | double | target acceleration in an EMERGENCY state [m/s^2] | -5.0 |
| emergency_jerk | double | target jerk in an EMERGENCY state [m/s^3] | -3.0 |
-# References / External links
+## References / External links
-# Future extensions / Unimplemented parts
+## Future extensions / Unimplemented parts
-# Related issues
+## Related issues
- <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058>
diff --git a/control/trajectory_follower_nodes/design/trajectory_follower-design.md b/control/trajectory_follower_nodes/design/trajectory_follower-design.md
index aee0370bda671..a106d119bb6ad 100644
--- a/control/trajectory_follower_nodes/design/trajectory_follower-design.md
+++ b/control/trajectory_follower_nodes/design/trajectory_follower-design.md
@@ -1,26 +1,26 @@
-Trajectory Follower Nodes {#trajectory_follower_nodes-package-design}
-=============================================
+# Trajectory Follower Nodes {#trajectory_follower_nodes-package-design}
-# Purpose
+## Purpose
Generate control commands to follow a given Trajectory.
-# Design
+## Design
This functionality is decomposed into three nodes.
+
- @subpage lateral-controller-design : generates lateral control messages.
- @subpage longitudinal-controller-design : generates longitudinal control messages.
- @subpage latlon-muxer-design : combines the lateral and longitudinal control commands
-into a single control command.
+ into a single control command.
Core functionalities are implemented in the @subpage trajectory_follower-package-design package.
@image html images/trajectory_follower-diagram.png "Overview of the Trajectory Follower package"
-# Debugging
+## Debugging
Debug information are published by the lateral and longitudinal controller using `autoware_auto_msgs/Float32MultiArrayDiagnostic` messages.
A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging.
-In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz.
\ No newline at end of file
+In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz.
diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp
index f1680f05cea43..089292d97021a 100644
--- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp
+++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp
@@ -15,12 +15,13 @@
#ifndef TRAJECTORY_FOLLOWER_NODES__LATERAL_CONTROLLER_NODE_HPP_
#define TRAJECTORY_FOLLOWER_NODES__LATERAL_CONTROLLER_NODE_HPP_
-#include <deque>
-#include <memory>
-#include <string>
-#include <vector>
-
-#include "trajectory_follower_nodes/visibility_control.hpp"
+#include "common/types.hpp"
+#include "osqp_interface/osqp_interface.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "tf2/utils.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
#include "trajectory_follower/interpolate.hpp"
#include "trajectory_follower/lowpass_filter.hpp"
#include "trajectory_follower/mpc.hpp"
@@ -31,25 +32,23 @@
#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
+#include "trajectory_follower_nodes/visibility_control.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
-#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
-#include "common/types.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
-#include "osqp_interface/osqp_interface.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_components/register_node_macro.hpp"
-#include "tf2/utils.h"
#include "tf2_msgs/msg/tf_message.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "vehicle_info_util/vehicle_info_util.hpp"
+#include <deque>
+#include <memory>
+#include <string>
+#include <vector>
namespace autoware
{
@@ -59,8 +58,8 @@ namespace control
{
namespace trajectory_follower_nodes
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node
@@ -104,7 +103,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node
int64_t m_path_filter_moving_ave_num;
//!< @brief point-to-point index distance for curvature calculation for trajectory //NOLINT
int64_t m_curvature_smoothing_num_traj;
- //!< @brief point-to-point index distance for curvature calculation for reference steer command //NOLINT
+ //!< @brief point-to-point index distance for curvature calculation for reference steer command
+ //!< //NOLINT
int64_t m_curvature_smoothing_num_ref_steer;
//!< @brief path resampling interval [m]
float64_t m_traj_resample_dist;
@@ -123,8 +123,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node
//!< @brief measured steering
autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr m_current_steering_ptr;
//!< @brief reference trajectory
- autoware_auto_planning_msgs::msg::Trajectory::SharedPtr
- m_current_trajectory_ptr;
+ autoware_auto_planning_msgs::msg::Trajectory::SharedPtr m_current_trajectory_ptr;
//!< @brief mpc filtered output in previous period
float64_t m_steer_cmd_prev = 0.0;
@@ -187,8 +186,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node
* @brief publish diagnostic message
* @param [in] diagnostic published diagnostic
*/
- void publishDiagnostic(autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic)
- const;
+ void publishDiagnostic(
+ autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) const;
/**
* @brief get stop command
diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp
index bcfdc4c05c1c0..59a23a0966090 100644
--- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp
+++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp
@@ -15,18 +15,17 @@
#ifndef TRAJECTORY_FOLLOWER_NODES__LATLON_MUXER_NODE_HPP_
#define TRAJECTORY_FOLLOWER_NODES__LATLON_MUXER_NODE_HPP_
-#include <memory>
-#include <string>
-
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
#include "trajectory_follower_nodes/visibility_control.hpp"
#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/time.hpp"
-#include "rclcpp_components/register_node_macro.hpp"
+#include <memory>
+#include <string>
namespace autoware
{
@@ -46,16 +45,19 @@ class LatLonMuxer : public rclcpp::Node
private:
// \brief Callback for the lateral control command
- void latCtrlCmdCallback(const autoware_auto_control_msgs::msg::AckermannLateralCommand::SharedPtr msg);
+ void latCtrlCmdCallback(
+ const autoware_auto_control_msgs::msg::AckermannLateralCommand::SharedPtr msg);
// \brief Callback for the longitudinal control command
- void lonCtrlCmdCallback(const autoware_auto_control_msgs::msg::LongitudinalCommand::SharedPtr msg);
+ void lonCtrlCmdCallback(
+ const autoware_auto_control_msgs::msg::LongitudinalCommand::SharedPtr msg);
// \brief Publish the combined control command message
void publishCmd();
// \brief Check that the received messages are not too old
// \return bool True if the stored messages timed out
bool checkTimeout();
- rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr m_control_cmd_pub;
+ rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+ m_control_cmd_pub;
rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannLateralCommand>::SharedPtr
m_lat_control_cmd_sub;
rclcpp::Subscription<autoware_auto_control_msgs::msg::LongitudinalCommand>::SharedPtr
diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp
index 87d17db4a8f80..1f4900835d943 100644
--- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp
+++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp
@@ -15,24 +15,12 @@
#ifndef TRAJECTORY_FOLLOWER_NODES__LONGITUDINAL_CONTROLLER_NODE_HPP_
#define TRAJECTORY_FOLLOWER_NODES__LONGITUDINAL_CONTROLLER_NODE_HPP_
-#include <memory>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
-#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Geometry"
-#include "geometry_msgs/msg/pose_stamped.hpp"
#include "motion_common/motion_common.hpp"
#include "motion_common/trajectory_common.hpp"
-#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/utils.h"
-#include "tf2_msgs/msg/tf_message.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "trajectory_follower/debug_values.hpp"
@@ -42,6 +30,19 @@
#include "trajectory_follower/smooth_stop.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"
+#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
+#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
namespace autoware
{
namespace motion
@@ -50,8 +51,8 @@ namespace control
{
namespace trajectory_follower_nodes
{
-using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
+using autoware::common::types::float64_t;
namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
namespace motion_common = ::autoware::motion::motion_common;
@@ -83,12 +84,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node
};
// ros variables
- rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr
- m_sub_current_velocity;
+ rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr m_sub_current_velocity;
rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr m_sub_trajectory;
- rclcpp::Publisher<autoware_auto_control_msgs::msg::LongitudinalCommand>::SharedPtr m_pub_control_cmd;
- rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr m_pub_slope;
- rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr m_pub_debug;
+ rclcpp::Publisher<autoware_auto_control_msgs::msg::LongitudinalCommand>::SharedPtr
+ m_pub_control_cmd;
+ rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr
+ m_pub_slope;
+ rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr
+ m_pub_debug;
rclcpp::TimerBase::SharedPtr m_timer_control;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_sub;
@@ -209,8 +212,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node
* @brief set current and previous velocity with received message
* @param [in] msg current state message
*/
- void callbackCurrentVelocity(
- const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+ void callbackCurrentVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
/**
* @brief set reference trajectory with received message
@@ -284,7 +286,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node
enum Shift getCurrentShift(const size_t nearest_idx) const;
/**
- * @brief filter acceleration command with limitation of acceleration and jerk, and slope compensation
+ * @brief filter acceleration command with limitation of acceleration and jerk, and slope
+ * compensation
* @param [in] raw_acc acceleration before filtered
* @param [in] control_data data for control calculation
*/
@@ -303,8 +306,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node
* @param [in] shift direction that vehicle move (forward or backward)
*/
float64_t applySlopeCompensation(
- const float64_t acc, const float64_t pitch,
- const Shift shift) const;
+ const float64_t acc, const float64_t pitch, const Shift shift) const;
/**
* @brief keep target motion acceleration negative before stop
@@ -322,8 +324,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node
* @param [in] nearest_idx index of the trajectory point nearest to the vehicle position
*/
autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue(
- const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Point & point,
- const size_t nearest_idx) const;
+ const autoware_auto_planning_msgs::msg::Trajectory & traj,
+ const geometry_msgs::msg::Point & point, const size_t nearest_idx) const;
/**
* @brief calculate predicted velocity after time delay based on past control commands
@@ -335,7 +337,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node
/**
* @brief calculate velocity feedback with feed forward and pid controller
- * @param [in] target_motion reference velocity and acceleration. This acceleration will be used as feed forward.
+ * @param [in] target_motion reference velocity and acceleration. This acceleration will be used
+ * as feed forward.
* @param [in] dt time step to use
* @param [in] current_vel current velocity of the vehicle
*/
@@ -349,8 +352,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node
* @param [in] raw_pitch current raw pitch of the vehicle (unfiltered)
*/
void updatePitchDebugValues(
- const float64_t pitch, const float64_t traj_pitch,
- const float64_t raw_pitch);
+ const float64_t pitch, const float64_t traj_pitch, const float64_t raw_pitch);
/**
* @brief update variables for velocity and acceleration
diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp
index 8d169f7516c3d..e4f75f3e456b6 100644
--- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp
+++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp
@@ -17,21 +17,21 @@
////////////////////////////////////////////////////////////////////////////////
#if defined(__WIN32)
- #if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
- #define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport)
- #define TRAJECTORY_FOLLOWER_LOCAL
- #else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
- #define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport)
- #define TRAJECTORY_FOLLOWER_LOCAL
- #endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
+#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
+#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport)
+#define TRAJECTORY_FOLLOWER_LOCAL
+#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
+#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport)
+#define TRAJECTORY_FOLLOWER_LOCAL
+#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS)
#elif defined(__linux__)
- #define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default")))
- #define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden")))
+#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default")))
+#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
- #define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default")))
- #define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden")))
+#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default")))
+#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden")))
#else
- #error "Unsupported Build Configuration"
+#error "Unsupported Build Configuration"
#endif
#endif // TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_
diff --git a/control/trajectory_follower_nodes/package.xml b/control/trajectory_follower_nodes/package.xml
index 215ebaea5187b..253afb67faedd 100644
--- a/control/trajectory_follower_nodes/package.xml
+++ b/control/trajectory_follower_nodes/package.xml
@@ -10,23 +10,23 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_auto_cmake</buildtool_depend>
- <depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_control_msgs</depend>
- <depend>autoware_auto_vehicle_msgs</depend>
+ <depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_system_msgs</depend>
- <depend>trajectory_follower</depend>
+ <depend>autoware_auto_vehicle_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
+ <depend>trajectory_follower</depend>
<depend>vehicle_info_util</depend>
<exec_depend>ros2launch</exec_depend>
<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_index_python</test_depend>
- <!-- <test_depend>ament_lint_auto</test_depend> -->
- <!-- <test_depend>ament_lint_common</test_depend> -->
<test_depend>autoware_testing</test_depend>
<test_depend>fake_test_node</test_depend>
+ <!-- <test_depend>ament_lint_auto</test_depend> -->
+ <!-- <test_depend>ament_lint_common</test_depend> -->
<export>
<build_type>ament_cmake</build_type>
diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml
new file mode 100644
index 0000000000000..e692fa8e14811
--- /dev/null
+++ b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml
@@ -0,0 +1,67 @@
+/**:
+ ros__parameters:
+ # -- system --
+ ctrl_period: 0.03 # control period [s]
+ traj_resample_dist: 0.1 # path resampling interval [m]
+ use_steer_prediction: false # flag for using steer prediction (do not use steer measurement)
+ admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value
+ admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value
+
+ # -- path smoothing --
+ enable_path_smoothing: false # flag for path smoothing
+ path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing
+ curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num)
+ curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
+
+ # -- mpc optimization --
+ qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp)
+ mpc_prediction_horizon: 50 # prediction horizon step
+ mpc_prediction_dt: 0.1 # prediction horizon period [s]
+ mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q
+ mpc_weight_heading_error: 0.0 # heading error weight in matrix Q
+ mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q
+ mpc_weight_steering_input: 1.0 # steering error weight in matrix R
+ mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R
+ mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R
+ mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R
+ mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R
+ mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point
+ mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point
+ mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point
+ mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point
+ mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point
+ mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point
+ mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point
+ mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point
+ mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03)
+ mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability
+ mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability
+ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero
+ mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration
+ mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing
+
+ # -- vehicle model --
+ vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
+ input_delay: 0.24 # steering input delay time for delay compensation
+ vehicle_model_steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s]
+ steer_lim_deg: 20.0 # steering angle limit [deg]
+ steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s]
+ acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss]
+ velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s]
+
+ # -- lowpass filter for noise reduction --
+ steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz]
+ error_deriv_lpf_cutoff_hz: 5.0
+
+ # stop state: steering command is kept in the previous value in the stop state.
+ stop_state_entry_ego_speed: 0.001
+ stop_state_entry_target_speed: 0.001
+
+ # vehicle parameters
+ mass_kg: 2400.0
+ mass_fl: 600.0
+ mass_fr: 600.0
+ mass_rl: 600.0
+ mass_rr: 600.0
+ cf: 155494.663
+ cr: 155494.663
diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.yaml
deleted file mode 100644
index 5a3f4edcd516a..0000000000000
--- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.yaml
+++ /dev/null
@@ -1,68 +0,0 @@
-/**:
- ros__parameters:
-
- # -- system --
- ctrl_period: 0.03 # control period [s]
- traj_resample_dist: 0.1 # path resampling interval [m]
- use_steer_prediction: false # flag for using steer prediction (do not use steer measurement)
- admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value
- admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value
-
- # -- path smoothing --
- enable_path_smoothing: false # flag for path smoothing
- path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing
- curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num)
- curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
-
- # -- mpc optimization --
- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp)
- mpc_prediction_horizon: 50 # prediction horizon step
- mpc_prediction_dt: 0.1 # prediction horizon period [s]
- mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q
- mpc_weight_heading_error: 0.0 # heading error weight in matrix Q
- mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q
- mpc_weight_steering_input: 1.0 # steering error weight in matrix R
- mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R
- mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R
- mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R
- mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R
- mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point
- mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point
- mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point
- mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point
- mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point
- mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point
- mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point
- mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point
- mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03)
- mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability
- mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability
- mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero
- mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration
- mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing
-
- # -- vehicle model --
- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
- input_delay: 0.24 # steering input delay time for delay compensation
- vehicle_model_steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s]
- steer_lim_deg: 20.0 # steering angle limit [deg]
- steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s]
- acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss]
- velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s]
-
- # -- lowpass filter for noise reduction --
- steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz]
- error_deriv_lpf_cutoff_hz: 5.0
-
- # stop state: steering command is kept in the previous value in the stop state.
- stop_state_entry_ego_speed: 0.001
- stop_state_entry_target_speed: 0.001
-
- # vehicle parameters
- mass_kg: 2400.0
- mass_fl: 600.0
- mass_fr: 600.0
- mass_rl: 600.0
- mass_rr: 600.0
- cf: 155494.663
- cr: 155494.663
diff --git a/control/trajectory_follower_nodes/param/latlon_muxer_defaults.yaml b/control/trajectory_follower_nodes/param/latlon_muxer_defaults.param.yaml
similarity index 100%
rename from control/trajectory_follower_nodes/param/latlon_muxer_defaults.yaml
rename to control/trajectory_follower_nodes/param/latlon_muxer_defaults.param.yaml
diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.yaml b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml
similarity index 100%
rename from control/trajectory_follower_nodes/param/longitudinal_controller_defaults.yaml
rename to control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml
diff --git a/control/trajectory_follower_nodes/param/test_vehicle_info.yaml b/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml
similarity index 100%
rename from control/trajectory_follower_nodes/param/test_vehicle_info.yaml
rename to control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml
diff --git a/control/trajectory_follower_nodes/src/lateral_controller_node.cpp b/control/trajectory_follower_nodes/src/lateral_controller_node.cpp
index 7569f1c2a8709..4b8e609429da9 100644
--- a/control/trajectory_follower_nodes/src/lateral_controller_node.cpp
+++ b/control/trajectory_follower_nodes/src/lateral_controller_node.cpp
@@ -12,6 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "trajectory_follower_nodes/lateral_controller_node.hpp"
+
+#include "tf2_ros/create_timer_ros.h"
+
#include <algorithm>
#include <deque>
#include <limits>
@@ -20,10 +24,6 @@
#include <utility>
#include <vector>
-#include "trajectory_follower_nodes/lateral_controller_node.hpp"
-
-#include "tf2_ros/create_timer_ros.h"
-
namespace autoware
{
namespace motion
@@ -34,15 +34,15 @@ namespace trajectory_follower_nodes
{
namespace
{
-using namespace std::chrono_literals;
+using namespace std::literals::chrono_literals;
-template<typename T>
+template <typename T>
void update_param(
const std::vector<rclcpp::Parameter> & parameters, const std::string & name, T & value)
{
auto it = std::find_if(
parameters.cbegin(), parameters.cend(),
- [&name](const rclcpp::Parameter & parameter) {return parameter.get_name() == name;});
+ [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; });
if (it != parameters.cend()) {
value = static_cast<T>(it->template get_value<T>());
}
@@ -58,19 +58,17 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options)
m_enable_path_smoothing = declare_parameter<bool8_t>("enable_path_smoothing");
m_path_filter_moving_ave_num = declare_parameter<int64_t>("path_filter_moving_ave_num");
m_curvature_smoothing_num_traj = declare_parameter<int64_t>("curvature_smoothing_num_traj");
- m_curvature_smoothing_num_ref_steer = declare_parameter<int64_t>(
- "curvature_smoothing_num_ref_steer");
+ m_curvature_smoothing_num_ref_steer =
+ declare_parameter<int64_t>("curvature_smoothing_num_ref_steer");
m_traj_resample_dist = declare_parameter<float64_t>("traj_resample_dist");
- m_mpc.m_admissible_position_error =
- declare_parameter<float64_t>("admissible_position_error");
+ m_mpc.m_admissible_position_error = declare_parameter<float64_t>("admissible_position_error");
m_mpc.m_admissible_yaw_error_rad = declare_parameter<float64_t>("admissible_yaw_error_rad");
m_mpc.m_use_steer_prediction = declare_parameter<bool8_t>("use_steer_prediction");
m_mpc.m_param.steer_tau = declare_parameter<float64_t>("vehicle_model_steer_tau");
/* stop state parameters */
m_stop_state_entry_ego_speed = declare_parameter<float64_t>("stop_state_entry_ego_speed");
- m_stop_state_entry_target_speed =
- declare_parameter<float64_t>("stop_state_entry_target_speed");
+ m_stop_state_entry_target_speed = declare_parameter<float64_t>("stop_state_entry_target_speed");
/* mpc parameters */
const float64_t steer_lim_deg = declare_parameter<float64_t>("steer_lim_deg");
@@ -85,10 +83,8 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options)
const std::string vehicle_model_type = declare_parameter<std::string>("vehicle_model_type");
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr;
if (vehicle_model_type == "kinematics") {
- vehicle_model_ptr =
- std::make_shared<trajectory_follower::KinematicsBicycleModel>(
- wheelbase, m_mpc.m_steer_lim,
- m_mpc.m_param.steer_tau);
+ vehicle_model_ptr = std::make_shared<trajectory_follower::KinematicsBicycleModel>(
+ wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau);
} else if (vehicle_model_type == "kinematics_no_delay") {
vehicle_model_ptr = std::make_shared<trajectory_follower::KinematicsBicycleModelNoDelay>(
wheelbase, m_mpc.m_steer_lim);
@@ -100,7 +96,8 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options)
const float64_t cf = declare_parameter<float64_t>("vehicle.cf");
const float64_t cr = declare_parameter<float64_t>("vehicle.cr");
- // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time // NOLINT
+ // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time
+ // // NOLINT
vehicle_model_ptr = std::make_shared<trajectory_follower::DynamicsBicycleModel>(
wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr);
} else {
@@ -128,8 +125,7 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options)
/* initialize lowpass filter */
{
- const float64_t steering_lpf_cutoff_hz =
- declare_parameter<float64_t>("steering_lpf_cutoff_hz");
+ const float64_t steering_lpf_cutoff_hz = declare_parameter<float64_t>("steering_lpf_cutoff_hz");
const float64_t error_deriv_lpf_cutoff_hz =
declare_parameter<float64_t>("error_deriv_lpf_cutoff_hz");
m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
@@ -138,25 +134,21 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options)
/* set up ros system */
initTimer(m_mpc.m_ctrl_period);
- m_pub_ctrl_cmd =
- create_publisher<autoware_auto_control_msgs::msg::AckermannLateralCommand>(
+ m_pub_ctrl_cmd = create_publisher<autoware_auto_control_msgs::msg::AckermannLateralCommand>(
"~/output/control_cmd", 1);
- m_pub_predicted_traj =
- create_publisher<autoware_auto_planning_msgs::msg::Trajectory>(
- "~/output/predicted_trajectory",
- 1);
- m_pub_diagnostic =
- create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
+ m_pub_predicted_traj = create_publisher<autoware_auto_planning_msgs::msg::Trajectory>(
+ "~/output/predicted_trajectory", 1);
+ m_pub_diagnostic = create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
"~/output/diagnostic", 1);
m_sub_ref_path = create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
"~/input/reference_trajectory", rclcpp::QoS{1},
std::bind(&LateralController::onTrajectory, this, _1));
m_sub_steering = create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>(
- "~/input/current_steering", rclcpp::QoS{1}, std::bind(
- &LateralController::onSteering, this, _1));
+ "~/input/current_steering", rclcpp::QoS{1},
+ std::bind(&LateralController::onSteering, this, _1));
m_sub_odometry = create_subscription<nav_msgs::msg::Odometry>(
- "~/input/current_odometry", rclcpp::QoS{1}, std::bind(
- &LateralController::onOdometry, this, _1));
+ "~/input/current_odometry", rclcpp::QoS{1},
+ std::bind(&LateralController::onOdometry, this, _1));
// TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations
declareMPCparameters();
@@ -194,8 +186,8 @@ void LateralController::onTimer()
}
const bool8_t is_mpc_solved = m_mpc.calculateMPC(
- *m_current_steering_ptr, m_current_odometry_ptr->twist.twist.linear.x,
- m_current_pose_ptr->pose, ctrl_cmd, predicted_traj, diagnostic);
+ *m_current_steering_ptr, m_current_odometry_ptr->twist.twist.linear.x, m_current_pose_ptr->pose,
+ ctrl_cmd, predicted_traj, diagnostic);
if (isStoppedState()) {
// Reset input buffer
@@ -213,8 +205,7 @@ void LateralController::onTimer()
if (!is_mpc_solved) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(
- get_logger(), *get_clock(), 5000 /*ms*/,
- "MPC is not solved. publish 0 velocity.");
+ get_logger(), *get_clock(), 5000 /*ms*/, "MPC is not solved. publish 0 velocity.");
ctrl_cmd = getStopControlCommand();
}
@@ -227,27 +218,23 @@ void LateralController::onTimer()
bool8_t LateralController::checkData() const
{
if (!m_mpc.hasVehicleModel()) {
- RCLCPP_DEBUG(
- get_logger(), "MPC does not have a vehicle model");
+ RCLCPP_DEBUG(get_logger(), "MPC does not have a vehicle model");
return false;
}
if (!m_mpc.hasQPSolver()) {
- RCLCPP_DEBUG(
- get_logger(), "MPC does not have a QP solver");
+ RCLCPP_DEBUG(get_logger(), "MPC does not have a QP solver");
return false;
}
if (!m_current_odometry_ptr) {
RCLCPP_DEBUG(
- get_logger(), "waiting data. current_velocity = %d",
- m_current_odometry_ptr != nullptr);
+ get_logger(), "waiting data. current_velocity = %d", m_current_odometry_ptr != nullptr);
return false;
}
if (!m_current_steering_ptr) {
RCLCPP_DEBUG(
- get_logger(), "waiting data. current_steering = %d",
- m_current_steering_ptr != nullptr);
+ get_logger(), "waiting data. current_steering = %d", m_current_steering_ptr != nullptr);
return false;
}
@@ -289,16 +276,11 @@ bool8_t LateralController::updateCurrentPose()
geometry_msgs::msg::TransformStamped transform;
try {
transform = m_tf_buffer.lookupTransform(
- m_current_trajectory_ptr->header.frame_id,
- "base_link",
- tf2::TimePointZero);
+ m_current_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero);
} catch (tf2::TransformException & ex) {
+ RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000 /*ms*/, ex.what());
RCLCPP_WARN_SKIPFIRST_THROTTLE(
- get_logger(), *get_clock(), 5000 /*ms*/,
- ex.what());
- RCLCPP_WARN_SKIPFIRST_THROTTLE(
- get_logger(), *get_clock(), 5000 /*ms*/,
- m_tf_buffer.allFramesAsString().c_str());
+ get_logger(), *get_clock(), 5000 /*ms*/, m_tf_buffer.allFramesAsString().c_str());
return false;
}
@@ -324,7 +306,7 @@ void LateralController::onSteering(
}
autoware_auto_control_msgs::msg::AckermannLateralCommand LateralController::getStopControlCommand()
-const
+ const
{
autoware_auto_control_msgs::msg::AckermannLateralCommand cmd;
cmd.steering_tire_angle = static_cast<decltype(cmd.steering_tire_angle)>(m_steer_cmd_prev);
@@ -332,8 +314,8 @@ const
return cmd;
}
-autoware_auto_control_msgs::msg::AckermannLateralCommand LateralController::getInitialControlCommand()
-const
+autoware_auto_control_msgs::msg::AckermannLateralCommand
+LateralController::getInitialControlCommand() const
{
autoware_auto_control_msgs::msg::AckermannLateralCommand cmd;
cmd.steering_tire_angle = m_current_steering_ptr->steering_tire_angle;
@@ -348,19 +330,19 @@ bool8_t LateralController::isStoppedState() const
// control was turned off when approaching/exceeding the stop line on a curve or
// emergency stop situation and it caused large tracking error.
const int64_t nearest = trajectory_follower::MPCUtils::calcNearestIndex(
- *m_current_trajectory_ptr,
- m_current_pose_ptr->pose);
+ *m_current_trajectory_ptr, m_current_pose_ptr->pose);
// If the nearest index is not found, return false
- if (nearest < 0) {return false;}
+ if (nearest < 0) {
+ return false;
+ }
const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x;
const float64_t target_vel =
m_current_trajectory_ptr->points.at(static_cast<size_t>(nearest)).longitudinal_velocity_mps;
if (
std::fabs(current_vel) < m_stop_state_entry_ego_speed &&
- std::fabs(target_vel) < m_stop_state_entry_target_speed)
- {
+ std::fabs(target_vel) < m_stop_state_entry_target_speed) {
return true;
} else {
return false;
@@ -376,8 +358,7 @@ void LateralController::publishCtrlCmd(
}
void LateralController::publishPredictedTraj(
- autoware_auto_planning_msgs::msg::Trajectory & predicted_traj)
-const
+ autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const
{
predicted_traj.header.stamp = this->now();
predicted_traj.header.frame_id = m_current_trajectory_ptr->header.frame_id;
@@ -385,8 +366,7 @@ const
}
void LateralController::publishDiagnostic(
- autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic)
-const
+ autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) const
{
diagnostic.diag_header.data_stamp = this->now();
diagnostic.diag_header.name = std::string("linear-MPC lateral controller");
@@ -397,8 +377,8 @@ void LateralController::initTimer(float64_t period_s)
{
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<float64_t>(period_s));
- m_timer = rclcpp::create_timer(this, get_clock(), period_ns,
- std::bind(&LateralController::onTimer, this));
+ m_timer = rclcpp::create_timer(
+ this, get_clock(), period_ns, std::bind(&LateralController::onTimer, this));
}
void LateralController::declareMPCparameters()
@@ -406,43 +386,40 @@ void LateralController::declareMPCparameters()
m_mpc.m_param.prediction_horizon = declare_parameter<int64_t>("mpc_prediction_horizon");
m_mpc.m_param.prediction_dt = declare_parameter<float64_t>("mpc_prediction_dt");
m_mpc.m_param.weight_lat_error = declare_parameter<float64_t>("mpc_weight_lat_error");
- m_mpc.m_param.weight_heading_error =
- declare_parameter<float64_t>("mpc_weight_heading_error");
- m_mpc.m_param.weight_heading_error_squared_vel = declare_parameter<float64_t>(
- "mpc_weight_heading_error_squared_vel");
- m_mpc.m_param.weight_steering_input =
- declare_parameter<float64_t>("mpc_weight_steering_input");
- m_mpc.m_param.weight_steering_input_squared_vel = declare_parameter<float64_t>(
- "mpc_weight_steering_input_squared_vel");
+ m_mpc.m_param.weight_heading_error = declare_parameter<float64_t>("mpc_weight_heading_error");
+ m_mpc.m_param.weight_heading_error_squared_vel =
+ declare_parameter<float64_t>("mpc_weight_heading_error_squared_vel");
+ m_mpc.m_param.weight_steering_input = declare_parameter<float64_t>("mpc_weight_steering_input");
+ m_mpc.m_param.weight_steering_input_squared_vel =
+ declare_parameter<float64_t>("mpc_weight_steering_input_squared_vel");
m_mpc.m_param.weight_lat_jerk = declare_parameter<float64_t>("mpc_weight_lat_jerk");
m_mpc.m_param.weight_steer_rate = declare_parameter<float64_t>("mpc_weight_steer_rate");
m_mpc.m_param.weight_steer_acc = declare_parameter<float64_t>("mpc_weight_steer_acc");
- m_mpc.m_param.low_curvature_weight_lat_error = declare_parameter<float64_t>(
- "mpc_low_curvature_weight_lat_error");
- m_mpc.m_param.low_curvature_weight_heading_error = declare_parameter<float64_t>(
- "mpc_low_curvature_weight_heading_error");
- m_mpc.m_param.low_curvature_weight_heading_error_squared_vel = declare_parameter<float64_t>(
- "mpc_low_curvature_weight_heading_error_squared_vel");
- m_mpc.m_param.low_curvature_weight_steering_input = declare_parameter<float64_t>(
- "mpc_low_curvature_weight_steering_input");
- m_mpc.m_param.low_curvature_weight_steering_input_squared_vel = declare_parameter<float64_t>(
- "mpc_low_curvature_weight_steering_input_squared_vel");
- m_mpc.m_param.low_curvature_weight_lat_jerk = declare_parameter<float64_t>(
- "mpc_low_curvature_weight_lat_jerk");
- m_mpc.m_param.low_curvature_weight_steer_rate = declare_parameter<float64_t>(
- "mpc_low_curvature_weight_steer_rate");
- m_mpc.m_param.low_curvature_weight_steer_acc = declare_parameter<float64_t>(
- "mpc_low_curvature_weight_steer_acc");
- m_mpc.m_param.low_curvature_thresh_curvature = declare_parameter<float64_t>(
- "mpc_low_curvature_thresh_curvature");
+ m_mpc.m_param.low_curvature_weight_lat_error =
+ declare_parameter<float64_t>("mpc_low_curvature_weight_lat_error");
+ m_mpc.m_param.low_curvature_weight_heading_error =
+ declare_parameter<float64_t>("mpc_low_curvature_weight_heading_error");
+ m_mpc.m_param.low_curvature_weight_heading_error_squared_vel =
+ declare_parameter<float64_t>("mpc_low_curvature_weight_heading_error_squared_vel");
+ m_mpc.m_param.low_curvature_weight_steering_input =
+ declare_parameter<float64_t>("mpc_low_curvature_weight_steering_input");
+ m_mpc.m_param.low_curvature_weight_steering_input_squared_vel =
+ declare_parameter<float64_t>("mpc_low_curvature_weight_steering_input_squared_vel");
+ m_mpc.m_param.low_curvature_weight_lat_jerk =
+ declare_parameter<float64_t>("mpc_low_curvature_weight_lat_jerk");
+ m_mpc.m_param.low_curvature_weight_steer_rate =
+ declare_parameter<float64_t>("mpc_low_curvature_weight_steer_rate");
+ m_mpc.m_param.low_curvature_weight_steer_acc =
+ declare_parameter<float64_t>("mpc_low_curvature_weight_steer_acc");
+ m_mpc.m_param.low_curvature_thresh_curvature =
+ declare_parameter<float64_t>("mpc_low_curvature_thresh_curvature");
m_mpc.m_param.weight_terminal_lat_error =
declare_parameter<float64_t>("mpc_weight_terminal_lat_error");
- m_mpc.m_param.weight_terminal_heading_error = declare_parameter<float64_t>(
- "mpc_weight_terminal_heading_error");
+ m_mpc.m_param.weight_terminal_heading_error =
+ declare_parameter<float64_t>("mpc_weight_terminal_heading_error");
m_mpc.m_param.zero_ff_steer_deg = declare_parameter<float64_t>("mpc_zero_ff_steer_deg");
m_mpc.m_param.acceleration_limit = declare_parameter<float64_t>("mpc_acceleration_limit");
- m_mpc.m_param.velocity_time_constant =
- declare_parameter<float64_t>("mpc_velocity_time_constant");
+ m_mpc.m_param.velocity_time_constant = declare_parameter<float64_t>("mpc_velocity_time_constant");
}
rcl_interfaces::msg::SetParametersResult LateralController::paramCallback(
@@ -460,18 +437,15 @@ rcl_interfaces::msg::SetParametersResult LateralController::paramCallback(
update_param(parameters, "mpc_weight_lat_error", param.weight_lat_error);
update_param(parameters, "mpc_weight_heading_error", param.weight_heading_error);
update_param(
- parameters, "mpc_weight_heading_error_squared_vel",
- param.weight_heading_error_squared_vel);
+ parameters, "mpc_weight_heading_error_squared_vel", param.weight_heading_error_squared_vel);
update_param(parameters, "mpc_weight_steering_input", param.weight_steering_input);
update_param(
- parameters, "mpc_weight_steering_input_squared_vel",
- param.weight_steering_input_squared_vel);
+ parameters, "mpc_weight_steering_input_squared_vel", param.weight_steering_input_squared_vel);
update_param(parameters, "mpc_weight_lat_jerk", param.weight_lat_jerk);
update_param(parameters, "mpc_weight_steer_rate", param.weight_steer_rate);
update_param(parameters, "mpc_weight_steer_acc", param.weight_steer_acc);
update_param(
- parameters, "mpc_low_curvature_weight_lat_error",
- param.low_curvature_weight_lat_error);
+ parameters, "mpc_low_curvature_weight_lat_error", param.low_curvature_weight_lat_error);
update_param(
parameters, "mpc_low_curvature_weight_heading_error",
param.low_curvature_weight_heading_error);
@@ -485,21 +459,16 @@ rcl_interfaces::msg::SetParametersResult LateralController::paramCallback(
parameters, "mpc_low_curvature_weight_steering_input_squared_vel",
param.low_curvature_weight_steering_input_squared_vel);
update_param(
- parameters, "mpc_low_curvature_weight_lat_jerk",
- param.low_curvature_weight_lat_jerk);
+ parameters, "mpc_low_curvature_weight_lat_jerk", param.low_curvature_weight_lat_jerk);
update_param(
- parameters, "mpc_low_curvature_weight_steer_rate",
- param.low_curvature_weight_steer_rate);
+ parameters, "mpc_low_curvature_weight_steer_rate", param.low_curvature_weight_steer_rate);
update_param(
- parameters, "mpc_low_curvature_weight_steer_acc",
- param.low_curvature_weight_steer_acc);
+ parameters, "mpc_low_curvature_weight_steer_acc", param.low_curvature_weight_steer_acc);
update_param(
- parameters, "mpc_low_curvature_thresh_curvature",
- param.low_curvature_thresh_curvature);
+ parameters, "mpc_low_curvature_thresh_curvature", param.low_curvature_thresh_curvature);
update_param(parameters, "mpc_weight_terminal_lat_error", param.weight_terminal_lat_error);
update_param(
- parameters, "mpc_weight_terminal_heading_error",
- param.weight_terminal_heading_error);
+ parameters, "mpc_weight_terminal_heading_error", param.weight_terminal_heading_error);
update_param(parameters, "mpc_zero_ff_steer_deg", param.zero_ff_steer_deg);
update_param(parameters, "mpc_acceleration_limit", param.acceleration_limit);
update_param(parameters, "mpc_velocity_time_constant", param.velocity_time_constant);
@@ -533,8 +502,7 @@ bool8_t LateralController::isValidTrajectory(
!isfinite(p.pose.orientation.y) || !isfinite(p.pose.orientation.z) ||
!isfinite(p.longitudinal_velocity_mps) || !isfinite(p.lateral_velocity_mps) ||
!isfinite(p.lateral_velocity_mps) || !isfinite(p.heading_rate_rps) ||
- !isfinite(p.front_wheel_angle_rad) || !isfinite(p.rear_wheel_angle_rad))
- {
+ !isfinite(p.front_wheel_angle_rad) || !isfinite(p.rear_wheel_angle_rad)) {
return false;
}
}
diff --git a/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp b/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp
index f083615312feb..d50033ecb3199 100644
--- a/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp
+++ b/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp
@@ -12,11 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "trajectory_follower_nodes/latlon_muxer_node.hpp"
+
#include <functional>
#include <memory>
-#include "trajectory_follower_nodes/latlon_muxer_node.hpp"
-
namespace autoware
{
namespace motion
@@ -29,16 +29,13 @@ namespace trajectory_follower_nodes
LatLonMuxer::LatLonMuxer(const rclcpp::NodeOptions & node_options)
: rclcpp::Node("latlon_muxer", node_options)
{
- m_control_cmd_pub =
- create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
- "~/output/control_cmd",
- rclcpp::QoS{1}.transient_local());
+ m_control_cmd_pub = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
+ "~/output/control_cmd", rclcpp::QoS{1}.transient_local());
m_lat_control_cmd_sub =
create_subscription<autoware_auto_control_msgs::msg::AckermannLateralCommand>(
- "~/input/lateral/control_cmd", rclcpp::QoS{1},
- std::bind(&LatLonMuxer::latCtrlCmdCallback, this, std::placeholders::_1));
- m_lon_control_cmd_sub =
- create_subscription<autoware_auto_control_msgs::msg::LongitudinalCommand>(
+ "~/input/lateral/control_cmd", rclcpp::QoS{1},
+ std::bind(&LatLonMuxer::latCtrlCmdCallback, this, std::placeholders::_1));
+ m_lon_control_cmd_sub = create_subscription<autoware_auto_control_msgs::msg::LongitudinalCommand>(
"~/input/longitudinal/control_cmd", rclcpp::QoS{1},
std::bind(&LatLonMuxer::lonCtrlCmdCallback, this, std::placeholders::_1));
m_timeout_thr_sec = declare_parameter<double>("timeout_thr_sec");
@@ -49,15 +46,13 @@ bool LatLonMuxer::checkTimeout()
const auto now = this->now();
if ((now - m_lat_cmd->stamp).seconds() > m_timeout_thr_sec) {
RCLCPP_ERROR_THROTTLE(
- get_logger(),
- *get_clock(), 1000 /*ms*/,
+ get_logger(), *get_clock(), 1000 /*ms*/,
"Lateral control command too old, muxed command will not be published.");
return false;
}
if ((now - m_lon_cmd->stamp).seconds() > m_timeout_thr_sec) {
RCLCPP_ERROR_THROTTLE(
- get_logger(),
- *get_clock(), 1000 /*ms*/,
+ get_logger(), *get_clock(), 1000 /*ms*/,
"Longitudinal control command too old, muxed command will not be published.");
return false;
}
diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp
index 39f872a479985..835a554d21db3 100644
--- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp
+++ b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp
@@ -12,19 +12,18 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <algorithm>
-#include <limits>
-#include <memory>
-#include <string>
-#include <utility>
-#include <vector>
-
#include "trajectory_follower_nodes/longitudinal_controller_node.hpp"
#include "motion_common/motion_common.hpp"
#include "motion_common/trajectory_common.hpp"
#include "time_utils/time_utils.hpp"
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
namespace autoware
{
@@ -57,23 +56,20 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_
auto & p = m_state_transition_params;
// drive
p.drive_state_stop_dist = declare_parameter<float64_t>("drive_state_stop_dist"); // [m]
- p.drive_state_offset_stop_dist = declare_parameter<float64_t>(
- "drive_state_offset_stop_dist"); // [m]
+ p.drive_state_offset_stop_dist =
+ declare_parameter<float64_t>("drive_state_offset_stop_dist"); // [m]
// stopping
- p.stopping_state_stop_dist =
- declare_parameter<float64_t>("stopping_state_stop_dist"); // [m]
+ p.stopping_state_stop_dist = declare_parameter<float64_t>("stopping_state_stop_dist"); // [m]
// stop
- p.stopped_state_entry_vel =
- declare_parameter<float64_t>("stopped_state_entry_vel"); // [m/s]
- p.stopped_state_entry_acc =
- declare_parameter<float64_t>("stopped_state_entry_acc"); // [m/s²]
+ p.stopped_state_entry_vel = declare_parameter<float64_t>("stopped_state_entry_vel"); // [m/s]
+ p.stopped_state_entry_acc = declare_parameter<float64_t>("stopped_state_entry_acc"); // [m/s²]
// emergency
- p.emergency_state_overshoot_stop_dist = declare_parameter<float64_t>(
- "emergency_state_overshoot_stop_dist"); // [m]
- p.emergency_state_traj_trans_dev = declare_parameter<float64_t>(
- "emergency_state_traj_trans_dev"); // [m]
- p.emergency_state_traj_rot_dev = declare_parameter<float64_t>(
- "emergency_state_traj_rot_dev"); // [m]
+ p.emergency_state_overshoot_stop_dist =
+ declare_parameter<float64_t>("emergency_state_overshoot_stop_dist"); // [m]
+ p.emergency_state_traj_trans_dev =
+ declare_parameter<float64_t>("emergency_state_traj_trans_dev"); // [m]
+ p.emergency_state_traj_rot_dev =
+ declare_parameter<float64_t>("emergency_state_traj_rot_dev"); // [m]
}
// parameters for drive state
@@ -100,8 +96,8 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_
m_lpf_vel_error =
std::make_shared<trajectory_follower::LowpassFilter1d>(0.0, lpf_vel_error_gain);
- m_current_vel_threshold_pid_integrate = declare_parameter<float64_t>(
- "current_vel_threshold_pid_integration"); // [m/s]
+ m_current_vel_threshold_pid_integrate =
+ declare_parameter<float64_t>("current_vel_threshold_pid_integration"); // [m/s]
m_enable_brake_keeping_before_stop =
declare_parameter<bool8_t>("enable_brake_keeping_before_stop"); // [-]
@@ -110,30 +106,29 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_
// parameters for smooth stop state
{
- const float64_t max_strong_acc{declare_parameter<float64_t>(
- "smooth_stop_max_strong_acc")}; // [m/s^2]
- const float64_t min_strong_acc{declare_parameter<float64_t>(
- "smooth_stop_min_strong_acc")}; // [m/s^2]
- const float64_t weak_acc{declare_parameter<float64_t>(
- "smooth_stop_weak_acc")}; // [m/s^2]
- const float64_t weak_stop_acc{declare_parameter<float64_t>(
- "smooth_stop_weak_stop_acc")}; // [m/s^2]
- const float64_t strong_stop_acc{declare_parameter<float64_t>(
- "smooth_stop_strong_stop_acc")}; // [m/s^2]
-
- const float64_t max_fast_vel{declare_parameter<float64_t>(
- "smooth_stop_max_fast_vel")}; // [m/s]
- const float64_t min_running_vel{declare_parameter<float64_t>(
- "smooth_stop_min_running_vel")}; // [m/s]
- const float64_t min_running_acc{declare_parameter<float64_t>(
- "smooth_stop_min_running_acc")}; // [m/s^2]
- const float64_t weak_stop_time{declare_parameter<float64_t>(
- "smooth_stop_weak_stop_time")}; // [s]
-
- const float64_t weak_stop_dist{declare_parameter<float64_t>(
- "smooth_stop_weak_stop_dist")}; // [m]
- const float64_t strong_stop_dist{declare_parameter<float64_t>(
- "smooth_stop_strong_stop_dist")}; // [m]
+ const float64_t max_strong_acc{
+ declare_parameter<float64_t>("smooth_stop_max_strong_acc")}; // [m/s^2]
+ const float64_t min_strong_acc{
+ declare_parameter<float64_t>("smooth_stop_min_strong_acc")}; // [m/s^2]
+ const float64_t weak_acc{declare_parameter<float64_t>("smooth_stop_weak_acc")}; // [m/s^2]
+ const float64_t weak_stop_acc{
+ declare_parameter<float64_t>("smooth_stop_weak_stop_acc")}; // [m/s^2]
+ const float64_t strong_stop_acc{
+ declare_parameter<float64_t>("smooth_stop_strong_stop_acc")}; // [m/s^2]
+
+ const float64_t max_fast_vel{
+ declare_parameter<float64_t>("smooth_stop_max_fast_vel")}; // [m/s]
+ const float64_t min_running_vel{
+ declare_parameter<float64_t>("smooth_stop_min_running_vel")}; // [m/s]
+ const float64_t min_running_acc{
+ declare_parameter<float64_t>("smooth_stop_min_running_acc")}; // [m/s^2]
+ const float64_t weak_stop_time{
+ declare_parameter<float64_t>("smooth_stop_weak_stop_time")}; // [s]
+
+ const float64_t weak_stop_dist{
+ declare_parameter<float64_t>("smooth_stop_weak_stop_dist")}; // [m]
+ const float64_t strong_stop_dist{
+ declare_parameter<float64_t>("smooth_stop_strong_stop_dist")}; // [m]
m_smooth_stop.setParams(
max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel,
@@ -143,32 +138,32 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_
// parameters for stop state
{
auto & p = m_stopped_state_params;
- p.vel = declare_parameter<float64_t>("stopped_vel"); // [m/s]
- p.acc = declare_parameter<float64_t>("stopped_acc"); // [m/s^2]
+ p.vel = declare_parameter<float64_t>("stopped_vel"); // [m/s]
+ p.acc = declare_parameter<float64_t>("stopped_acc"); // [m/s^2]
p.jerk = declare_parameter<float64_t>("stopped_jerk"); // [m/s^3]
}
// parameters for emergency state
{
auto & p = m_emergency_state_params;
- p.vel = declare_parameter<float64_t>("emergency_vel"); // [m/s]
+ p.vel = declare_parameter<float64_t>("emergency_vel"); // [m/s]
p.acc = declare_parameter<float64_t>("emergency_acc"); // [m/s^2]
p.jerk = declare_parameter<float64_t>("emergency_jerk"); // [m/s^3]
}
// parameters for acceleration limit
- m_max_acc = declare_parameter<float64_t>("max_acc"); // [m/s^2]
+ m_max_acc = declare_parameter<float64_t>("max_acc"); // [m/s^2]
m_min_acc = declare_parameter<float64_t>("min_acc"); // [m/s^2]
// parameters for jerk limit
- m_max_jerk = declare_parameter<float64_t>("max_jerk"); // [m/s^3]
+ m_max_jerk = declare_parameter<float64_t>("max_jerk"); // [m/s^3]
m_min_jerk = declare_parameter<float64_t>("min_jerk"); // [m/s^3]
// parameters for slope compensation
m_use_traj_for_pitch = declare_parameter<bool8_t>("use_trajectory_for_pitch_calculation");
const float64_t lpf_pitch_gain{declare_parameter<float64_t>("lpf_pitch_gain")};
m_lpf_pitch = std::make_shared<trajectory_follower::LowpassFilter1d>(0.0, lpf_pitch_gain);
- m_max_pitch_rad = declare_parameter<float64_t>("max_pitch_rad"); // [rad]
+ m_max_pitch_rad = declare_parameter<float64_t>("max_pitch_rad"); // [rad]
m_min_pitch_rad = declare_parameter<float64_t>("min_pitch_rad"); // [rad]
// subscriber, publisher
@@ -185,20 +180,16 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_
m_pub_debug = create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
"~/output/diagnostic", rclcpp::QoS{1});
-
// Timer
{
const auto period_ns = rclcpp::Rate(m_control_rate).period();
- m_timer_control = rclcpp::create_timer(this, get_clock(), period_ns,
- std::bind(&LongitudinalController::callbackTimerControl, this));
+ m_timer_control = rclcpp::create_timer(
+ this, get_clock(), period_ns, std::bind(&LongitudinalController::callbackTimerControl, this));
}
// set parameter callback
- m_set_param_res =
- this->add_on_set_parameters_callback(
- std::bind(
- &LongitudinalController::paramCallback, this,
- _1));
+ m_set_param_res = this->add_on_set_parameters_callback(
+ std::bind(&LongitudinalController::paramCallback, this, _1));
// set lowpass filter for acc
m_lpf_acc = std::make_shared<trajectory_follower::LowpassFilter1d>(0.0, 0.2);
@@ -217,16 +208,13 @@ void LongitudinalController::callbackTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
{
if (!trajectory_follower::longitudinal_utils::isValidTrajectory(*msg)) {
- RCLCPP_ERROR_THROTTLE(
- get_logger(), *get_clock(), 3000,
- "received invalid trajectory. ignore.");
+ RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 3000, "received invalid trajectory. ignore.");
return;
}
if (msg->points.size() < 2) {
RCLCPP_WARN_THROTTLE(
- get_logger(), *get_clock(), 3000,
- "Unexpected trajectory size < 2. Ignored.");
+ get_logger(), *get_clock(), 3000, "Unexpected trajectory size < 2. Ignored.");
return;
}
@@ -237,15 +225,15 @@ rcl_interfaces::msg::SetParametersResult LongitudinalController::paramCallback(
const std::vector<rclcpp::Parameter> & parameters)
{
auto update_param = [&](const std::string & name, float64_t & v) {
- auto it = std::find_if(
- parameters.cbegin(), parameters.cend(),
- [&name](const rclcpp::Parameter & parameter) {return parameter.get_name() == name;});
- if (it != parameters.cend()) {
- v = it->as_double();
- return true;
- }
- return false;
- };
+ auto it = std::find_if(
+ parameters.cbegin(), parameters.cend(),
+ [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; });
+ if (it != parameters.cend()) {
+ v = it->as_double();
+ return true;
+ }
+ return false;
+ };
// delay compensation
update_param("delay_compensation_time", m_delay_compensation_time);
@@ -360,8 +348,7 @@ void LongitudinalController::callbackTimerControl()
// wait for initial pointers
if (
!m_current_velocity_ptr || !m_prev_velocity_ptr || !m_trajectory_ptr ||
- !m_tf_buffer.canTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero))
- {
+ !m_tf_buffer.canTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero)) {
return;
}
@@ -380,11 +367,11 @@ void LongitudinalController::callbackTimerControl()
// self pose is far from trajectory
if (control_data.is_far_from_trajectory) {
- m_control_state = ControlState::EMERGENCY; // update control state
+ m_control_state = ControlState::EMERGENCY; // update control state
const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command
m_prev_raw_ctrl_cmd = raw_ctrl_cmd;
publishCtrlCmd(raw_ctrl_cmd, control_data.current_motion.vel); // publish control command
- publishDebugData(raw_ctrl_cmd, control_data); // publish debug data
+ publishDebugData(raw_ctrl_cmd, control_data); // publish debug data
return;
}
@@ -427,18 +414,18 @@ LongitudinalController::ControlData LongitudinalController::getControlData(
// shift
control_data.shift = getCurrentShift(control_data.nearest_idx);
- if (control_data.shift != m_prev_shift) {m_pid_vel.reset();}
+ if (control_data.shift != m_prev_shift) {
+ m_pid_vel.reset();
+ }
m_prev_shift = control_data.shift;
// distance to stopline
- control_data.stop_dist =
- trajectory_follower::longitudinal_utils::calcStopDistance(
- current_pose.position,
- *m_trajectory_ptr);
+ control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance(
+ current_pose.position, *m_trajectory_ptr);
// pitch
- const float64_t raw_pitch = trajectory_follower::longitudinal_utils::getPitchByPose(
- current_pose.orientation);
+ const float64_t raw_pitch =
+ trajectory_follower::longitudinal_utils::getPitchByPose(current_pose.orientation);
const float64_t traj_pitch = trajectory_follower::longitudinal_utils::getPitchByTraj(
*m_trajectory_ptr, control_data.nearest_idx, m_wheel_base);
control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch);
@@ -447,22 +434,19 @@ LongitudinalController::ControlData LongitudinalController::getControlData(
return control_data;
}
-LongitudinalController::Motion LongitudinalController::calcEmergencyCtrlCmd(const float64_t dt)
-const
+LongitudinalController::Motion LongitudinalController::calcEmergencyCtrlCmd(
+ const float64_t dt) const
{
// These accelerations are without slope compensation
const auto & p = m_emergency_state_params;
const float64_t vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter(
- p.vel, m_prev_raw_ctrl_cmd.vel,
- dt, p.acc);
+ p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc);
const float64_t acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter(
- p.acc, m_prev_raw_ctrl_cmd.acc,
- dt, p.jerk);
+ p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk);
auto clock = rclcpp::Clock{RCL_ROS_TIME};
RCLCPP_ERROR_THROTTLE(
- get_logger(), clock, 3000,
- "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc);
+ get_logger(), clock, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc);
return Motion{vel, acc};
}
@@ -484,8 +468,7 @@ LongitudinalController::ControlState LongitudinalController::updateControlState(
const bool8_t stopping_condition = stop_dist < p.stopping_state_stop_dist;
if (
std::fabs(current_vel) > p.stopped_state_entry_vel ||
- std::fabs(current_acc) > p.stopped_state_entry_acc)
- {
+ std::fabs(current_acc) > p.stopped_state_entry_acc) {
m_last_running_time = std::make_shared<rclcpp::Time>(this->now());
}
const bool8_t stopped_condition =
@@ -565,8 +548,8 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd(
current_pose, m_delay_compensation_time, current_vel);
const auto target_interpolated_point =
calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose.position, nearest_idx);
- target_motion =
- Motion{target_interpolated_point.longitudinal_velocity_mps,
+ target_motion = Motion{
+ target_interpolated_point.longitudinal_velocity_mps,
target_interpolated_point.acceleration_mps2};
target_motion = keepBrakeBeforeStop(*m_trajectory_ptr, target_motion, nearest_idx);
@@ -574,8 +557,7 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd(
const float64_t pred_vel_in_target =
predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time);
m_debug_values.setValues(
- trajectory_follower::DebugValues::TYPE::PREDICTED_VEL,
- pred_vel_in_target);
+ trajectory_follower::DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target);
raw_ctrl_cmd.vel = target_motion.vel;
raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target);
@@ -591,24 +573,18 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd(
raw_ctrl_cmd.vel = m_stopped_state_params.vel;
RCLCPP_DEBUG(
- get_logger(),
- "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f",
- raw_ctrl_cmd.vel, raw_ctrl_cmd.acc);
+ get_logger(), "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel,
+ raw_ctrl_cmd.acc);
} else if (current_control_state == ControlState::STOPPED) {
// This acceleration is without slope compensation
const auto & p = m_stopped_state_params;
raw_ctrl_cmd.vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter(
- p.vel,
- m_prev_raw_ctrl_cmd.vel,
- control_data.dt, p.acc);
+ p.vel, m_prev_raw_ctrl_cmd.vel, control_data.dt, p.acc);
raw_ctrl_cmd.acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter(
- p.acc,
- m_prev_raw_ctrl_cmd.acc,
- control_data.dt, p.jerk);
+ p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk);
RCLCPP_DEBUG(
- get_logger(), "[Stopped]. vel: %3.3f, acc: %3.3f",
- raw_ctrl_cmd.vel, raw_ctrl_cmd.acc);
+ get_logger(), "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc);
} else if (current_control_state == ControlState::EMERGENCY) {
raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt);
}
@@ -655,8 +631,7 @@ void LongitudinalController::publishDebugData(
m_debug_values.setValues(DebugValues::TYPE::SHIFT, static_cast<float64_t>(control_data.shift));
m_debug_values.setValues(DebugValues::TYPE::STOP_DIST, control_data.stop_dist);
m_debug_values.setValues(
- DebugValues::TYPE::CONTROL_STATE,
- static_cast<float64_t>(m_control_state));
+ DebugValues::TYPE::CONTROL_STATE, static_cast<float64_t>(m_control_state));
m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PUBLISHED, ctrl_cmd.acc);
// publish debug values
@@ -672,8 +647,7 @@ void LongitudinalController::publishDebugData(
autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic slope_msg{};
slope_msg.diag_header.data_stamp = this->now();
slope_msg.diag_array.data.push_back(
- static_cast<decltype(slope_msg.diag_array.data)::value_type>(
- control_data.slope_angle));
+ static_cast<decltype(slope_msg.diag_array.data)::value_type>(control_data.slope_angle));
m_pub_slope->publish(slope_msg);
}
@@ -694,12 +668,13 @@ float64_t LongitudinalController::getDt()
LongitudinalController::Motion LongitudinalController::getCurrentMotion() const
{
- const float64_t dv = m_current_velocity_ptr->twist.twist.linear.x -
- m_prev_velocity_ptr->twist.twist.linear.x;
- const float64_t dt =
- std::max(
+ const float64_t dv =
+ m_current_velocity_ptr->twist.twist.linear.x - m_prev_velocity_ptr->twist.twist.linear.x;
+ const float64_t dt = std::max(
(rclcpp::Time(m_current_velocity_ptr->header.stamp) -
- rclcpp::Time(m_prev_velocity_ptr->header.stamp)).seconds(), 1e-03);
+ rclcpp::Time(m_prev_velocity_ptr->header.stamp))
+ .seconds(),
+ 1e-03);
const float64_t accel = dv / dt;
const float64_t current_vel = m_current_velocity_ptr->twist.twist.linear.x;
@@ -708,8 +683,8 @@ LongitudinalController::Motion LongitudinalController::getCurrentMotion() const
return Motion{current_vel, current_acc};
}
-enum LongitudinalController::Shift LongitudinalController::getCurrentShift(const size_t nearest_idx)
-const
+enum LongitudinalController::Shift LongitudinalController::getCurrentShift(
+ const size_t nearest_idx) const
{
constexpr float64_t epsilon = 1e-5;
@@ -725,8 +700,7 @@ const
}
float64_t LongitudinalController::calcFilteredAcc(
- const float64_t raw_acc,
- const ControlData & control_data)
+ const float64_t raw_acc, const ControlData & control_data)
{
using trajectory_follower::DebugValues;
const float64_t acc_max_filtered = ::motion::motion_common::clamp(raw_acc, m_min_acc, m_max_acc);
@@ -766,9 +740,7 @@ void LongitudinalController::storeAccelCmd(const float64_t accel)
if (m_ctrl_cmd_vec.size() <= 2) {
return;
}
- if (
- (this->now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time)
- {
+ if ((this->now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time) {
m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin());
}
}
@@ -821,11 +793,10 @@ LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop(
return output_motion;
}
-autoware_auto_planning_msgs::msg::TrajectoryPoint LongitudinalController::
-calcInterpolatedTargetValue(
+autoware_auto_planning_msgs::msg::TrajectoryPoint
+LongitudinalController::calcInterpolatedTargetValue(
const autoware_auto_planning_msgs::msg::Trajectory & traj,
- const geometry_msgs::msg::Point & point,
- const size_t nearest_idx) const
+ const geometry_msgs::msg::Point & point, const size_t nearest_idx) const
{
if (traj.points.size() == 1) {
return traj.points.at(0);
@@ -870,15 +841,11 @@ float64_t LongitudinalController::predictedVelocityInTargetPoint(
const auto past_delay_time =
this->now() - rclcpp::Duration::from_seconds(delay_compensation_time);
for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) {
- if (
- (this->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() <
- m_delay_compensation_time)
- {
+ if ((this->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) {
if (i == 0) {
// size of m_ctrl_cmd_vec is less than m_delay_compensation_time
- pred_vel =
- current_vel_abs + static_cast<float64_t>(m_ctrl_cmd_vec.at(i).acceleration) *
- delay_compensation_time;
+ pred_vel = current_vel_abs + static_cast<float64_t>(m_ctrl_cmd_vec.at(i).acceleration) *
+ delay_compensation_time;
return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0;
}
// add velocity to accel * dt
@@ -936,8 +903,7 @@ void LongitudinalController::updatePitchDebugValues(
m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch);
m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, raw_pitch * to_degrees);
m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_RAD, traj_pitch);
- m_debug_values.setValues(
- DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees);
+ m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees);
}
void LongitudinalController::updateDebugVelAcc(
@@ -955,8 +921,7 @@ void LongitudinalController::updateDebugVelAcc(
m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel);
m_debug_values.setValues(DebugValues::TYPE::TARGET_ACC, target_motion.acc);
m_debug_values.setValues(
- DebugValues::TYPE::NEAREST_VEL,
- interpolated_point.longitudinal_velocity_mps);
+ DebugValues::TYPE::NEAREST_VEL, interpolated_point.longitudinal_velocity_mps);
m_debug_values.setValues(DebugValues::TYPE::NEAREST_ACC, interpolated_point.acceleration_mps2);
m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL, target_motion.vel - current_vel);
}
diff --git a/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp b/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp
index b94d3a251b694..ef1198bc72012 100644
--- a/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp
+++ b/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp
@@ -12,27 +12,26 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "fake_test_node/fake_test_node.hpp"
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "trajectory_follower_test_utils.hpp"
#include <trajectory_follower_nodes/lateral_controller_node.hpp>
-#include <memory>
-#include <vector>
-
-#include "ament_index_cpp/get_package_share_directory.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
-#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
-#include "gtest/gtest.h"
-#include "fake_test_node/fake_test_node.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/time.hpp"
-#include "trajectory_follower_test_utils.hpp"
+#include <memory>
+#include <vector>
using LateralController = autoware::motion::control::trajectory_follower_nodes::LateralController;
using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand;
@@ -51,14 +50,16 @@ std::shared_ptr<LateralController> makeLateralNode()
const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes");
rclcpp::NodeOptions node_options;
node_options.arguments(
- {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.yaml",
- "--params-file", share_dir + "/param/test_vehicle_info.yaml"});
+ {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml",
+ "--params-file", share_dir + "/param/test_vehicle_info.param.yaml"});
std::shared_ptr<LateralController> node = std::make_shared<LateralController>(node_options);
// Enable all logging in the node
- auto ret = rcutils_logging_set_logger_level(
- node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
- if (ret != RCUTILS_RET_OK) {std::cout << "Failed to set logging severerity to DEBUG\n";}
+ auto ret =
+ rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
+ if (ret != RCUTILS_RET_OK) {
+ std::cout << "Failed to set logging severerity to DEBUG\n";
+ }
return node;
}
@@ -71,25 +72,24 @@ TEST_F(FakeNodeFixture, no_input)
std::shared_ptr<LateralController> node = makeLateralNode();
// Publisher/Subscribers
rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
- this->create_publisher<Trajectory>(
- "lateral_controller/input/reference_trajectory");
+ this->create_publisher<Trajectory>("lateral_controller/input/reference_trajectory");
rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
- this->create_publisher<VehicleOdometry>(
- "lateral_controller/input/current_odometry");
+ this->create_publisher<VehicleOdometry>("lateral_controller/input/current_odometry");
rclcpp::Publisher<SteeringReport>::SharedPtr steer_pub =
- this->create_publisher<SteeringReport>(
- "lateral_controller/input/current_steering");
+ this->create_publisher<SteeringReport>("lateral_controller/input/current_steering");
rclcpp::Subscription<LateralCommand>::SharedPtr cmd_sub =
this->create_subscription<LateralCommand>(
- "lateral_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
- cmd_msg = msg; received_lateral_command = true;
- });
+ "lateral_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_lateral_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
// No published data: expect a stopped command
- test_utils::waitForMessage(node, this, received_lateral_command, std::chrono::seconds{1LL}, false);
+ test_utils::waitForMessage(
+ node, this, received_lateral_command, std::chrono::seconds{1LL}, false);
ASSERT_FALSE(received_lateral_command);
}
@@ -102,20 +102,18 @@ TEST_F(FakeNodeFixture, empty_trajectory)
std::shared_ptr<LateralController> node = makeLateralNode();
// Publisher/Subscribers
rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
- this->create_publisher<Trajectory>(
- "lateral_controller/input/reference_trajectory");
+ this->create_publisher<Trajectory>("lateral_controller/input/reference_trajectory");
rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
- this->create_publisher<VehicleOdometry>(
- "lateral_controller/input/current_odometry");
+ this->create_publisher<VehicleOdometry>("lateral_controller/input/current_odometry");
rclcpp::Publisher<SteeringReport>::SharedPtr steer_pub =
- this->create_publisher<SteeringReport>(
- "lateral_controller/input/current_steering");
+ this->create_publisher<SteeringReport>("lateral_controller/input/current_steering");
rclcpp::Subscription<LateralCommand>::SharedPtr cmd_sub =
this->create_subscription<LateralCommand>(
- "lateral_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
- cmd_msg = msg; received_lateral_command = true;
- });
+ "lateral_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_lateral_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
@@ -142,7 +140,8 @@ TEST_F(FakeNodeFixture, empty_trajectory)
odom_pub->publish(odom_msg);
steer_pub->publish(steer_msg);
- test_utils::waitForMessage(node, this, received_lateral_command, std::chrono::seconds{1LL}, false);
+ test_utils::waitForMessage(
+ node, this, received_lateral_command, std::chrono::seconds{1LL}, false);
ASSERT_FALSE(received_lateral_command);
}
@@ -155,20 +154,18 @@ TEST_F(FakeNodeFixture, straight_trajectory)
std::shared_ptr<LateralController> node = makeLateralNode();
// Publisher/Subscribers
rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
- this->create_publisher<Trajectory>(
- "lateral_controller/input/reference_trajectory");
+ this->create_publisher<Trajectory>("lateral_controller/input/reference_trajectory");
rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
- this->create_publisher<VehicleOdometry>(
- "lateral_controller/input/current_odometry");
+ this->create_publisher<VehicleOdometry>("lateral_controller/input/current_odometry");
rclcpp::Publisher<SteeringReport>::SharedPtr steer_pub =
- this->create_publisher<SteeringReport>(
- "lateral_controller/input/current_steering");
+ this->create_publisher<SteeringReport>("lateral_controller/input/current_steering");
rclcpp::Subscription<LateralCommand>::SharedPtr cmd_sub =
this->create_subscription<LateralCommand>(
- "lateral_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
- cmd_msg = msg; received_lateral_command = true;
- });
+ "lateral_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_lateral_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
@@ -229,20 +226,18 @@ TEST_F(FakeNodeFixture, right_turn)
std::shared_ptr<LateralController> node = makeLateralNode();
// Publisher/Subscribers
rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
- this->create_publisher<Trajectory>(
- "lateral_controller/input/reference_trajectory");
+ this->create_publisher<Trajectory>("lateral_controller/input/reference_trajectory");
rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
- this->create_publisher<VehicleOdometry>(
- "lateral_controller/input/current_odometry");
+ this->create_publisher<VehicleOdometry>("lateral_controller/input/current_odometry");
rclcpp::Publisher<SteeringReport>::SharedPtr steer_pub =
- this->create_publisher<SteeringReport>(
- "lateral_controller/input/current_steering");
+ this->create_publisher<SteeringReport>("lateral_controller/input/current_steering");
rclcpp::Subscription<LateralCommand>::SharedPtr cmd_sub =
this->create_subscription<LateralCommand>(
- "lateral_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
- cmd_msg = msg; received_lateral_command = true;
- });
+ "lateral_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_lateral_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
@@ -303,20 +298,18 @@ TEST_F(FakeNodeFixture, left_turn)
std::shared_ptr<LateralController> node = makeLateralNode();
// Publisher/Subscribers
rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
- this->create_publisher<Trajectory>(
- "lateral_controller/input/reference_trajectory");
+ this->create_publisher<Trajectory>("lateral_controller/input/reference_trajectory");
rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
- this->create_publisher<VehicleOdometry>(
- "lateral_controller/input/current_odometry");
+ this->create_publisher<VehicleOdometry>("lateral_controller/input/current_odometry");
rclcpp::Publisher<SteeringReport>::SharedPtr steer_pub =
- this->create_publisher<SteeringReport>(
- "lateral_controller/input/current_steering");
+ this->create_publisher<SteeringReport>("lateral_controller/input/current_steering");
rclcpp::Subscription<LateralCommand>::SharedPtr cmd_sub =
this->create_subscription<LateralCommand>(
- "lateral_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
- cmd_msg = msg; received_lateral_command = true;
- });
+ "lateral_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_lateral_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
@@ -377,20 +370,18 @@ TEST_F(FakeNodeFixture, stopped)
std::shared_ptr<LateralController> node = makeLateralNode();
// Publisher/Subscribers
rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
- this->create_publisher<Trajectory>(
- "lateral_controller/input/reference_trajectory");
+ this->create_publisher<Trajectory>("lateral_controller/input/reference_trajectory");
rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
- this->create_publisher<VehicleOdometry>(
- "lateral_controller/input/current_odometry");
+ this->create_publisher<VehicleOdometry>("lateral_controller/input/current_odometry");
rclcpp::Publisher<SteeringReport>::SharedPtr steer_pub =
- this->create_publisher<SteeringReport>(
- "lateral_controller/input/current_steering");
+ this->create_publisher<SteeringReport>("lateral_controller/input/current_steering");
rclcpp::Subscription<LateralCommand>::SharedPtr cmd_sub =
this->create_subscription<LateralCommand>(
- "lateral_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
- cmd_msg = msg; received_lateral_command = true;
- });
+ "lateral_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_lateral_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
diff --git a/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp b/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp
index df79ad04959ca..bb1fd9d3b0bc6 100644
--- a/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp
+++ b/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp
@@ -12,23 +12,21 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "fake_test_node/fake_test_node.hpp"
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "trajectory_follower_test_utils.hpp"
#include <trajectory_follower_nodes/latlon_muxer_node.hpp>
-#include <memory>
-#include <string>
-#include <vector>
-
#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
-#include "fake_test_node/fake_test_node.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/time.hpp"
-#include "gtest/gtest.h"
-#include "trajectory_follower_test_utils.hpp"
+#include <memory>
+#include <string>
+#include <vector>
using LatLonMuxer = autoware::motion::control::trajectory_follower_nodes::LatLonMuxer;
using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand;
@@ -50,17 +48,16 @@ TEST_F(FakeNodeFixture, TestCorrectOutput)
std::shared_ptr<LatLonMuxer> node = std::make_shared<LatLonMuxer>(node_options);
// Publisher/Subscribers
rclcpp::Publisher<LateralCommand>::SharedPtr lat_pub =
- this->create_publisher<LateralCommand>(
- "latlon_muxer/input/lateral/control_cmd");
+ this->create_publisher<LateralCommand>("latlon_muxer/input/lateral/control_cmd");
rclcpp::Publisher<LongitudinalCommand>::SharedPtr lon_pub =
- this->create_publisher<LongitudinalCommand>(
- "latlon_muxer/input/longitudinal/control_cmd");
+ this->create_publisher<LongitudinalCommand>("latlon_muxer/input/longitudinal/control_cmd");
rclcpp::Subscription<ControlCommand>::SharedPtr cmd_sub =
this->create_subscription<ControlCommand>(
- "latlon_muxer/output/control_cmd", *node,
- [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) {
- cmd_msg = msg; received_combined_command = true;
- });
+ "latlon_muxer/output/control_cmd", *node,
+ [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_combined_command = true;
+ });
// Publish messages
LateralCommand lat_msg;
lat_msg.steering_tire_angle = 1.5;
@@ -97,17 +94,16 @@ TEST_F(FakeNodeFixture, TestLateralTimeout)
std::shared_ptr<LatLonMuxer> node = std::make_shared<LatLonMuxer>(node_options);
// Publisher/Subscribers
rclcpp::Publisher<LateralCommand>::SharedPtr lat_pub =
- this->create_publisher<LateralCommand>(
- "latlon_muxer/input/lateral/control_cmd");
+ this->create_publisher<LateralCommand>("latlon_muxer/input/lateral/control_cmd");
rclcpp::Publisher<LongitudinalCommand>::SharedPtr lon_pub =
- this->create_publisher<LongitudinalCommand>(
- "latlon_muxer/input/longitudinal/control_cmd");
+ this->create_publisher<LongitudinalCommand>("latlon_muxer/input/longitudinal/control_cmd");
rclcpp::Subscription<ControlCommand>::SharedPtr cmd_sub =
this->create_subscription<ControlCommand>(
- "latlon_muxer/output/control_cmd", *node,
- [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) {
- cmd_msg = msg; received_combined_command = true;
- });
+ "latlon_muxer/output/control_cmd", *node,
+ [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_combined_command = true;
+ });
// Publish messages
LateralCommand lat_msg;
LongitudinalCommand lon_msg;
@@ -118,8 +114,7 @@ TEST_F(FakeNodeFixture, TestLateralTimeout)
lon_pub->publish(lon_msg);
test_utils::waitForMessage(
- node, this, received_combined_command, std::chrono::seconds{1LL},
- false);
+ node, this, received_combined_command, std::chrono::seconds{1LL}, false);
// Ensure combined command was not published
ASSERT_FALSE(received_combined_command);
}
@@ -135,17 +130,16 @@ TEST_F(FakeNodeFixture, TestLongitudinalTimeout)
std::shared_ptr<LatLonMuxer> node = std::make_shared<LatLonMuxer>(node_options);
// Publisher/Subscribers
rclcpp::Publisher<LateralCommand>::SharedPtr lat_pub =
- this->create_publisher<LateralCommand>(
- "latlon_muxer/input/lateral/control_cmd");
+ this->create_publisher<LateralCommand>("latlon_muxer/input/lateral/control_cmd");
rclcpp::Publisher<LongitudinalCommand>::SharedPtr lon_pub =
- this->create_publisher<LongitudinalCommand>(
- "latlon_muxer/input/longitudinal/control_cmd");
+ this->create_publisher<LongitudinalCommand>("latlon_muxer/input/longitudinal/control_cmd");
rclcpp::Subscription<ControlCommand>::SharedPtr cmd_sub =
this->create_subscription<ControlCommand>(
- "latlon_muxer/output/control_cmd", *node,
- [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) {
- cmd_msg = msg; received_combined_command = true;
- });
+ "latlon_muxer/output/control_cmd", *node,
+ [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_combined_command = true;
+ });
// Publish messages
LateralCommand lat_msg;
LongitudinalCommand lon_msg;
@@ -156,8 +150,7 @@ TEST_F(FakeNodeFixture, TestLongitudinalTimeout)
lon_pub->publish(lon_msg);
test_utils::waitForMessage(
- node, this, received_combined_command, std::chrono::seconds{1LL},
- false);
+ node, this, received_combined_command, std::chrono::seconds{1LL}, false);
// Ensure combined command was not published
ASSERT_FALSE(received_combined_command);
}
@@ -173,17 +166,16 @@ TEST_F(FakeNodeFixture, TestLatlonTimeout)
std::shared_ptr<LatLonMuxer> node = std::make_shared<LatLonMuxer>(node_options);
// Publisher/Subscribers
rclcpp::Publisher<LateralCommand>::SharedPtr lat_pub =
- this->create_publisher<LateralCommand>(
- "latlon_muxer/input/lateral/control_cmd");
+ this->create_publisher<LateralCommand>("latlon_muxer/input/lateral/control_cmd");
rclcpp::Publisher<LongitudinalCommand>::SharedPtr lon_pub =
- this->create_publisher<LongitudinalCommand>(
- "latlon_muxer/input/longitudinal/control_cmd");
+ this->create_publisher<LongitudinalCommand>("latlon_muxer/input/longitudinal/control_cmd");
rclcpp::Subscription<ControlCommand>::SharedPtr cmd_sub =
this->create_subscription<ControlCommand>(
- "latlon_muxer/output/control_cmd", *node,
- [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) {
- cmd_msg = msg; received_combined_command = true;
- });
+ "latlon_muxer/output/control_cmd", *node,
+ [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_combined_command = true;
+ });
// Publish messages
LateralCommand lat_msg;
LongitudinalCommand lon_msg;
@@ -194,8 +186,7 @@ TEST_F(FakeNodeFixture, TestLatlonTimeout)
lon_pub->publish(lon_msg);
test_utils::waitForMessage(
- node, this, received_combined_command, std::chrono::seconds{1LL},
- false);
+ node, this, received_combined_command, std::chrono::seconds{1LL}, false);
// Ensure combined command was not published
ASSERT_FALSE(received_combined_command);
}
diff --git a/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp
index 836b6bc938f10..2130253c2d1df 100644
--- a/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp
+++ b/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp
@@ -12,24 +12,24 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "fake_test_node/fake_test_node.hpp"
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "trajectory_follower_test_utils.hpp"
#include <trajectory_follower_nodes/longitudinal_controller_node.hpp>
-#include <memory>
-
-#include "ament_index_cpp/get_package_share_directory.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
-#include "fake_test_node/fake_test_node.hpp"
-#include "gtest/gtest.h"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/time.hpp"
-#include "trajectory_follower_test_utils.hpp"
+
+#include <memory>
using LongitudinalController =
autoware::motion::control::trajectory_follower_nodes::LongitudinalController;
@@ -46,20 +46,23 @@ std::shared_ptr<LongitudinalController> makeLongitudinalNode()
const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes");
rclcpp::NodeOptions node_options;
node_options.arguments(
- {"--ros-args", "--params-file", share_dir + "/param/longitudinal_controller_defaults.yaml",
- "--params-file", share_dir + "/param/test_vehicle_info.yaml"});
- std::shared_ptr<LongitudinalController> node = std::make_shared<LongitudinalController>(
- node_options);
+ {"--ros-args", "--params-file",
+ share_dir + "/param/longitudinal_controller_defaults.param.yaml", "--params-file",
+ share_dir + "/param/test_vehicle_info.param.yaml"});
+ std::shared_ptr<LongitudinalController> node =
+ std::make_shared<LongitudinalController>(node_options);
// Enable all logging in the node
- auto ret = rcutils_logging_set_logger_level(
- node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
- if (ret != RCUTILS_RET_OK) {std::cout << "Failed to set logging severerity to DEBUG\n";}
+ auto ret =
+ rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
+ if (ret != RCUTILS_RET_OK) {
+ std::cout << "Failed to set logging severerity to DEBUG\n";
+ }
return node;
}
-
-TEST_F(FakeNodeFixture, longitudinal_keep_velocity) {
+TEST_F(FakeNodeFixture, longitudinal_keep_velocity)
+{
// Data to test
LongitudinalCommand::SharedPtr cmd_msg;
bool received_longitudinal_command = false;
@@ -68,16 +71,17 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) {
std::shared_ptr<LongitudinalController> node = makeLongitudinalNode();
// Publisher/Subscribers
- rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub = this->create_publisher<VehicleOdometry>(
- "longitudinal_controller/input/current_odometry");
- rclcpp::Publisher<Trajectory>::SharedPtr traj_pub = this->create_publisher<Trajectory>(
- "longitudinal_controller/input/current_trajectory");
+ rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
+ this->create_publisher<VehicleOdometry>("longitudinal_controller/input/current_odometry");
+ rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
+ this->create_publisher<Trajectory>("longitudinal_controller/input/current_trajectory");
rclcpp::Subscription<LongitudinalCommand>::SharedPtr cmd_sub =
this->create_subscription<LongitudinalCommand>(
- "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
- cmd_msg = msg;received_longitudinal_command = true;
- });
+ "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_longitudinal_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
@@ -129,7 +133,8 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) {
EXPECT_DOUBLE_EQ(cmd_msg->acceleration, 0.0);
}
-TEST_F(FakeNodeFixture, longitudinal_slow_down) {
+TEST_F(FakeNodeFixture, longitudinal_slow_down)
+{
// Data to test
LongitudinalCommand::SharedPtr cmd_msg;
bool received_longitudinal_command = false;
@@ -138,16 +143,17 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) {
std::shared_ptr<LongitudinalController> node = makeLongitudinalNode();
// Publisher/Subscribers
- rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub = this->create_publisher<VehicleOdometry>(
- "longitudinal_controller/input/current_odometry");
- rclcpp::Publisher<Trajectory>::SharedPtr traj_pub = this->create_publisher<Trajectory>(
- "longitudinal_controller/input/current_trajectory");
+ rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
+ this->create_publisher<VehicleOdometry>("longitudinal_controller/input/current_odometry");
+ rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
+ this->create_publisher<Trajectory>("longitudinal_controller/input/current_trajectory");
rclcpp::Subscription<LongitudinalCommand>::SharedPtr cmd_sub =
this->create_subscription<LongitudinalCommand>(
- "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
- cmd_msg = msg;received_longitudinal_command = true;
- });
+ "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_longitudinal_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
@@ -199,7 +205,8 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) {
EXPECT_LT(cmd_msg->acceleration, 0.0f);
}
-TEST_F(FakeNodeFixture, longitudinal_accelerate) {
+TEST_F(FakeNodeFixture, longitudinal_accelerate)
+{
// Data to test
LongitudinalCommand::SharedPtr cmd_msg;
bool received_longitudinal_command = false;
@@ -208,16 +215,17 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) {
std::shared_ptr<LongitudinalController> node = makeLongitudinalNode();
// Publisher/Subscribers
- rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub = this->create_publisher<VehicleOdometry>(
- "longitudinal_controller/input/current_odometry");
- rclcpp::Publisher<Trajectory>::SharedPtr traj_pub = this->create_publisher<Trajectory>(
- "longitudinal_controller/input/current_trajectory");
+ rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
+ this->create_publisher<VehicleOdometry>("longitudinal_controller/input/current_odometry");
+ rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
+ this->create_publisher<Trajectory>("longitudinal_controller/input/current_trajectory");
rclcpp::Subscription<LongitudinalCommand>::SharedPtr cmd_sub =
this->create_subscription<LongitudinalCommand>(
- "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
- cmd_msg = msg;received_longitudinal_command = true;
- });
+ "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_longitudinal_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
@@ -269,7 +277,8 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) {
EXPECT_GT(cmd_msg->acceleration, 0.0f);
}
-TEST_F(FakeNodeFixture, longitudinal_stopped) {
+TEST_F(FakeNodeFixture, longitudinal_stopped)
+{
// Data to test
LongitudinalCommand::SharedPtr cmd_msg;
bool received_longitudinal_command = false;
@@ -278,16 +287,17 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) {
std::shared_ptr<LongitudinalController> node = makeLongitudinalNode();
// Publisher/Subscribers
- rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub = this->create_publisher<VehicleOdometry>(
- "longitudinal_controller/input/current_odometry");
- rclcpp::Publisher<Trajectory>::SharedPtr traj_pub = this->create_publisher<Trajectory>(
- "longitudinal_controller/input/current_trajectory");
+ rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
+ this->create_publisher<VehicleOdometry>("longitudinal_controller/input/current_odometry");
+ rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
+ this->create_publisher<Trajectory>("longitudinal_controller/input/current_trajectory");
rclcpp::Subscription<LongitudinalCommand>::SharedPtr cmd_sub =
this->create_subscription<LongitudinalCommand>(
- "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
- cmd_msg = msg;received_longitudinal_command = true;
- });
+ "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_longitudinal_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
@@ -331,7 +341,8 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) {
EXPECT_LT(cmd_msg->acceleration, 0.0f); // when stopped negative acceleration to brake
}
-TEST_F(FakeNodeFixture, longitudinal_reverse) {
+TEST_F(FakeNodeFixture, longitudinal_reverse)
+{
// Data to test
LongitudinalCommand::SharedPtr cmd_msg;
bool received_longitudinal_command = false;
@@ -340,16 +351,17 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) {
std::shared_ptr<LongitudinalController> node = makeLongitudinalNode();
// Publisher/Subscribers
- rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub = this->create_publisher<VehicleOdometry>(
- "longitudinal_controller/input/current_odometry");
- rclcpp::Publisher<Trajectory>::SharedPtr traj_pub = this->create_publisher<Trajectory>(
- "longitudinal_controller/input/current_trajectory");
+ rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
+ this->create_publisher<VehicleOdometry>("longitudinal_controller/input/current_odometry");
+ rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
+ this->create_publisher<Trajectory>("longitudinal_controller/input/current_trajectory");
rclcpp::Subscription<LongitudinalCommand>::SharedPtr cmd_sub =
this->create_subscription<LongitudinalCommand>(
- "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
- cmd_msg = msg;received_longitudinal_command = true;
- });
+ "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_longitudinal_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
@@ -393,7 +405,8 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) {
EXPECT_GT(cmd_msg->acceleration, 0.0f);
}
-TEST_F(FakeNodeFixture, longitudinal_emergency) {
+TEST_F(FakeNodeFixture, longitudinal_emergency)
+{
// Data to test
LongitudinalCommand::SharedPtr cmd_msg;
bool received_longitudinal_command = false;
@@ -402,16 +415,17 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) {
std::shared_ptr<LongitudinalController> node = makeLongitudinalNode();
// Publisher/Subscribers
- rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub = this->create_publisher<VehicleOdometry>(
- "longitudinal_controller/input/current_odometry");
- rclcpp::Publisher<Trajectory>::SharedPtr traj_pub = this->create_publisher<Trajectory>(
- "longitudinal_controller/input/current_trajectory");
+ rclcpp::Publisher<VehicleOdometry>::SharedPtr odom_pub =
+ this->create_publisher<VehicleOdometry>("longitudinal_controller/input/current_odometry");
+ rclcpp::Publisher<Trajectory>::SharedPtr traj_pub =
+ this->create_publisher<Trajectory>("longitudinal_controller/input/current_trajectory");
rclcpp::Subscription<LongitudinalCommand>::SharedPtr cmd_sub =
this->create_subscription<LongitudinalCommand>(
- "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
- [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
- cmd_msg = msg;received_longitudinal_command = true;
- });
+ "longitudinal_controller/output/control_cmd", *this->get_fake_node(),
+ [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) {
+ cmd_msg = msg;
+ received_longitudinal_command = true;
+ });
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> br =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->get_fake_node());
diff --git a/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp b/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp
index 3cea0b22ebf3e..043a4408938ea 100644
--- a/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp
+++ b/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp
@@ -15,22 +15,22 @@
#ifndef TRAJECTORY_FOLLOWER_TEST_UTILS_HPP_
#define TRAJECTORY_FOLLOWER_TEST_UTILS_HPP_
-#include <memory>
-#include <string>
-
#include "fake_test_node/fake_test_node.hpp"
-#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time.hpp"
#include "tf2_ros/static_transform_broadcaster.h"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include <memory>
+#include <string>
+
namespace test_utils
{
using FakeNodeFixture = autoware::tools::testing::FakeTestNode;
inline void waitForMessage(
- const std::shared_ptr<rclcpp::Node> & node, FakeNodeFixture * fixture,
- const bool & received_flag,
+ const std::shared_ptr<rclcpp::Node> & node, FakeNodeFixture * fixture, const bool & received_flag,
const std::chrono::duration<int64_t> max_wait_time = std::chrono::seconds{10LL},
const bool fail_on_timeout = true)
{
@@ -43,9 +43,7 @@ inline void waitForMessage(
time_passed += dt;
if (time_passed > max_wait_time) {
if (fail_on_timeout) {
- throw std::runtime_error(
- std::string(
- "Did not receive a message soon enough"));
+ throw std::runtime_error(std::string("Did not receive a message soon enough"));
} else {
break;
}
@@ -70,14 +68,15 @@ inline geometry_msgs::msg::TransformStamped getDummyTransform()
return transform_stamped;
}
-// TODO(Horibe): modify the controller nodes so that they does not publish topics when data is not ready.
-// then, remove this function.
+// TODO(Horibe): modify the controller nodes so that they does not publish topics when data is not
+// ready. then, remove this function.
template <typename T>
-inline void spinWhile(T &node){
- for (size_t i = 0; i < 10; i++) {
- rclcpp::spin_some(node);
- const auto dt{std::chrono::milliseconds{100LL}};
- std::this_thread::sleep_for(dt);
+inline void spinWhile(T & node)
+{
+ for (size_t i = 0; i < 10; i++) {
+ rclcpp::spin_some(node);
+ const auto dt{std::chrono::milliseconds{100LL}};
+ std::this_thread::sleep_for(dt);
}
}
} // namespace test_utils
diff --git a/planning/costmap_generator/README.md b/planning/costmap_generator/README.md
index 209b6ce1daa77..eb8affeecf1c0 100644
--- a/planning/costmap_generator/README.md
+++ b/planning/costmap_generator/README.md
@@ -11,7 +11,7 @@ This node reads `PointCloud` and/or `DynamicObjectArray` and creates an `Occupan
| `~input/objects` | autoware_auto_perception_msgs::PredictedObjects | predicted objects, for obstacles areas |
| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects |
| `~input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map, for drivable areas |
-| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation |
+| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation |
### Output topics
diff --git a/planning/costmap_generator/launch/costmap_generator.launch.xml b/planning/costmap_generator/launch/costmap_generator.launch.xml
index aca124b9bdbfa..75b16f4fc2e51 100644
--- a/planning/costmap_generator/launch/costmap_generator.launch.xml
+++ b/planning/costmap_generator/launch/costmap_generator.launch.xml
@@ -7,33 +7,33 @@
<arg name="output_occupancy_grid" default="~/output/occupancy_grid"/>
<node pkg="costmap_generator" exec="costmap_generator" name="costmap_generator" output="screen">
- <remap from="~/input/objects" to="$(var input_objects)" />
- <remap from="~/input/points_no_ground" to="$(var input_points_no_ground)" />
- <remap from="~/input/vector_map" to="$(var input_lanelet_map)" />
- <remap from="~/input/scenario" to="$(var input_scenario)" />
- <remap from="~/output/grid_map" to="$(var output_grid_map)" />
- <remap from="~/output/occupancy_grid" to="$(var output_occupancy_grid)" />
+ <remap from="~/input/objects" to="$(var input_objects)"/>
+ <remap from="~/input/points_no_ground" to="$(var input_points_no_ground)"/>
+ <remap from="~/input/vector_map" to="$(var input_lanelet_map)"/>
+ <remap from="~/input/scenario" to="$(var input_scenario)"/>
+ <remap from="~/output/grid_map" to="$(var output_grid_map)"/>
+ <remap from="~/output/occupancy_grid" to="$(var output_occupancy_grid)"/>
- <param name="costmap_frame" value="map" />
- <param name="vehicle_frame" value="base_link" />
- <param name="map_frame" value="map" />
+ <param name="costmap_frame" value="map"/>
+ <param name="vehicle_frame" value="base_link"/>
+ <param name="map_frame" value="map"/>
- <param name="update_rate" value="10.0" />
+ <param name="update_rate" value="10.0"/>
- <param name="use_wayarea" value="true" />
- <param name="use_objects" value="true" />
- <param name="use_points" value="true" />
+ <param name="use_wayarea" value="true"/>
+ <param name="use_objects" value="true"/>
+ <param name="use_points" value="true"/>
- <param name="grid_min_value" value="0.0" />
- <param name="grid_max_value" value="1.0" />
- <param name="grid_resolution" value="0.2" />
- <param name="grid_length_x" value="70.0" />
- <param name="grid_length_y" value="70.0" />
- <param name="grid_position_x" value="0.0" />
- <param name="grid_position_y" value="0.0" />
- <param name="maximum_lidar_height_thres" value="0.3" />
- <param name="minimum_lidar_height_thres" value="-2.2" />
- <param name="expand_polygon_size" value="1.0" />
- <param name="size_of_expansion_kernel" value="9" />
+ <param name="grid_min_value" value="0.0"/>
+ <param name="grid_max_value" value="1.0"/>
+ <param name="grid_resolution" value="0.2"/>
+ <param name="grid_length_x" value="70.0"/>
+ <param name="grid_length_y" value="70.0"/>
+ <param name="grid_position_x" value="0.0"/>
+ <param name="grid_position_y" value="0.0"/>
+ <param name="maximum_lidar_height_thres" value="0.3"/>
+ <param name="minimum_lidar_height_thres" value="-2.2"/>
+ <param name="expand_polygon_size" value="1.0"/>
+ <param name="size_of_expansion_kernel" value="9"/>
</node>
</launch>
diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp
index 15a0b7a4d5c0a..99ffdf8c63ce4 100644
--- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp
+++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp
@@ -163,8 +163,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options)
// Timer
const auto period_ns = rclcpp::Rate(update_rate_).period();
- timer_ = rclcpp::create_timer(this, get_clock(), period_ns,
- std::bind(&CostmapGenerator::onTimer, this));
+ timer_ =
+ rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&CostmapGenerator::onTimer, this));
// Initialize
initGridmap();
diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml
index 2c16c5ba00d7c..87606ba80fe16 100644
--- a/planning/costmap_generator/package.xml
+++ b/planning/costmap_generator/package.xml
@@ -15,7 +15,6 @@
<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_auto_perception_msgs</depend>
- <depend>tier4_planning_msgs</depend>
<depend>grid_map_ros</depend>
<depend>lanelet2_extension</depend>
<depend>libpcl-all-dev</depend>
@@ -27,12 +26,12 @@
<depend>tf2_eigen</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
+ <depend>tier4_planning_msgs</depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
-
<export>
<build_type>ament_cmake</build_type>
</export>
diff --git a/planning/freespace_planner/README.md b/planning/freespace_planner/README.md
index 8b97b30856252..b7b31baac23f9 100644
--- a/planning/freespace_planner/README.md
+++ b/planning/freespace_planner/README.md
@@ -18,7 +18,7 @@ In other words, the output trajectory doesn't include both forward and backward
| `~input/route` | autoware_auto_planning_msgs::Route | route and goal pose |
| `~input/occupancy_grid` | nav_msgs::OccupancyGrid | costmap, for drivable areas |
| `~input/odometry` | nav_msgs::Odometry | vehicle velocity, for checking whether vehicle is stopped |
-| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation |
+| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation |
### Output topics
diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp
index 77513625678e5..438212ce57d56 100644
--- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp
+++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp
@@ -37,10 +37,10 @@
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
-#include <tier4_planning_msgs/msg/scenario.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_planning_msgs/msg/scenario.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
@@ -56,7 +56,6 @@ namespace freespace_planner
{
using autoware_auto_planning_msgs::msg::HADMapRoute;
using autoware_auto_planning_msgs::msg::Trajectory;
-using tier4_planning_msgs::msg::Scenario;
using freespace_planning_algorithms::AbstractPlanningAlgorithm;
using freespace_planning_algorithms::AstarParam;
using freespace_planning_algorithms::PlannerCommonParam;
@@ -66,6 +65,7 @@ using geometry_msgs::msg::TransformStamped;
using geometry_msgs::msg::Twist;
using nav_msgs::msg::OccupancyGrid;
using nav_msgs::msg::Odometry;
+using tier4_planning_msgs::msg::Scenario;
struct NodeParam
{
diff --git a/planning/freespace_planner/launch/freespace_planner.launch.xml b/planning/freespace_planner/launch/freespace_planner.launch.xml
index ed83f5aae8289..7c2408b0311f5 100644
--- a/planning/freespace_planner/launch/freespace_planner.launch.xml
+++ b/planning/freespace_planner/launch/freespace_planner.launch.xml
@@ -2,23 +2,23 @@
<arg name="input_route" default="input_route"/>
<arg name="input_occupancy_grid" default="input_occupancy_grid"/>
<arg name="input_scenario" default="input_scenario"/>
- <arg name="input_odometry" default="/localization/kinematic_state" />
+ <arg name="input_odometry" default="/localization/kinematic_state"/>
<arg name="output_trajectory" default="output_trajectory"/>
<arg name="is_completed" default="is_completed"/>
<arg name="param_file" default="$(find-pkg-share freespace_planner)/config/freespace_planner.param.yaml"/>
<!-- vehicle info -->
- <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml" />
+ <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
<node pkg="freespace_planner" exec="freespace_planner" name="freespace_planner" output="screen">
- <remap from="~/input/route" to="$(var input_route)" />
- <remap from="~/input/occupancy_grid" to="$(var input_occupancy_grid)" />
- <remap from="~/input/scenario" to="$(var input_scenario)" />
- <remap from="~/input/odometry" to="$(var input_odometry)" />
- <remap from="~/output/trajectory" to="$(var output_trajectory)" />
- <remap from="is_completed" to="$(var is_completed)" />
+ <remap from="~/input/route" to="$(var input_route)"/>
+ <remap from="~/input/occupancy_grid" to="$(var input_occupancy_grid)"/>
+ <remap from="~/input/scenario" to="$(var input_scenario)"/>
+ <remap from="~/input/odometry" to="$(var input_odometry)"/>
+ <remap from="~/output/trajectory" to="$(var output_trajectory)"/>
+ <remap from="is_completed" to="$(var is_completed)"/>
<param from="$(var param_file)"/>
- <param from="$(var vehicle_info_param_file)" />
+ <param from="$(var vehicle_info_param_file)"/>
</node>
</launch>
diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml
index 5f94d5c606456..f402c91ea2023 100644
--- a/planning/freespace_planner/package.xml
+++ b/planning/freespace_planner/package.xml
@@ -13,8 +13,6 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<depend>autoware_auto_planning_msgs</depend>
- <depend>tier4_planning_msgs</depend>
- <depend>tier4_autoware_utils</depend>
<depend>freespace_planning_algorithms</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
@@ -23,6 +21,8 @@
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
+ <depend>tier4_autoware_utils</depend>
+ <depend>tier4_planning_msgs</depend>
<depend>vehicle_info_util</depend>
<depend>visualization_msgs</depend>
diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp
index e3ad37c1ef87f..e63087bb7708b 100644
--- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp
+++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp
@@ -44,7 +44,6 @@ namespace
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using TrajectoryPoints = std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>;
-using tier4_planning_msgs::msg::Scenario;
using freespace_planning_algorithms::AstarSearch;
using freespace_planning_algorithms::PlannerWaypoint;
using freespace_planning_algorithms::PlannerWaypoints;
@@ -54,6 +53,7 @@ using geometry_msgs::msg::PoseStamped;
using geometry_msgs::msg::TransformStamped;
using geometry_msgs::msg::Twist;
using nav_msgs::msg::Odometry;
+using tier4_planning_msgs::msg::Scenario;
bool isActive(const Scenario::ConstSharedPtr & scenario)
{
@@ -261,8 +261,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti
// Timer
{
const auto period_ns = rclcpp::Rate(node_param_.update_rate).period();
- timer_ = rclcpp::create_timer(this, get_clock(), period_ns,
- std::bind(&FreespacePlannerNode::onTimer, this));
+ timer_ = rclcpp::create_timer(
+ this, get_clock(), period_ns, std::bind(&FreespacePlannerNode::onTimer, this));
}
}
@@ -350,8 +350,8 @@ bool FreespacePlannerNode::isPlanRequired()
if (node_param_.replan_when_obstacle_found) {
algo_->setMap(*occupancy_grid_);
- const size_t nearest_index_partial =
- tier4_autoware_utils::findNearestIndex(partial_trajectory_.points, current_pose_.pose.position);
+ const size_t nearest_index_partial = tier4_autoware_utils::findNearestIndex(
+ partial_trajectory_.points, current_pose_.pose.position);
const size_t end_index_partial = partial_trajectory_.points.size() - 1;
const auto forward_trajectory =
@@ -421,8 +421,8 @@ void FreespacePlannerNode::onTimer()
// Get current pose
constexpr const char * vehicle_frame = "base_link";
- current_pose_ =
- tier4_autoware_utils::transform2pose(getTransform(occupancy_grid_->header.frame_id, vehicle_frame));
+ current_pose_ = tier4_autoware_utils::transform2pose(
+ getTransform(occupancy_grid_->header.frame_id, vehicle_frame));
if (current_pose_.header.frame_id == "") {
return;
}
diff --git a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md
index f9c2c074b77a6..63112315e0558 100644
--- a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md
+++ b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md
@@ -1,104 +1,91 @@
-simple_planning_simulator {#simple_planning_simulator-package-design}
-===========
+# simple_planning_simulator {#simple_planning_simulator-package-design}
-
-# Purpose / Use cases
+## Purpose / Use cases
This node simulates the vehicle motion for a vehicle command in 2D using a simple vehicle model.
-
-
-# Design
-
+## Design
The purpose of this simulator is for the integration test of planning and control modules. This does not simulate sensing or perception, but is implemented in pure c++ only and works without GPU.
## Assumptions / Known limits
- - It simulates only in 2D motion.
- - It does not perform physical operations such as collision and sensing, but only calculates the integral results of vehicle dynamics.
-
+- It simulates only in 2D motion.
+- It does not perform physical operations such as collision and sensing, but only calculates the integral results of vehicle dynamics.
## Inputs / Outputs / API
+### input
-**input**
- - input/vehicle_control_command [`autoware_auto_msgs/msg/VehicleControlCommand`] : target command to drive a vehicle.
- - input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle.
- - input/vehicle_state_command [`autoware_auto_msgs/msg/VehicleStateCommand`] : target state command (e.g. gear).
- - /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose
+- input/vehicle_control_command [`autoware_auto_msgs/msg/VehicleControlCommand`] : target command to drive a vehicle.
+- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle.
+- input/vehicle_state_command [`autoware_auto_msgs/msg/VehicleStateCommand`] : target state command (e.g. gear).
+- /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose
-**output**
- - /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link)
- - /vehicle/vehicle_kinematic_state [`autoware_auto_msgs/msg/VehicleKinematicState`] : simulated kinematic state (defined in CoM)
- - /vehicle/state_report [`autoware_auto_msgs/msg/VehicleStateReport`] : current vehicle state (e.g. gear, mode, etc.)
+### output
+- /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link)
+- /vehicle/vehicle_kinematic_state [`autoware_auto_msgs/msg/VehicleKinematicState`] : simulated kinematic state (defined in CoM)
+- /vehicle/state_report [`autoware_auto_msgs/msg/VehicleStateReport`] : current vehicle state (e.g. gear, mode, etc.)
## Inner-workings / Algorithms
### Common Parameters
-|Name|Type|Description|Default value|
-|:---|:---|:---|:---|
-|simulated_frame_id | string | set to the child_frame_id in output tf |"base_link"|
-|origin_frame_id | string | set to the frame_id in output tf |"odom"|
-|initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | "INITIAL_POSE_TOPIC" |
-|add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results.| true|
-|pos_noise_stddev | double | Standard deviation for position noise | 0.01|
-|rpy_noise_stddev | double | Standard deviation for Euler angle noise| 0.0001|
-|vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0|
-|angvel_noise_stddev | double | Standard deviation for angular velocity noise| 0.0|
-|steer_noise_stddev | double | Standard deviation for steering angle noise| 0.0001|
-
-
+| Name | Type | Description | Default value |
+| :-------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------ | :------------------- | -------------------- |
+| simulated_frame_id | string | set to the child_frame_id in output tf | "base_link" |
+| origin_frame_id | string | set to the frame_id in output tf | "odom" |
+| initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | "INITIAL_POSE_TOPIC" |
+| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true |
+| pos_noise_stddev | double | Standard deviation for position noise | 0.01 |
+| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 |
+| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 |
+| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 |
+| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 |
### Vehicle Model Parameters
+#### vehicle_model_type options
-**vehicle_model_type options**
-
- - `IDEAL_STEER_VEL`
- - `IDEAL_STEER_ACC`
- - `IDEAL_STEER_ACC_GEARED`
- - `DELAY_STEER_VEL`
- - `DELAY_STEER_ACC`
- - `DELAY_STEER_ACC_GEARED`
+- `IDEAL_STEER_VEL`
+- `IDEAL_STEER_ACC`
+- `IDEAL_STEER_ACC_GEARED`
+- `DELAY_STEER_VEL`
+- `DELAY_STEER_ACC`
+- `DELAY_STEER_ACC_GEARED`
The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear.
The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V).
-
-|Name|Type|Description|I_ST_V|I_ST_A|I_ST_A_G|D_ST_V|D_ST_A|D_ST_A_G|Default value| unit |
-|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---|
-|acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] |
-|steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24| [s] |
-|vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25| [s] |
-|acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] |
-|steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27| [s] |
-|vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] |
-|vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0| [m/s] |
-|vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] |
-|steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] |
-|steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] |
-<!-- |deadzone_delta_steer | double | dead zone for the steering dynamics | x | x | x | o | o | 0.0 | [rad] | -->
-
-
-*Note*: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a *delay* model. The definition of the *time constant* is the time it takes for the step response to rise up to 63% of its final value. The *deadtime* is a delay in the response to a control input.
-
+| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit |
+| :------------------ | :------------------- | :--------------------------------------------------- | :---------------------------------- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | --- |
+| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] |
+| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] |
+| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] |
+| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] |
+| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] |
+| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] |
+| vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] |
+| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] |
+| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] |
+| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] |
+| <!-- | deadzone_delta_steer | double | dead zone for the steering dynamics | x | x | x | o | o | 0.0 | [rad] | --> |
+
+_Note_: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a _delay_ model. The definition of the _time constant_ is the time it takes for the step response to rise up to 63% of its final value. The _deadtime_ is a delay in the response to a control input.
### Default TF configuration
Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration.
In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0.
-
## Error detection and handling
The only validation on inputs being done is testing for a valid vehicle model type.
+## Security considerations
-# Security considerations
<!-- Required -->
<!-- Things to consider:
- Spoofing (How do you check for and handle fake input?)
@@ -108,18 +95,17 @@ The only validation on inputs being done is testing for a valid vehicle model ty
- Denial of Service (How do you handle spamming?).
- Elevation of Privilege (Do you need to change permission levels during execution?) -->
+## References / External links
-# References / External links
This is originally developed in the Autoware.AI. See the link below.
-https://github.com/Autoware-AI/simulation/tree/master/wf_simulator
-
-# Future extensions / Unimplemented parts
+<https://github.com/Autoware-AI/simulation/tree/master/wf_simulator>
- - Improving the accuracy of vehicle models (e.g., adding steering dead zones and slip behavior)
- - Cooperation with modules that output pseudo pointcloud or pseudo perception results
+## Future extensions / Unimplemented parts
+- Improving the accuracy of vehicle models (e.g., adding steering dead zones and slip behavior)
+- Cooperation with modules that output pseudo pointcloud or pseudo perception results
-# Related issues
+## Related issues
- - #1142: Follow-up to #570 - Integrate simple_planning_simulator Into CI
+- #1142: Follow-up to #570 - Integrate simple_planning_simulator Into CI
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
index a948f4694d409..369b9b63c7465 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -15,54 +15,49 @@
#ifndef SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_
#define SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_
-
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
-
-#include <memory>
-#include <string>
-#include <random>
-
+#include "common/types.hpp"
#include "rclcpp/rclcpp.hpp"
-
+#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
#include "simple_planning_simulator/visibility_control.hpp"
-
-#include "geometry_msgs/msg/pose.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "geometry_msgs/msg/twist.hpp"
-#include "geometry_msgs/msg/transform_stamped.hpp"
-#include "nav_msgs/msg/odometry.hpp"
+#include "tier4_api_utils/tier4_api_utils.hpp"
#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
+#include "autoware_auto_geometry_msgs/msg/complex32.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/engage.hpp"
#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
-#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp"
-#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp"
+#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
-#include "autoware_auto_geometry_msgs/msg/complex32.hpp"
-#include "common/types.hpp"
-
-#include "tier4_api_utils/tier4_api_utils.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "nav_msgs/msg/odometry.hpp"
#include "tier4_external_api_msgs/srv/initialize_pose.hpp"
-#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <memory>
+#include <random>
+#include <string>
+#include <vector>
namespace simulation
{
namespace simple_planning_simulator
{
+using autoware::common::types::bool8_t;
using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
-using autoware::common::types::bool8_t;
using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_geometry_msgs::msg::Complex32;
@@ -71,26 +66,25 @@ using autoware_auto_vehicle_msgs::msg::ControlModeReport;
using autoware_auto_vehicle_msgs::msg::Engage;
using autoware_auto_vehicle_msgs::msg::GearCommand;
using autoware_auto_vehicle_msgs::msg::GearReport;
-using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
-using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport;
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
using autoware_auto_vehicle_msgs::msg::HazardLightsReport;
using autoware_auto_vehicle_msgs::msg::SteeringReport;
+using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport;
using autoware_auto_vehicle_msgs::msg::VehicleControlCommand;
using autoware_auto_vehicle_msgs::msg::VelocityReport;
-using tier4_external_api_msgs::srv::InitializePose;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseStamped;
using geometry_msgs::msg::PoseWithCovarianceStamped;
using geometry_msgs::msg::TransformStamped;
using geometry_msgs::msg::Twist;
using nav_msgs::msg::Odometry;
+using tier4_external_api_msgs::srv::InitializePose;
class DeltaTime
{
public:
- DeltaTime()
- : prev_updated_time_ptr_(nullptr) {}
+ DeltaTime() : prev_updated_time_ptr_(nullptr) {}
float64_t get_dt(const rclcpp::Time & now)
{
if (prev_updated_time_ptr_ == nullptr) {
@@ -147,7 +141,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
rclcpp::CallbackGroup::SharedPtr group_api_service_;
tier4_api_utils::Service<InitializePose>::SharedPtr srv_set_pose_;
- uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time
+ uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time
rclcpp::TimerBase::SharedPtr on_timer_; //!< @brief timer for simulation
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
@@ -172,7 +166,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
/* frame_id */
std::string simulated_frame_id_; //!< @brief simulated vehicle frame id
- std::string origin_frame_id_; //!< @brief map frame_id
+ std::string origin_frame_id_; //!< @brief map frame_id
/* flags */
bool8_t is_initialized_; //!< @brief flag to check the initial position is set
@@ -186,8 +180,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
float64_t y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate
/* vehicle model */
- enum class VehicleModelType
- {
+ enum class VehicleModelType {
IDEAL_STEER_ACC = 0,
IDEAL_STEER_ACC_GEARED = 1,
DELAY_STEER_ACC = 2,
@@ -218,13 +211,13 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
void on_gear_cmd(const GearCommand::ConstSharedPtr msg);
/**
- * @brief set current_vehicle_state_ with received message
- */
+ * @brief set current_vehicle_state_ with received message
+ */
void on_turn_indicators_cmd(const TurnIndicatorsCommand::ConstSharedPtr msg);
/**
- * @brief set current_vehicle_state_ with received message
- */
+ * @brief set current_vehicle_state_ with received message
+ */
void on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg);
/**
@@ -331,8 +324,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
void publish_turn_indicators_report();
/**
- * @brief publish hazard lights report
- */
+ * @brief publish hazard lights report
+ */
void publish_hazard_lights_report();
/**
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp
index 5f1de6e2e819b..c111803d4d8c5 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp
@@ -15,13 +15,12 @@
#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_
-#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
-#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp"
-#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp"
-#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp"
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp"
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp"
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp"
-
+#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp"
+#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp"
+#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp"
+#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp
index f0a610c54eb0f..1941189463859 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp
@@ -15,15 +15,14 @@
#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_
+#include "eigen3/Eigen/Core"
+#include "eigen3/Eigen/LU"
+#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+
#include <deque>
#include <iostream>
#include <queue>
-#include "eigen3/Eigen/LU"
-#include "eigen3/Eigen/Core"
-
-#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
-
class SimModelDelaySteerAcc : public SimModelInterface
{
public:
@@ -53,8 +52,7 @@ class SimModelDelaySteerAcc : public SimModelInterface
private:
const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant
- enum IDX
- {
+ enum IDX {
X = 0,
Y,
YAW,
@@ -62,8 +60,7 @@ class SimModelDelaySteerAcc : public SimModelInterface
STEER,
ACCX,
};
- enum IDX_U
- {
+ enum IDX_U {
ACCX_DES = 0,
STEER_DES,
DRIVE_SHIFT,
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp
index da37a7825956c..d8bce80e8b0f6 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp
@@ -15,15 +15,14 @@
#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_
+#include "eigen3/Eigen/Core"
+#include "eigen3/Eigen/LU"
+#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+
#include <deque>
#include <iostream>
#include <queue>
-#include "eigen3/Eigen/LU"
-#include "eigen3/Eigen/Core"
-
-#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
-
class SimModelDelaySteerAccGeared : public SimModelInterface
{
public:
@@ -53,8 +52,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface
private:
const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant
- enum IDX
- {
+ enum IDX {
X = 0,
Y,
YAW,
@@ -62,8 +60,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface
STEER,
ACCX,
};
- enum IDX_U
- {
+ enum IDX_U {
ACCX_DES = 0,
STEER_DES,
DRIVE_SHIFT,
@@ -148,7 +145,9 @@ class SimModelDelaySteerAccGeared : public SimModelInterface
* @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand)
* @param [in] dt delta time to update state
*/
- void updateStateWithGear(Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt);
+ void updateStateWithGear(
+ Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear,
+ const double dt);
};
#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp
index d719b420e0ace..804d3b6b43790 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp
@@ -15,14 +15,13 @@
#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_
-#include <deque>
-#include <iostream>
-#include <queue>
-
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/LU"
-
#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+
+#include <deque>
+#include <iostream>
+#include <queue>
/**
* @class SimModelDelaySteerVel
* @brief calculate delay steering dynamics
@@ -56,34 +55,32 @@ class SimModelDelaySteerVel : public SimModelInterface
private:
const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant
- enum IDX
- {
+ enum IDX {
X = 0,
Y,
YAW,
VX,
STEER,
};
- enum IDX_U
- {
+ enum IDX_U {
VX_DES = 0,
STEER_DES,
};
- const float64_t vx_lim_; //!< @brief velocity limit
- const float64_t vx_rate_lim_; //!< @brief acceleration limit
+ const float64_t vx_lim_; //!< @brief velocity limit
+ const float64_t vx_rate_lim_; //!< @brief acceleration limit
const float64_t steer_lim_; //!< @brief steering limit [rad]
const float64_t steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s]
const float64_t wheelbase_; //!< @brief vehicle wheelbase length [m]
float64_t prev_vx_ = 0.0;
float64_t current_ax_ = 0.0;
- std::deque<float64_t> vx_input_queue_; //!< @brief buffer for velocity command
+ std::deque<float64_t> vx_input_queue_; //!< @brief buffer for velocity command
std::deque<float64_t> steer_input_queue_; //!< @brief buffer for angular velocity command
- const float64_t vx_delay_; //!< @brief time delay for velocity command [s]
+ const float64_t vx_delay_; //!< @brief time delay for velocity command [s]
const float64_t vx_time_constant_;
- //!< @brief time constant for 1D model of velocity dynamics
- const float64_t steer_delay_; //!< @brief time delay for angular-velocity command [s]
+ //!< @brief time constant for 1D model of velocity dynamics
+ const float64_t steer_delay_; //!< @brief time delay for angular-velocity command [s]
const float64_t
steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp
index c4aca56e5bd51..7669d88c71f99 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp
@@ -15,13 +15,12 @@
#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_
-#include <iostream>
-
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/LU"
-
#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+#include <iostream>
+
/**
* @class SimModelIdealSteerAcc
* @brief calculate ideal steering dynamics
@@ -41,15 +40,8 @@ class SimModelIdealSteerAcc : public SimModelInterface
~SimModelIdealSteerAcc() = default;
private:
- enum IDX
- {
- X = 0,
- Y,
- YAW,
- VX
- };
- enum IDX_U
- {
+ enum IDX { X = 0, Y, YAW, VX };
+ enum IDX_U {
AX_DES = 0,
STEER_DES,
};
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp
index 64a9e3109cf44..e2bef39dfa9bc 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp
@@ -15,13 +15,12 @@
#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_
-#include <iostream>
-
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/LU"
-
#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+#include <iostream>
+
/**
* @class SimModelIdealSteerAccGeared
* @brief calculate ideal steering dynamics
@@ -41,21 +40,14 @@ class SimModelIdealSteerAccGeared : public SimModelInterface
~SimModelIdealSteerAccGeared() = default;
private:
- enum IDX
- {
- X = 0,
- Y,
- YAW,
- VX
- };
- enum IDX_U
- {
+ enum IDX { X = 0, Y, YAW, VX };
+ enum IDX_U {
AX_DES = 0,
STEER_DES,
};
const float64_t wheelbase_; //!< @brief vehicle wheelbase length
- float64_t current_acc_; //!< @brief current_acc with gear consideration
+ float64_t current_acc_; //!< @brief current_acc with gear consideration
/**
* @brief get vehicle position x
@@ -117,7 +109,9 @@ class SimModelIdealSteerAccGeared : public SimModelInterface
* @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand)
* @param [in] dt delta time to update state
*/
- void updateStateWithGear(Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt);
+ void updateStateWithGear(
+ Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear,
+ const double dt);
};
#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp
index dae5f0fab633c..06f62e45c1b16 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp
@@ -15,12 +15,11 @@
#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_
-#include <iostream>
-
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/LU"
-
#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
+
+#include <iostream>
/**
* @class SimModelIdealSteerVel
* @brief calculate ideal steering dynamics
@@ -40,14 +39,8 @@ class SimModelIdealSteerVel : public SimModelInterface
~SimModelIdealSteerVel() = default;
private:
- enum IDX
- {
- X = 0,
- Y,
- YAW
- };
- enum IDX_U
- {
+ enum IDX { X = 0, Y, YAW };
+ enum IDX_U {
VX_DES = 0,
STEER_DES,
};
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp
index 684b7f19ce738..d883706a4ace1 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp
@@ -15,13 +15,14 @@
#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_
+#include "common/types.hpp"
#include "eigen3/Eigen/Core"
+
#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp"
-#include "common/types.hpp"
+using autoware::common::types::bool8_t;
using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
-using autoware::common::types::bool8_t;
/**
* @class SimModelInterface
@@ -144,12 +145,12 @@ class SimModelInterface
/**
* @brief get state vector dimension
*/
- inline int getDimX() {return dim_x_;}
+ inline int getDimX() { return dim_x_; }
/**
* @brief get input vector demension
*/
- inline int getDimU() {return dim_u_;}
+ inline int getDimU() { return dim_u_; }
/**
* @brief calculate derivative of states with vehicle model
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp
index d1179bbcc833d..093893b3f9ae9 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp
@@ -12,13 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-
#ifndef SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_
#define SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_
#if defined(__WIN32)
-#if defined(PLANNING_SIMULATOR_BUILDING_DLL) || \
- defined(PLANNING_SIMULATOR_EXPORTS)
+#if defined(PLANNING_SIMULATOR_BUILDING_DLL) || defined(PLANNING_SIMULATOR_EXPORTS)
#define PLANNING_SIMULATOR_PUBLIC __declspec(dllexport)
#define PLANNING_SIMULATOR_LOCAL
#else // defined(PLANNING_SIMULATOR_BUILDING_DLL) ||
diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
index bb4cba446151f..ce87de1d62862 100644
--- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
+++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
@@ -12,32 +12,25 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-import os
-
-from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.actions import GroupAction
-from launch.actions import IncludeLaunchDescription
from launch.actions import OpaqueFunction
-from launch.actions import SetLaunchConfiguration
-from launch.conditions import IfCondition
-from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import ComposableNodeContainer
-from launch_ros.actions import LoadComposableNodes
from launch_ros.actions import Node
-from launch_ros.descriptions import ComposableNode
from launch_ros.substitutions import FindPackageShare
import yaml
+
def launch_setup(context, *args, **kwargs):
# vehicle information param path
vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context)
with open(vehicle_info_param_path, "r") as f:
vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- vehicle_characteristics_param_path = LaunchConfiguration("vehicle_characteristics_param_file").perform(context)
+ vehicle_characteristics_param_path = LaunchConfiguration(
+ "vehicle_characteristics_param_file"
+ ).perform(context)
with open(vehicle_characteristics_param_path, "r") as f:
vehicle_characteristics_param = yaml.safe_load(f)["/**"]["ros__parameters"]
@@ -46,11 +39,11 @@ def launch_setup(context, *args, **kwargs):
simulator_model_param = yaml.safe_load(f)["/**"]["ros__parameters"]
simple_planning_simulator_node = Node(
- package='simple_planning_simulator',
- executable='simple_planning_simulator_exe',
- name='simple_planning_simulator',
- namespace='simulation',
- output='screen',
+ package="simple_planning_simulator",
+ executable="simple_planning_simulator_exe",
+ name="simple_planning_simulator",
+ namespace="simulation",
+ output="screen",
parameters=[
vehicle_info_param,
vehicle_characteristics_param,
@@ -60,37 +53,33 @@ def launch_setup(context, *args, **kwargs):
},
],
remappings=[
- ('input/ackermann_control_command', '/control/command/control_cmd'),
- ('input/gear_command', '/control/command/gear_cmd'),
- ('input/turn_indicators_command', '/control/command/turn_indicators_cmd'),
- ('input/hazard_lights_command', '/control/command/hazard_lights_cmd'),
- ('input/trajectory', '/planning/scenario_planning/trajectory'),
- ('input/engage', '/vehicle/engage'),
- ('output/twist', '/vehicle/status/velocity_status'),
- ('output/odometry', '/localization/kinematic_state'),
- ('output/steering', '/vehicle/status/steering_status'),
- ('output/gear_report', '/vehicle/status/gear_status'),
- ('output/turn_indicators_report', '/vehicle/status/turn_indicators_status'),
- ('output/hazard_lights_report', '/vehicle/status/hazard_lights_status'),
- ('output/control_mode_report', '/vehicle/status/control_mode'),
- ('/initialpose', '/initialpose'),
- ]
+ ("input/ackermann_control_command", "/control/command/control_cmd"),
+ ("input/gear_command", "/control/command/gear_cmd"),
+ ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
+ ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
+ ("input/trajectory", "/planning/scenario_planning/trajectory"),
+ ("input/engage", "/vehicle/engage"),
+ ("output/twist", "/vehicle/status/velocity_status"),
+ ("output/odometry", "/localization/kinematic_state"),
+ ("output/steering", "/vehicle/status/steering_status"),
+ ("output/gear_report", "/vehicle/status/gear_status"),
+ ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
+ ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
+ ("output/control_mode_report", "/vehicle/status/control_mode"),
+ ("/initialpose", "/initialpose"),
+ ],
)
map_to_odom_tf_publisher = Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name='static_map_to_odom_tf_publisher',
- output='screen',
- arguments=['0.0', '0.0', '0.0', '0', '0', '0', 'map', 'odom'])
-
- group = GroupAction(
- [
- simple_planning_simulator_node,
- map_to_odom_tf_publisher
- ]
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="static_map_to_odom_tf_publisher",
+ output="screen",
+ arguments=["0.0", "0.0", "0.0", "0", "0", "0", "map", "odom"],
)
+ group = GroupAction([simple_planning_simulator_node, map_to_odom_tf_publisher])
+
return [group]
@@ -129,9 +118,4 @@ def add_launch_arg(name: str, default_value=None, description=None):
"path to config file for simulator_model",
)
-
-
- return launch.LaunchDescription(
- launch_arguments
- + [OpaqueFunction(function=launch_setup)]
- )
+ return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])
diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml
index 619694b3de0dc..98ac90bafa677 100644
--- a/simulator/simple_planning_simulator/package.xml
+++ b/simulator/simple_planning_simulator/package.xml
@@ -10,23 +10,21 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_auto_cmake</buildtool_depend>
- <depend>tier4_api_utils</depend>
- <depend>autoware_auto_planning_msgs</depend>
+ <depend>autoware_auto_common</depend>
<depend>autoware_auto_control_msgs</depend>
- <depend>autoware_auto_vehicle_msgs</depend>
+ <depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_tf2</depend>
- <depend>autoware_auto_common</depend>
- <depend>tier4_external_api_msgs</depend>
+ <depend>autoware_auto_vehicle_msgs</depend>
<depend>geometry_msgs</depend>
- <depend>nav_msgs</depend>
- <depend>vehicle_info_util</depend>
- <depend>tier4_autoware_utils</depend>
-
<depend>motion_common</depend>
-
+ <depend>nav_msgs</depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
+ <depend>tier4_api_utils</depend>
+ <depend>tier4_autoware_utils</depend>
+ <depend>tier4_external_api_msgs</depend>
+ <depend>vehicle_info_util</depend>
<exec_depend>launch_ros</exec_depend>
@@ -34,5 +32,7 @@
<!-- <test_depend>ament_lint_auto</test_depend> -->
<!-- <test_depend>ament_lint_common</test_depend> -->
- <export><build_type>ament_cmake</build_type></export>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
</package>
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
index f9927114fc35c..2eb04d42acc81 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -15,11 +15,11 @@
#include "simple_planning_simulator/simple_planning_simulator_core.hpp"
#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp"
-#include "tier4_autoware_utils/ros/update_param.hpp"
#include "common/types.hpp"
#include "motion_common/motion_common.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "simple_planning_simulator/vehicle_model/sim_model.hpp"
+#include "tier4_autoware_utils/ros/update_param.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"
#include <tf2/LinearMath/Quaternion.h>
@@ -31,7 +31,7 @@
#include <string>
#include <utility>
-using namespace std::chrono_literals;
+using namespace std::literals::chrono_literals;
namespace
{
@@ -314,30 +314,28 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons
using autoware_auto_vehicle_msgs::msg::GearCommand;
Eigen::VectorXd input(vehicle_model_ptr_->getDimU());
- // TODO (Watanabe): The definition of the sign of acceleration in REVERSE mode is different
+ // TODO(Watanabe): The definition of the sign of acceleration in REVERSE mode is different
// between .auto and proposal.iv, and will be discussed later.
float acc = accel;
if (!current_gear_cmd_ptr_) {
acc = 0.0;
} else if (
current_gear_cmd_ptr_->command == GearCommand::REVERSE ||
- current_gear_cmd_ptr_->command == GearCommand::REVERSE_2)
- {
+ current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) {
acc = -accel;
}
- if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL ||
- vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) {
+ if (
+ vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL ||
+ vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) {
input << vel, steer;
} else if ( // NOLINT
vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC ||
- vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC)
- {
+ vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) {
input << acc, steer;
} else if ( // NOLINT
vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED ||
- vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED)
- {
+ vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) {
input << acc, steer;
}
vehicle_model_ptr_->setInput(input);
@@ -349,8 +347,7 @@ void SimplePlanningSimulator::on_gear_cmd(const GearCommand::ConstSharedPtr msg)
if (
vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED ||
- vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED)
- {
+ vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) {
vehicle_model_ptr_->setGear(current_gear_cmd_ptr_->command);
}
}
@@ -420,17 +417,14 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist &
state << x, y, yaw;
} else if ( // NOLINT
vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC ||
- vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED)
- {
+ vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) {
state << x, y, yaw, vx;
} else if ( // NOLINT
- vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL)
- {
+ vehicle_model_type_ == VehicleModelType::DELAY_STEER_VEL) {
state << x, y, yaw, vx, steer;
} else if ( // NOLINT
vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC ||
- vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED)
- {
+ vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) {
state << x, y, yaw, vx, steer, accx;
}
vehicle_model_ptr_->setState(state);
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
index d2fff30b202ce..df3845bf14fa0 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
@@ -35,17 +35,17 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc(
initializeInputQueue(dt);
}
-float64_t SimModelDelaySteerAcc::getX() {return state_(IDX::X);}
-float64_t SimModelDelaySteerAcc::getY() {return state_(IDX::Y);}
-float64_t SimModelDelaySteerAcc::getYaw() {return state_(IDX::YAW);}
-float64_t SimModelDelaySteerAcc::getVx() {return state_(IDX::VX);}
-float64_t SimModelDelaySteerAcc::getVy() {return 0.0;}
-float64_t SimModelDelaySteerAcc::getAx() {return state_(IDX::ACCX);}
+float64_t SimModelDelaySteerAcc::getX() { return state_(IDX::X); }
+float64_t SimModelDelaySteerAcc::getY() { return state_(IDX::Y); }
+float64_t SimModelDelaySteerAcc::getYaw() { return state_(IDX::YAW); }
+float64_t SimModelDelaySteerAcc::getVx() { return state_(IDX::VX); }
+float64_t SimModelDelaySteerAcc::getVy() { return 0.0; }
+float64_t SimModelDelaySteerAcc::getAx() { return state_(IDX::ACCX); }
float64_t SimModelDelaySteerAcc::getWz()
{
return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_;
}
-float64_t SimModelDelaySteerAcc::getSteer() {return state_(IDX::STEER);}
+float64_t SimModelDelaySteerAcc::getSteer() { return state_(IDX::STEER); }
void SimModelDelaySteerAcc::update(const float64_t & dt)
{
Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_);
@@ -76,7 +76,7 @@ void SimModelDelaySteerAcc::initializeInputQueue(const float64_t & dt)
Eigen::VectorXd SimModelDelaySteerAcc::calcModel(
const Eigen::VectorXd & state, const Eigen::VectorXd & input)
{
- auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);};
+ auto sat = [](float64_t val, float64_t u, float64_t l) { return std::max(std::min(val, u), l); };
const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_);
const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_);
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
index 0a16ce0d32454..938966bcb13ce 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
@@ -12,11 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <algorithm>
-
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp"
+
#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp"
+#include <algorithm>
+
SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared(
float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim,
float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant,
@@ -36,17 +37,17 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared(
initializeInputQueue(dt);
}
-float64_t SimModelDelaySteerAccGeared::getX() {return state_(IDX::X);}
-float64_t SimModelDelaySteerAccGeared::getY() {return state_(IDX::Y);}
-float64_t SimModelDelaySteerAccGeared::getYaw() {return state_(IDX::YAW);}
-float64_t SimModelDelaySteerAccGeared::getVx() {return state_(IDX::VX);}
-float64_t SimModelDelaySteerAccGeared::getVy() {return 0.0;}
-float64_t SimModelDelaySteerAccGeared::getAx() {return state_(IDX::ACCX);}
+float64_t SimModelDelaySteerAccGeared::getX() { return state_(IDX::X); }
+float64_t SimModelDelaySteerAccGeared::getY() { return state_(IDX::Y); }
+float64_t SimModelDelaySteerAccGeared::getYaw() { return state_(IDX::YAW); }
+float64_t SimModelDelaySteerAccGeared::getVx() { return state_(IDX::VX); }
+float64_t SimModelDelaySteerAccGeared::getVy() { return 0.0; }
+float64_t SimModelDelaySteerAccGeared::getAx() { return state_(IDX::ACCX); }
float64_t SimModelDelaySteerAccGeared::getWz()
{
return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_;
}
-float64_t SimModelDelaySteerAccGeared::getSteer() {return state_(IDX::STEER);}
+float64_t SimModelDelaySteerAccGeared::getSteer() { return state_(IDX::STEER); }
void SimModelDelaySteerAccGeared::update(const float64_t & dt)
{
Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_);
@@ -83,7 +84,7 @@ void SimModelDelaySteerAccGeared::initializeInputQueue(const float64_t & dt)
Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel(
const Eigen::VectorXd & state, const Eigen::VectorXd & input)
{
- auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);};
+ auto sat = [](float64_t val, float64_t u, float64_t l) { return std::max(std::min(val, u), l); };
const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_);
const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_);
@@ -117,8 +118,7 @@ void SimModelDelaySteerAccGeared::updateStateWithGear(
gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 ||
gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 ||
gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 ||
- gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2)
- {
+ gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) {
if (state(IDX::VX) < 0.0) {
state(IDX::VX) = 0.0;
state(IDX::X) = prev_state(IDX::X);
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp
index 45dabfc572216..6841f01dd4a65 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp
@@ -39,13 +39,13 @@ float64_t SimModelDelaySteerVel::getX() { return state_(IDX::X); }
float64_t SimModelDelaySteerVel::getY() { return state_(IDX::Y); }
float64_t SimModelDelaySteerVel::getYaw() { return state_(IDX::YAW); }
float64_t SimModelDelaySteerVel::getVx() { return state_(IDX::VX); }
-float64_t SimModelDelaySteerVel::getVy() {return 0.0;}
-float64_t SimModelDelaySteerVel::getAx() {return current_ax_;}
+float64_t SimModelDelaySteerVel::getVy() { return 0.0; }
+float64_t SimModelDelaySteerVel::getAx() { return current_ax_; }
float64_t SimModelDelaySteerVel::getWz()
{
return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_;
}
-float64_t SimModelDelaySteerVel::getSteer() {return state_(IDX::STEER);}
+float64_t SimModelDelaySteerVel::getSteer() { return state_(IDX::STEER); }
void SimModelDelaySteerVel::update(const float64_t & dt)
{
Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_);
@@ -74,11 +74,10 @@ void SimModelDelaySteerVel::initializeInputQueue(const float64_t & dt)
}
}
-
Eigen::VectorXd SimModelDelaySteerVel::calcModel(
const Eigen::VectorXd & state, const Eigen::VectorXd & input)
{
- auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);};
+ auto sat = [](float64_t val, float64_t u, float64_t l) { return std::max(std::min(val, u), l); };
const float64_t vx = sat(state(IDX::VX), vx_lim_, -vx_lim_);
const float64_t steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_);
@@ -101,4 +100,3 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel(
return d_state;
}
-
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp
index ee74b645850cf..9933ba5be790c 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp
@@ -14,25 +14,23 @@
#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp"
-
SimModelIdealSteerAcc::SimModelIdealSteerAcc(float64_t wheelbase)
-: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {}
+: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase)
+{
+}
-float64_t SimModelIdealSteerAcc::getX() {return state_(IDX::X);}
-float64_t SimModelIdealSteerAcc::getY() {return state_(IDX::Y);}
-float64_t SimModelIdealSteerAcc::getYaw() {return state_(IDX::YAW);}
-float64_t SimModelIdealSteerAcc::getVx() {return state_(IDX::VX);}
-float64_t SimModelIdealSteerAcc::getVy() {return 0.0;}
-float64_t SimModelIdealSteerAcc::getAx() {return input_(IDX_U::AX_DES);}
+float64_t SimModelIdealSteerAcc::getX() { return state_(IDX::X); }
+float64_t SimModelIdealSteerAcc::getY() { return state_(IDX::Y); }
+float64_t SimModelIdealSteerAcc::getYaw() { return state_(IDX::YAW); }
+float64_t SimModelIdealSteerAcc::getVx() { return state_(IDX::VX); }
+float64_t SimModelIdealSteerAcc::getVy() { return 0.0; }
+float64_t SimModelIdealSteerAcc::getAx() { return input_(IDX_U::AX_DES); }
float64_t SimModelIdealSteerAcc::getWz()
{
return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_;
}
-float64_t SimModelIdealSteerAcc::getSteer() {return input_(IDX_U::STEER_DES);}
-void SimModelIdealSteerAcc::update(const float64_t & dt)
-{
- updateRungeKutta(dt, input_);
-}
+float64_t SimModelIdealSteerAcc::getSteer() { return input_(IDX_U::STEER_DES); }
+void SimModelIdealSteerAcc::update(const float64_t & dt) { updateRungeKutta(dt, input_); }
Eigen::VectorXd SimModelIdealSteerAcc::calcModel(
const Eigen::VectorXd & state, const Eigen::VectorXd & input)
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp
index 8a8a55dffc538..a9aeeac9f8834 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp
@@ -12,25 +12,28 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <algorithm>
-
#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp"
+
#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp"
+#include <algorithm>
+
SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(float64_t wheelbase)
-: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) {}
+: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0)
+{
+}
-float64_t SimModelIdealSteerAccGeared::getX() {return state_(IDX::X);}
-float64_t SimModelIdealSteerAccGeared::getY() {return state_(IDX::Y);}
-float64_t SimModelIdealSteerAccGeared::getYaw() {return state_(IDX::YAW);}
-float64_t SimModelIdealSteerAccGeared::getVx() {return state_(IDX::VX);}
-float64_t SimModelIdealSteerAccGeared::getVy() {return 0.0;}
-float64_t SimModelIdealSteerAccGeared::getAx() {return current_acc_;}
+float64_t SimModelIdealSteerAccGeared::getX() { return state_(IDX::X); }
+float64_t SimModelIdealSteerAccGeared::getY() { return state_(IDX::Y); }
+float64_t SimModelIdealSteerAccGeared::getYaw() { return state_(IDX::YAW); }
+float64_t SimModelIdealSteerAccGeared::getVx() { return state_(IDX::VX); }
+float64_t SimModelIdealSteerAccGeared::getVy() { return 0.0; }
+float64_t SimModelIdealSteerAccGeared::getAx() { return current_acc_; }
float64_t SimModelIdealSteerAccGeared::getWz()
{
return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_;
}
-float64_t SimModelIdealSteerAccGeared::getSteer() {return input_(IDX_U::STEER_DES);}
+float64_t SimModelIdealSteerAccGeared::getSteer() { return input_(IDX_U::STEER_DES); }
void SimModelIdealSteerAccGeared::update(const float64_t & dt)
{
const auto prev_state = state_;
@@ -70,8 +73,7 @@ void SimModelIdealSteerAccGeared::updateStateWithGear(
gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 ||
gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 ||
gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 ||
- gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2)
- {
+ gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) {
if (state(IDX::VX) < 0.0) {
state(IDX::VX) = 0.0;
state(IDX::X) = prev_state(IDX::X);
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp
index ea40f647e2379..7755f50054454 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp
@@ -14,21 +14,22 @@
#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp"
-
SimModelIdealSteerVel::SimModelIdealSteerVel(float64_t wheelbase)
-: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {}
+: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase)
+{
+}
-float64_t SimModelIdealSteerVel::getX() {return state_(IDX::X);}
-float64_t SimModelIdealSteerVel::getY() {return state_(IDX::Y);}
-float64_t SimModelIdealSteerVel::getYaw() {return state_(IDX::YAW);}
-float64_t SimModelIdealSteerVel::getVx() {return input_(IDX_U::VX_DES);}
-float64_t SimModelIdealSteerVel::getVy() {return 0.0;}
-float64_t SimModelIdealSteerVel::getAx() {return current_ax_;}
+float64_t SimModelIdealSteerVel::getX() { return state_(IDX::X); }
+float64_t SimModelIdealSteerVel::getY() { return state_(IDX::Y); }
+float64_t SimModelIdealSteerVel::getYaw() { return state_(IDX::YAW); }
+float64_t SimModelIdealSteerVel::getVx() { return input_(IDX_U::VX_DES); }
+float64_t SimModelIdealSteerVel::getVy() { return 0.0; }
+float64_t SimModelIdealSteerVel::getAx() { return current_ax_; }
float64_t SimModelIdealSteerVel::getWz()
{
return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_;
}
-float64_t SimModelIdealSteerVel::getSteer() {return input_(IDX_U::STEER_DES);}
+float64_t SimModelIdealSteerVel::getSteer() { return input_(IDX_U::STEER_DES); }
void SimModelIdealSteerVel::update(const float64_t & dt)
{
updateRungeKutta(dt, input_);
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp
index 13bf38fc11f72..c3c36fd7846fc 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp
@@ -14,8 +14,7 @@
#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"
-SimModelInterface::SimModelInterface(int dim_x, int dim_u)
-: dim_x_(dim_x), dim_u_(dim_u)
+SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u)
{
state_ = Eigen::VectorXd::Zero(dim_x_);
input_ = Eigen::VectorXd::Zero(dim_u_);
@@ -34,8 +33,8 @@ void SimModelInterface::updateEuler(const float64_t & dt, const Eigen::VectorXd
{
state_ += calcModel(state_, input) * dt;
}
-void SimModelInterface::getState(Eigen::VectorXd & state) {state = state_;}
-void SimModelInterface::getInput(Eigen::VectorXd & input) {input = input_;}
-void SimModelInterface::setState(const Eigen::VectorXd & state) {state_ = state;}
-void SimModelInterface::setInput(const Eigen::VectorXd & input) {input_ = input;}
-void SimModelInterface::setGear(const uint8_t gear) {gear_ = gear;}
+void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; }
+void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; }
+void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; }
+void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; }
+void SimModelInterface::setGear(const uint8_t gear) { gear_ = gear; }
diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
index ad872909740b3..7dc96e9c7f617 100644
--- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
+++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp
@@ -42,9 +42,8 @@ class PubSubNode : public rclcpp::Node
PubSubNode() : Node{"test_simple_planning_simulator_pubsub"}
{
current_odom_sub_ = create_subscription<Odometry>(
- "output/odometry", rclcpp::QoS{1}, [this](const Odometry::SharedPtr msg) {
- current_odom_ = msg;
- });
+ "output/odometry", rclcpp::QoS{1},
+ [this](const Odometry::SharedPtr msg) { current_odom_ = msg; });
pub_ackermann_command_ =
create_publisher<AckermannControlCommand>("input/ackermann_control_command", rclcpp::QoS{1});
pub_initialpose_ = create_publisher<PoseWithCovarianceStamped>("/initialpose", rclcpp::QoS{1});
@@ -187,8 +186,6 @@ void declareVehicleInfoParams(rclcpp::NodeOptions & node_options)
node_options.append_parameter_override("vehicle_height", 1.5);
}
-
-
// Send a control command and run the simulation.
// Then check if the vehicle is moving in the desired direction.
class TestSimplePlanningSimulator : public ::testing::TestWithParam<std::string>
@@ -263,14 +260,12 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel)
rclcpp::shutdown();
}
-const std::string VEHICLE_MODEL_LIST[] = {
- "IDEAL_STEER_VEL",
- "IDEAL_STEER_ACC",
- "IDEAL_STEER_ACC_GEARED",
- "DELAY_STEER_VEL",
- "DELAY_STEER_ACC",
- "DELAY_STEER_ACC_GEARED",
+// clang-format off
+const std::string VEHICLE_MODEL_LIST[] = { // NOLINT
+ "IDEAL_STEER_VEL", "IDEAL_STEER_ACC", "IDEAL_STEER_ACC_GEARED",
+ "DELAY_STEER_VEL", "DELAY_STEER_ACC", "DELAY_STEER_ACC_GEARED",
};
+// clang-format on
INSTANTIATE_TEST_CASE_P(
TestForEachVehicleModel, TestSimplePlanningSimulator, ::testing::ValuesIn(VEHICLE_MODEL_LIST));