diff --git a/.cspell.json b/.cspell.json index 94b509370e688..b0d572db8726b 100644 --- a/.cspell.json +++ b/.cspell.json @@ -1,7 +1,7 @@ { "ignorePaths": [ - "perception/bytetrack/lib/**", - "planning/behavior_velocity_intersection_module/scripts/**" + "perception/autoware_bytetrack/lib/**", + "planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/**" ], "ignoreRegExpList": [], "words": ["dltype", "tvmgen", "fromarray"] diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index ebfbf7615689a..9494bcc10f575 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -87,22 +87,22 @@ localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/autoware_geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/autoware_map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/autoware_map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp perception/autoware_bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp perception/autoware_cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp diff --git a/.github/README.md b/.github/overall-ci-infrastructure.md similarity index 100% rename from .github/README.md rename to .github/overall-ci-infrastructure.md diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt index 4c36ef2f4e70d..e7f1f56a451f4 100644 --- a/common/autoware_motion_utils/CMakeLists.txt +++ b/common/autoware_motion_utils/CMakeLists.txt @@ -1,15 +1,22 @@ cmake_minimum_required(VERSION 3.14) project(autoware_motion_utils) +option(BUILD_EXAMPLES "Build examples" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) +find_package(fmt REQUIRED) ament_auto_add_library(autoware_motion_utils SHARED DIRECTORY src ) +target_link_libraries(autoware_motion_utils + fmt::fmt +) + if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) @@ -22,4 +29,27 @@ if(BUILD_TESTING) ) endif() +if(BUILD_EXAMPLES) + message(STATUS "Building examples") + + include(FetchContent) + fetchcontent_declare( + matplotlibcpp17 + GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git + GIT_TAG master + ) + fetchcontent_makeavailable(matplotlibcpp17) + + file(GLOB_RECURSE example_files examples/*.cpp) + + foreach(example_file ${example_files}) + get_filename_component(example_name ${example_file} NAME_WE) + ament_auto_add_executable(${example_name} ${example_file}) + target_link_libraries(${example_name} + autoware_motion_utils + matplotlibcpp17::matplotlibcpp17 + ) + endforeach() +endif() + ament_auto_package() diff --git a/common/autoware_motion_utils/examples/example_interpolator.cpp b/common/autoware_motion_utils/examples/example_interpolator.cpp new file mode 100644 index 0000000000000..8c98143af7e28 --- /dev/null +++ b/common/autoware_motion_utils/examples/example_interpolator.cpp @@ -0,0 +1,115 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" + +#include + +#include + +#include +#include + +int main() +{ + pybind11::scoped_interpreter guard{}; + auto plt = matplotlibcpp17::pyplot::import(); + + // create random values + std::vector axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; + std::vector values; + std::random_device seed_gen; + std::mt19937 engine(seed_gen()); + std::uniform_real_distribution<> dist(-1.0, 1.0); + for (size_t i = 0; i < axis.size(); ++i) { + values.push_back(dist(engine)); + } + // Scatter Data + plt.scatter(Args(axis, values)); + + using autoware::motion_utils::trajectory_container::interpolator::Interpolator; + using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator; + // Linear Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::Linear; + auto interpolator = *InterpolatorCreator().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "Linear")); + } + + // AkimaSpline Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline; + auto interpolator = + *InterpolatorCreator().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "AkimaSpline")); + } + + // CubicSpline Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; + auto interpolator = + *InterpolatorCreator().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "CubicSpline")); + } + + // NearestNeighbor Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor; + auto interpolator = + *InterpolatorCreator>().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "NearestNeighbor")); + } + + // Stairstep Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::Stairstep; + auto interpolator = + *InterpolatorCreator>().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "Stairstep")); + } + + plt.legend(); + plt.show(); + return 0; +} diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp new file mode 100644 index 0000000000000..a6542d1fc212f --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp @@ -0,0 +1,23 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ +#include +#include +#include +#include +#include +#include +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp new file mode 100644 index 0000000000000..42cd1aa6759a3 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp @@ -0,0 +1,98 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Class for Akima spline interpolation. + * + * This class provides methods to perform Akima spline interpolation on a set of data points. + */ +class AkimaSpline : public Interpolator +{ + template + friend class InterpolatorCreator; + +private: + Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the Akima spline. + + AkimaSpline() = default; + + /** + * @brief Compute the spline parameters. + * + * This method computes the coefficients for the Akima spline. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + void compute_parameters( + const Eigen::Ref & axis, + const Eigen::Ref & values); + + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + void build_impl( + const Eigen::Ref & axis, const std::vector & values) override; + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] double compute_impl(const double & s) const override; + + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double & s) const override; + +public: + /** + * @brief Get the minimum number of required points for the interpolator. + * + * @return The minimum number of required points. + */ + [[nodiscard]] size_t minimum_required_points() const override { return 5; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp new file mode 100644 index 0000000000000..43f1102fd0f73 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp @@ -0,0 +1,100 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Class for cubic spline interpolation. + * + * This class provides methods to perform cubic spline interpolation on a set of data points. + */ +class CubicSpline : public Interpolator +{ + template + friend class InterpolatorCreator; + +private: + Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the cubic spline. + Eigen::VectorXd h_; ///< Interval sizes between axis points. + + CubicSpline() = default; + + /** + * @brief Compute the spline parameters. + * + * This method computes the coefficients for the cubic spline. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + void compute_parameters( + const Eigen::Ref & axis, + const Eigen::Ref & values); + + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis values. + * @param values The values to interpolate. + * @return True if the interpolator was built successfully, false otherwise. + */ + void build_impl( + const Eigen::Ref & axis, const std::vector & values) override; + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] double compute_impl(const double & s) const override; + + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double & s) const override; + +public: + /** + * @brief Get the minimum number of required points for the interpolator. + * + * @return The minimum number of required points. + */ + [[nodiscard]] size_t minimum_required_points() const override { return 4; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp new file mode 100644 index 0000000000000..f7de456b736a4 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp @@ -0,0 +1,145 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +// clang-format off +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT +// clang-format on + +#include +#include + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ +/** + * @brief Base class for interpolation implementations. + * + * This class provides the basic interface and common functionality for different types + * of interpolation. It is intended to be subclassed by specific interpolation algorithms. + * + * @tparam T The type of the values being interpolated. + */ +template +class InterpolatorCommonImpl +{ +protected: + Eigen::VectorXd axis_; ///< Axis values for the interpolation. + + /** + * @brief Get the start of the interpolation range. + */ + [[nodiscard]] double start() const { return axis_(0); } + + /** + * @brief Get the end of the interpolation range. + */ + [[nodiscard]] double end() const { return axis_(axis_.size() - 1); } + + /** + * @brief Compute the interpolated value at the given point. + * + * This method should be overridden by subclasses to provide the specific interpolation logic. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] virtual T compute_impl(const double & s) const = 0; + + /** + * @brief Build the interpolator with the given values. + * + * This method should be overridden by subclasses to provide the specific build logic. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + virtual void build_impl( + const Eigen::Ref & axis, const std::vector & values) = 0; + + /** + * @brief Validate the input to the compute method. + * + * Checks that the interpolator has been built and that the input value is within range. + * + * @param s The input value. + * @throw std::runtime_error if the interpolator has not been built. + */ + void validate_compute_input(const double & s) const + { + if (s < start() || s > end()) { + RCLCPP_WARN( + rclcpp::get_logger("Interpolator"), + "Input value %f is outside the range of the interpolator [%f, %f].", s, start(), end()); + } + } + + [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const + { + if (end_inclusive && s == end()) { + return static_cast(axis_.size()) - 2; + } + auto comp = [](const double & a, const double & b) { return a <= b; }; + return std::distance(axis_.begin(), std::lower_bound(axis_.begin(), axis_.end(), s, comp)) - 1; + } + +public: + /** + * @brief Build the interpolator with the given axis and values. + * + * @param axis The axis values. + * @param values The values to interpolate. + * @return True if the interpolator was built successfully, false otherwise. + */ + bool build(const Eigen::Ref & axis, const std::vector & values) + { + if (axis.size() != static_cast(values.size())) { + return false; + } + if (axis.size() < static_cast(minimum_required_points())) { + return false; + } + build_impl(axis, values); + return true; + } + + /** + * @brief Get the minimum number of required points for the interpolator. + * + * This method should be overridden by subclasses to return the specific requirement. + * + * @return The minimum number of required points. + */ + [[nodiscard]] virtual size_t minimum_required_points() const = 0; + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] T compute(const double & s) const + { + validate_compute_input(s); + return compute_impl(s); + } +}; +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT +// clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp new file mode 100644 index 0000000000000..9fd451bcdcf6e --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +// clang-format off +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT +// clang-format on + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ +/** + * @brief Common Implementation of nearest neighbor. + * + * This class implements the core functionality for nearest neighbor interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class NearestNeighborCommonImpl : public Interpolator +{ +protected: + std::vector values_; ///< Interpolation values. + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] T compute_impl(const double & s) const override + { + const int32_t idx = this->get_index(s); + return (std::abs(s - this->axis_[idx]) <= std::abs(s - this->axis_[idx + 1])) + ? this->values_.at(idx) + : this->values_.at(idx + 1); + } + + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis values. + * @param values The values to interpolate. + * @return True if the interpolator was built successfully, false otherwise. + */ + void build_impl( + const Eigen::Ref & axis, const std::vector & values) override + { + this->axis_ = axis; + this->values_ = values; + } + +public: + /** + * @brief Get the minimum number of required points for the interpolator. + * + * @return The minimum number of required points. + */ + [[nodiscard]] size_t minimum_required_points() const override { return 1; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT +// clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp new file mode 100644 index 0000000000000..9ac5282429353 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +// clang-format off +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT +// clang-format on + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ + +/** + * @brief Base class for stairstep interpolation. + * + * This class implements the core functionality for stairstep interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class StairstepCommonImpl : public Interpolator +{ +protected: + std::vector values_; ///< Interpolation values. + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] T compute_impl(const double & s) const override + { + const int32_t idx = this->get_index(s, false); + return this->values_.at(idx); + } + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + void build_impl( + const Eigen::Ref & axis, const std::vector & values) override + { + this->axis_ = axis; + this->values_ = values; + } + +public: + /** + * @brief Default constructor. + */ + StairstepCommonImpl() = default; + + /** + * @brief Get the minimum number of required points for the interpolator. + */ + [[nodiscard]] size_t minimum_required_points() const override { return 2; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT +// clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp new file mode 100644 index 0000000000000..18dce8b46c2c7 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp @@ -0,0 +1,91 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp" + +namespace autoware::motion_utils::trajectory_container::interpolator +{ +/** + * @brief Template class for interpolation. + * + * This class serves as the base class for specific interpolation types. + * + * @tparam T The type of the values being interpolated. (e.g. double, int, etc.) + */ +template +class Interpolator : public detail::InterpolatorCommonImpl +{ +}; + +/** + * @brief Specialization of Interpolator for double values. + * + * This class adds methods for computing first and second derivatives. + */ +template <> +class Interpolator : public detail::InterpolatorCommonImpl +{ +protected: + /** + * @brief Compute the first derivative at the given point. + * + * This method should be overridden by subclasses to provide the specific logic. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] virtual double compute_first_derivative_impl(const double & s) const = 0; + + /** + * @brief Compute the second derivative at the given point. + * + * This method should be overridden by subclasses to provide the specific logic. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] virtual double compute_second_derivative_impl(const double & s) const = 0; + +public: + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative(const double & s) const + { + this->validate_compute_input(s); + return compute_first_derivative_impl(s); + } + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative(const double & s) const + { + this->validate_compute_input(s); + return compute_second_derivative_impl(s); + } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp new file mode 100644 index 0000000000000..911adcb6545b6 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ + +#include + +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +// Forward declaration +template +class Interpolator; + +template +class InterpolatorCreator +{ +private: + Eigen::VectorXd axis_; + std::vector values_; + +public: + [[nodiscard]] InterpolatorCreator & set_axis(const Eigen::Ref & axis) + { + axis_ = axis; + return *this; + } + + [[nodiscard]] InterpolatorCreator & set_axis(const std::vector & axis) + { + axis_ = Eigen::Map(axis.data(), static_cast(axis.size())); + return *this; + } + + [[nodiscard]] InterpolatorCreator & set_values(const std::vector & values) + { + values_ = values; + return *this; + } + + [[nodiscard]] InterpolatorCreator & set_values(const Eigen::Ref & values) + { + values_ = std::vector(values.begin(), values.end()); + return *this; + } + + template + [[nodiscard]] std::optional create(Args &&... args) + { + auto interpolator = InterpolatorType(std::forward(args)...); + bool success = interpolator.build(axis_, values_); + if (!success) { + return std::nullopt; + } + return interpolator; + } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp new file mode 100644 index 0000000000000..df0806f285339 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp @@ -0,0 +1,90 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Class for linear interpolation. + * + * This class provides methods to perform linear interpolation on a set of data points. + */ +class Linear : public Interpolator +{ + template + friend class InterpolatorCreator; + +private: + Eigen::VectorXd values_; ///< Interpolation values. + + /** + * @brief Default constructor. + */ + Linear() = default; + + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis values. + * @param values The values to interpolate. + * @return True if the interpolator was built successfully, false otherwise. + */ + void build_impl( + const Eigen::Ref & axis, const std::vector & values) override; + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] double compute_impl(const double & s) const override; + + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double &) const override; + +public: + /** + * @brief Get the minimum number of required points for the interpolator. + * + * @return The minimum number of required points. + */ + [[nodiscard]] size_t minimum_required_points() const override; +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp new file mode 100644 index 0000000000000..c433607902b38 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp @@ -0,0 +1,83 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp" + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Template class for nearest neighbor interpolation. + * + * This class provides methods to perform nearest neighbor interpolation on a set of data points. + * + * @tparam T The type of the values being interpolated. + */ +template +class NearestNeighbor; + +/** + * @brief Template class for nearest neighbor interpolation. + * + * This class provides the interface for nearest neighbor interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class NearestNeighbor : public detail::NearestNeighborCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + NearestNeighbor() = default; +}; + +/** + * @brief Specialization of NearestNeighbor for double values. + * + * This class provides methods to perform nearest neighbor interpolation on double values. + */ +template <> +class NearestNeighbor : public detail::NearestNeighborCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + NearestNeighbor() = default; + + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double &) const override { return 0.0; } + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp new file mode 100644 index 0000000000000..e8c8f8c015b63 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp @@ -0,0 +1,82 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp" + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Template class for stairstep interpolation. + * + * This class provides methods to perform stairstep interpolation on a set of data points. + * + * @tparam T The type of the values being interpolated. + */ +template +class Stairstep; + +/** + * @brief Template class for stairstep interpolation. + * + * This class provides the interface for stairstep interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class Stairstep : public detail::StairstepCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + Stairstep() = default; +}; + +/** + * @brief Specialization of Stairstep for double values. + * + * This class provides methods to perform stairstep interpolation on double values. + */ +template <> +class Stairstep : public detail::StairstepCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + Stairstep() = default; + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double &) const override { return 0.0; } + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp new file mode 100644 index 0000000000000..283f46dd5d3e9 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp @@ -0,0 +1,93 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 "autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp" + +#include + +#include +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +void AkimaSpline::compute_parameters( + const Eigen::Ref & axis, const Eigen::Ref & values) +{ + const auto n = static_cast(axis.size()); + + Eigen::VectorXd h = axis.tail(n - 1) - axis.head(n - 1); + + Eigen::VectorXd m(n - 1); + for (int32_t i = 0; i < n - 1; ++i) { + m[i] = (values[i + 1] - values[i]) / h[i]; + } + + Eigen::VectorXd s(n); + s[0] = m[0]; + s[1] = (m[0] + m[1]) / 2; + for (int32_t i = 2; i < n - 2; ++i) { + if ((std::abs(m[i + 1] - m[i]) + std::abs(m[i - 1] - m[i - 2])) == 0) { + s[i] = (m[i] + m[i - 1]) / 2; + } else { + s[i] = (std::abs(m[i + 1] - m[i]) * m[i - 1] + std::abs(m[i - 1] - m[i - 2]) * m[i]) / + (std::abs(m[i + 1] - m[i]) + std::abs(m[i - 1] - m[i - 2])); + } + } + s[n - 2] = (m[n - 2] + m[n - 3]) / 2; + s[n - 1] = m[n - 2]; + + a_.resize(n - 1); + b_.resize(n - 1); + c_.resize(n - 1); + d_.resize(n - 1); + for (int32_t i = 0; i < n - 1; ++i) { + a_[i] = values[i]; + b_[i] = s[i]; + c_[i] = (3 * m[i] - 2 * s[i] - s[i + 1]) / h[i]; + d_[i] = (s[i] + s[i + 1] - 2 * m[i]) / (h[i] * h[i]); + } +} + +void AkimaSpline::build_impl( + const Eigen::Ref & axis, const std::vector & values) +{ + this->axis_ = axis; + compute_parameters( + this->axis_, + Eigen::Map(values.data(), static_cast(values.size()))); +} + +double AkimaSpline::compute_impl(const double & s) const +{ + const int32_t i = this->get_index(s); + const double dx = s - this->axis_[i]; + return a_[i] + b_[i] * dx + c_[i] * dx * dx + d_[i] * dx * dx * dx; +} + +double AkimaSpline::compute_first_derivative_impl(const double & s) const +{ + const int32_t i = this->get_index(s); + const double dx = s - this->axis_[i]; + return b_[i] + 2 * c_[i] * dx + 3 * d_[i] * dx * dx; +} + +double AkimaSpline::compute_second_derivative_impl(const double & s) const +{ + const int32_t i = this->get_index(s); + const double dx = s - this->axis_[i]; + return 2 * c_[i] + 6 * d_[i] * dx; +} + +} // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp new file mode 100644 index 0000000000000..9f262b6a702f9 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp @@ -0,0 +1,94 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +void CubicSpline::compute_parameters( + const Eigen::Ref & axis, const Eigen::Ref & values) +{ + const int32_t n = static_cast(axis.size()) - 1; + + h_ = axis.tail(n) - axis.head(n); + a_ = values.transpose(); + + for (int32_t i = 0; i < n; ++i) { + h_(i) = axis(i + 1) - axis(i); + } + + Eigen::VectorXd alpha(n - 1); + for (int32_t i = 1; i < n; ++i) { + alpha(i - 1) = (3.0 / h_(i)) * (a_(i + 1) - a_(i)) - (3.0 / h_(i - 1)) * (a_(i) - a_(i - 1)); + } + + Eigen::VectorXd l(n + 1); + Eigen::VectorXd mu(n + 1); + Eigen::VectorXd z(n + 1); + l(0) = 1.0; + mu(0) = z(0) = 0.0; + + for (int32_t i = 1; i < n; ++i) { + l(i) = 2.0 * (axis(i + 1) - axis(i - 1)) - h_(i - 1) * mu(i - 1); + mu(i) = h_(i) / l(i); + z(i) = (alpha(i - 1) - h_(i - 1) * z(i - 1)) / l(i); + } + b_.resize(n); + d_.resize(n); + c_.resize(n + 1); + + l(n) = 1.0; + z(n) = c_(n) = 0.0; + + for (int32_t j = n - 1; j >= 0; --j) { + c_(j) = z(j) - mu(j) * c_(j + 1); + b_(j) = (a_(j + 1) - a_(j)) / h_(j) - h_(j) * (c_(j + 1) + 2.0 * c_(j)) / 3.0; + d_(j) = (c_(j + 1) - c_(j)) / (3.0 * h_(j)); + } +} + +void CubicSpline::build_impl( + const Eigen::Ref & axis, const std::vector & values) +{ + this->axis_ = axis; + compute_parameters( + this->axis_, + Eigen::Map(values.data(), static_cast(values.size()))); +} + +double CubicSpline::compute_impl(const double & s) const +{ + const int32_t i = this->get_index(s); + const double dx = s - this->axis_(i); + return a_(i) + b_(i) * dx + c_(i) * dx * dx + d_(i) * dx * dx * dx; +} + +double CubicSpline::compute_first_derivative_impl(const double & s) const +{ + const int32_t i = this->get_index(s); + const double dx = s - this->axis_(i); + return b_(i) + 2 * c_(i) * dx + 3 * d_(i) * dx * dx; +} + +double CubicSpline::compute_second_derivative_impl(const double & s) const +{ + const int32_t i = this->get_index(s); + const double dx = s - this->axis_(i); + return 2 * c_(i) + 6 * d_(i) * dx; +} + +} // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp new file mode 100644 index 0000000000000..4897ba4d2a56c --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp @@ -0,0 +1,62 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +void Linear::build_impl( + const Eigen::Ref & axis, const std::vector & values) +{ + this->axis_ = axis; + this->values_ = + Eigen::Map(values.data(), static_cast(values.size())); +} + +double Linear::compute_impl(const double & s) const +{ + const int32_t idx = this->get_index(s); + const double x0 = this->axis_(idx); + const double x1 = this->axis_(idx + 1); + const double y0 = this->values_(idx); + const double y1 = this->values_(idx + 1); + return y0 + (y1 - y0) * (s - x0) / (x1 - x0); +} + +double Linear::compute_first_derivative_impl(const double & s) const +{ + const int32_t idx = this->get_index(s); + const double x0 = this->axis_(idx); + const double x1 = this->axis_(idx + 1); + const double y0 = this->values_(idx); + const double y1 = this->values_(idx + 1); + return (y1 - y0) / (x1 - x0); +} + +double Linear::compute_second_derivative_impl(const double &) const +{ + return 0.0; +} + +size_t Linear::minimum_required_points() const +{ + return 2; +} + +} // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp new file mode 100644 index 0000000000000..77f7af0eae93f --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp @@ -0,0 +1,75 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 + +#include +#include + +#include +#include +#include + +template +class TestInterpolator : public ::testing::Test +{ +public: + std::optional interpolator; + std::vector axis; + std::vector values; + + void SetUp() override + { + // generate random values -1 to 1 + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-1, 1); + axis.resize(10); + values.resize(10); + for (size_t i = 0; i < axis.size(); ++i) { + axis[i] = static_cast(i); + values[i] = dis(gen); + } + } +}; + +using Interpolators = testing::Types< + autoware::motion_utils::trajectory_container::interpolator::CubicSpline, + autoware::motion_utils::trajectory_container::interpolator::AkimaSpline, + autoware::motion_utils::trajectory_container::interpolator::Linear, + autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor, + autoware::motion_utils::trajectory_container::interpolator::Stairstep>; + +TYPED_TEST_SUITE(TestInterpolator, Interpolators, ); + +TYPED_TEST(TestInterpolator, compute) +{ + using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator; + this->interpolator = + InterpolatorCreator().set_axis(this->axis).set_values(this->values).create(); + for (size_t i = 0; i < this->axis.size(); ++i) { + EXPECT_NEAR(this->values[i], this->interpolator->compute(this->axis[i]), 1e-6); + } +} + +// Instantiate test cases for all interpolators +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::CubicSpline>; +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::AkimaSpline>; +template class TestInterpolator; +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor>; +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::Stairstep>; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index cd97a8301ad9d..dc8c876bd56f6 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -348,21 +348,30 @@ bool AEB::fetchLatestData() return missing("object detection method (pointcloud or predicted objects)"); } - const auto imu_ptr = sub_imu_.takeData(); - if (use_imu_path_) { + const bool has_imu_path = std::invoke([&]() { + if (!use_imu_path_) return false; + const auto imu_ptr = sub_imu_.takeData(); if (!imu_ptr) { return missing("imu message"); } // imu_ptr is valid onImu(imu_ptr); - } - if (use_imu_path_ && !angular_velocity_ptr_) { - return missing("imu"); - } + return (!angular_velocity_ptr_) ? missing("imu") : true; + }); - predicted_traj_ptr_ = sub_predicted_traj_.takeData(); - if (use_predicted_trajectory_ && !predicted_traj_ptr_) { - return missing("control predicted trajectory"); + const bool has_predicted_path = std::invoke([&]() { + if (!use_predicted_trajectory_) { + return false; + } + predicted_traj_ptr_ = sub_predicted_traj_.takeData(); + return (!predicted_traj_ptr_) ? missing("control predicted trajectory") : true; + }); + + if (!has_imu_path && !has_predicted_path) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, + "[AEB] At least one path (IMU or predicted trajectory) is required for operation"); + return false; } autoware_state_ = sub_autoware_state_.takeData(); @@ -482,7 +491,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // step3. make function to check collision with ego path created with sensor data const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { - if (!use_imu_path_) return false; + if (!use_imu_path_ || !angular_velocity_ptr_) return false; const double current_w = angular_velocity_ptr_->z; constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; const std::string ns = "ego"; diff --git a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 74affea696893..526c4bb4b3d52 100644 --- a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -4,7 +4,6 @@ system_emergency_heartbeat_timeout: 0.5 use_emergency_handling: false check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat) - use_start_request: false enable_cmd_limit_filter: true filter_activated_count_threshold: 5 filter_activated_velocity_threshold: 1.0 @@ -12,7 +11,6 @@ stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 moderate_stop_service_acceleration: -1.5 - stopped_state_entry_duration_time: 0.1 stop_check_duration: 1.0 nominal: vel_lim: 25.0 diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp index 87f331c5ff2af..fb939d825b653 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp @@ -79,10 +79,6 @@ inline geometry_msgs::msg::Quaternion createOrientationMsgFromYaw(double yaw_ang // p-points a, b contains [x, y] coordinates. double determinant(std::array const & a, std::array const & b); -double triangleArea( - std::array const & a, std::array const & b, - std::array const & c); - double curvatureFromThreePoints( std::array const & a, std::array const & b, std::array const & c); diff --git a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp index 467d26bbb69a0..f96aba2b54547 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp @@ -25,15 +25,5 @@ double determinant(std::array const & a, std::array const return a[0] * b[1] - b[0] * a[1]; } -double triangleArea( - std::array const & a, std::array const & b, std::array const & c) -{ - double m1 = determinant(a, b); - double m2 = determinant(b, c); - double m3 = determinant(c, a); - - return 0.5 * (m1 + m2 + m3); -} - } // namespace utils } // namespace control_performance_analysis diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index cfc09459a5cb1..dd274ce247064 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -18,6 +18,11 @@ + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/gyro_odometer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/gyro_odometer.launch.xml index 3627c6698f4bd..e78101b0a1809 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/gyro_odometer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/gyro_odometer.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml new file mode 100644 index 0000000000000..301e961a94012 --- /dev/null +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index fa6bce0e38e55..00f2406aa82d6 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -9,7 +9,7 @@ - + @@ -18,6 +18,7 @@ + @@ -91,9 +92,19 @@ + + + + + + + + + + - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index fa479c71dcbac..dda9c1f2c97ce 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -35,10 +35,10 @@ - + - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 2f02be72ec5ad..70e09bb9072d8 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -20,16 +20,17 @@ automatic_pose_initializer autoware_ar_tag_based_localizer autoware_geo_pose_projector + autoware_gyro_odometer + autoware_lidar_marker_localizer autoware_pointcloud_preprocessor + autoware_pose_estimator_arbiter + autoware_pose_instability_detector eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt ekf_localizer - gyro_odometer ndt_scan_matcher - pose_estimator_arbiter pose_initializer - pose_instability_detector topic_tools yabloc_common yabloc_image_processing diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index c58c0848eb5b3..051cf64d7752e 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -48,7 +48,7 @@ - + diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index bca5a853d69f7..b364d8eaffebc 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -18,9 +18,9 @@ ament_cmake_auto autoware_cmake + autoware_map_tf_generator map_loader map_projection_loader - map_tf_generator ament_lint_auto autoware_lint_common diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/autoware_gyro_odometer/CMakeLists.txt similarity index 88% rename from localization/gyro_odometer/CMakeLists.txt rename to localization/autoware_gyro_odometer/CMakeLists.txt index a832383ff4761..b4d092531e6af 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/autoware_gyro_odometer/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(gyro_odometer) +project(autoware_gyro_odometer) find_package(autoware_cmake REQUIRED) autoware_package() @@ -22,6 +22,9 @@ if(BUILD_TESTING) target_link_libraries(test_gyro_odometer ${PROJECT_NAME} ) + target_include_directories(test_gyro_odometer PRIVATE + src + ) endif() rclcpp_components_register_node(${PROJECT_NAME} diff --git a/localization/gyro_odometer/README.md b/localization/autoware_gyro_odometer/README.md similarity index 94% rename from localization/gyro_odometer/README.md rename to localization/autoware_gyro_odometer/README.md index aa6f2a96f4838..d2ff600ae4bf0 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/autoware_gyro_odometer/README.md @@ -1,8 +1,8 @@ -# gyro_odometer +# autoware_gyro_odometer ## Purpose -`gyro_odometer` is the package to estimate twist by combining imu and vehicle speed. +`autoware_gyro_odometer` is the package to estimate twist by combining imu and vehicle speed. ## Inputs / Outputs @@ -21,7 +21,7 @@ ## Parameters -{{ json_to_markdown("localization/gyro_odometer/schema/gyro_odometer.schema.json") }} +{{ json_to_markdown("localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json") }} ## Assumptions / Known limits diff --git a/localization/gyro_odometer/config/gyro_odometer.param.yaml b/localization/autoware_gyro_odometer/config/gyro_odometer.param.yaml similarity index 100% rename from localization/gyro_odometer/config/gyro_odometer.param.yaml rename to localization/autoware_gyro_odometer/config/gyro_odometer.param.yaml diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/autoware_gyro_odometer/launch/gyro_odometer.launch.xml similarity index 87% rename from localization/gyro_odometer/launch/gyro_odometer.launch.xml rename to localization/autoware_gyro_odometer/launch/gyro_odometer.launch.xml index aed6050858fe1..b9f30a4389df7 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/autoware_gyro_odometer/launch/gyro_odometer.launch.xml @@ -9,9 +9,9 @@ - + - + diff --git a/localization/gyro_odometer/media/diagnostic.png b/localization/autoware_gyro_odometer/media/diagnostic.png similarity index 100% rename from localization/gyro_odometer/media/diagnostic.png rename to localization/autoware_gyro_odometer/media/diagnostic.png diff --git a/localization/gyro_odometer/package.xml b/localization/autoware_gyro_odometer/package.xml similarity index 92% rename from localization/gyro_odometer/package.xml rename to localization/autoware_gyro_odometer/package.xml index ba08a8852eca5..0e78d0dea51b6 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/autoware_gyro_odometer/package.xml @@ -1,9 +1,9 @@ - gyro_odometer + autoware_gyro_odometer 0.1.0 - The gyro_odometer package as a ROS 2 node + The autoware_gyro_odometer package as a ROS 2 node Yamato Ando Masahiro Sakamoto Kento Yabuuchi diff --git a/localization/gyro_odometer/schema/gyro_odometer.schema.json b/localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json similarity index 100% rename from localization/gyro_odometer/schema/gyro_odometer.schema.json rename to localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp similarity index 99% rename from localization/gyro_odometer/src/gyro_odometer_core.cpp rename to localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index c661f63d91535..bc2abb4a8a321 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gyro_odometer/gyro_odometer_core.hpp" +#include "gyro_odometer_core.hpp" #include @@ -23,6 +23,7 @@ #endif #include +#include #include #include #include diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp similarity index 95% rename from localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp rename to localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index f569ed25a0c7f..1b2c4246a037a 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ -#define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#ifndef GYRO_ODOMETER_CORE_HPP_ +#define GYRO_ODOMETER_CORE_HPP_ #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" @@ -85,4 +85,4 @@ class GyroOdometerNode : public rclcpp::Node } // namespace autoware::gyro_odometer -#endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#endif // GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp b/localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.cpp similarity index 100% rename from localization/gyro_odometer/test/test_gyro_odometer_helper.cpp rename to localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.cpp diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp b/localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.hpp similarity index 100% rename from localization/gyro_odometer/test/test_gyro_odometer_helper.hpp rename to localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.hpp diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/autoware_gyro_odometer/test/test_gyro_odometer_pubsub.cpp similarity index 99% rename from localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp rename to localization/autoware_gyro_odometer/test/test_gyro_odometer_pubsub.cpp index fc331a638a1dd..21211072054d4 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/autoware_gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gyro_odometer/gyro_odometer_core.hpp" +#include "gyro_odometer_core.hpp" #include "test_gyro_odometer_helper.hpp" #include diff --git a/localization/gyro_odometer/test/test_main.cpp b/localization/autoware_gyro_odometer/test/test_main.cpp similarity index 100% rename from localization/gyro_odometer/test/test_main.cpp rename to localization/autoware_gyro_odometer/test/test_main.cpp diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt new file mode 100755 index 0000000000000..d352cb2f0fa96 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_lidar_marker_localizer) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED COMPONENTS common io) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/lidar_marker_localizer.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::lidar_marker_localizer::LidarMarkerLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/README.md b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/README.md new file mode 100644 index 0000000000000..15afefaf20ee4 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/README.md @@ -0,0 +1,114 @@ +# LiDAR Marker Localizer + +**LiDARMarkerLocalizer** is a detect-reflector-based localization node . + +## Inputs / Outputs + +### `lidar_marker_localizer` node + +#### Input + +| Name | Type | Description | +| :--------------------- | :---------------------------------------------- | :--------------- | +| `~/input/lanelet2_map` | `autoware_map_msgs::msg::HADMapBin` | Data of lanelet2 | +| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2` | PointCloud | +| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose | + +#### Output + +| Name | Type | Description | +| :------------------------------ | :---------------------------------------------- | :----------------------------------------------------------------- | +| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose | +| `~/debug/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] Estimated pose | +| `~/debug/marker_detected` | `geometry_msgs::msg::PoseArray` | [debug topic] Detected marker poses | +| `~/debug/marker_mapped` | `visualization_msgs::msg::MarkerArray` | [debug topic] Loaded landmarks to visualize in Rviz as thin boards | +| `~/debug/marker_pointcloud` | `sensor_msgs::msg::PointCloud2` | [debug topic] PointCloud of the detected marker | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | + +## Parameters + +{{ json_to_markdown("localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json") }} + +## How to launch + +When launching Autoware, set `lidar-marker` for `pose_source`. + +```bash +ros2 launch autoware_launch ... \ + pose_source:=lidar-marker \ + ... +``` + +## Design + +### Flowchart + +```plantuml +@startuml + +group main process + start + if (Receive a map?) then (yes) + else (no) + stop + endif + + :Interpolate based on the received ego-vehicle's positions to align with sensor time; + + if (Could interpolate?) then (yes) + else (no) + stop + endif + + :Detect markers (see "Detection Algorithm"); + + :Calculate the distance from the ego-vehicle's positions to the nearest marker's position on the lanelet2 map; + + if (Find markers?) then (yes) + else (no) + if (the distance is nearby?) then (yes) + stop + note : Error. It should have been able to detect marker + else (no) + stop + note : Not Error. There are no markers around the ego-vehicle + endif + endif + + :Calculate the correction amount from the ego-vehicle's position; + + if (Is the found marker's position close to the one on the lanelet2 map?) then (yes) + else (no) + stop + note : Detected something that isn't a marker + endif + + :Publish result; + + stop +end group + +@enduml + +``` + +## Detection Algorithm + +![detection_algorithm](./doc_image/detection_algorithm.png) + +1. Split the LiDAR point cloud into rings along the x-axis of the base_link coordinate system at intervals of the `resolution` size. +2. Find the portion of intensity that matches the `intensity_pattern`. +3. Perform steps 1 and 2 for each ring, accumulate the matching indices, and detect portions where the count exceeds the `vote_threshold_for_detect_marker` as markers. + +## Sample Dataset + +- [Sample rosbag and map](https://drive.google.com/file/d/1FuGKbkWrvL_iKmtb45PO9SZl1vAaJFVG/view?usp=sharing) + +This dataset was acquired in National Institute for Land and Infrastructure Management, Full-scale tunnel experiment facility. +The reflectors were installed by [Taisei Corporation](https://www.taisei.co.jp/english/). + +## Collaborators + +- [TIER IV](https://tier4.jp/en/) +- [Taisei Corporation](https://www.taisei.co.jp/english/) +- [Yuri Shimizu](https://github.com/YuriShimizu824) diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/config/lidar_marker_localizer.param.yaml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/config/lidar_marker_localizer.param.yaml new file mode 100644 index 0000000000000..1c04d51e5df0c --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/config/lidar_marker_localizer.param.yaml @@ -0,0 +1,42 @@ +/**: + ros__parameters: + + # marker name + marker_name: "reflector" + + # for marker detection algorithm + resolution: 0.05 + # A sequence of high/low intensity to perform pattern matching. + # 1: high intensity (positive match), 0: not consider, -1: low intensity (negative match) + intensity_pattern: [-1, -1, 0, 1, 1, 1, 1, 1, 0, -1, -1] + match_intensity_difference_threshold: 20 + positive_match_num_threshold: 3 + negative_match_num_threshold: 3 + vote_threshold_for_detect_marker: 20 + marker_height_from_ground: 1.075 + + # for interpolate algorithm + self_pose_timeout_sec: 1.0 + self_pose_distance_tolerance_m: 1.0 + + # for validation + limit_distance_from_self_pose_to_nearest_marker: 2.0 + limit_distance_from_self_pose_to_marker: 2.0 + + # base_covariance + # [TBD] This value is dynamically scaled according to the distance at which markers are detected. + base_covariance: [0.04, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.00007569, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.00007569, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.00030625] + + # for visualize the detected marker pointcloud + marker_width: 0.8 + + # for save log + enable_save_log: false + save_file_directory_path: detected_reflector_intensity + save_file_name: detected_reflector_intensity + save_frame_id: velodyne_top diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/doc_image/detection_algorithm.png b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/doc_image/detection_algorithm.png new file mode 100644 index 0000000000000..905b432053078 Binary files /dev/null and b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/doc_image/detection_algorithm.png differ diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml new file mode 100644 index 0000000000000..78c5958df4775 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml new file mode 100755 index 0000000000000..bf60d96311ff1 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml @@ -0,0 +1,40 @@ + + + + autoware_lidar_marker_localizer + 0.0.0 + The autoware_lidar_marker_localizer package + Yamato Ando + Shintaro Sakoda + Apache License 2.0 + Eijiro Takeuchi + Yamato Ando + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + autoware_landmark_manager + autoware_map_msgs + autoware_point_types + autoware_universe_utils + localization_util + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + std_srvs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json new file mode 100644 index 0000000000000..31a1a8add8cab --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json @@ -0,0 +1,149 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Lidar Marker Localizer Node", + "type": "object", + "definitions": { + "lidar_marker_localizer": { + "type": "object", + "properties": { + "marker_name": { + "type": "string", + "description": "The name of the markers listed in the HD map.", + "default": "reflector" + }, + "resolution": { + "type": "number", + "description": "Grid size for marker detection algorithm. [m]", + "default": 0.05, + "minimum": 0.0 + }, + "intensity_pattern": { + "type": "array", + "description": "A sequence of high/low intensity to perform pattern matching. 1: high intensity (positive match), 0: not consider, -1: low intensity (negative match)", + "default": [-1, -1, 0, 1, 1, 1, 1, 1, 0, -1, -1] + }, + "match_intensity_difference_threshold": { + "type": "number", + "description": "Threshold for determining high/low.", + "default": 20, + "minimum": 0 + }, + "positive_match_num_threshold": { + "type": "number", + "description": "Threshold for the number of required matches with the pattern.", + "default": 3, + "minimum": 0 + }, + "negative_match_num_threshold": { + "type": "number", + "description": "Threshold for the number of required matches with the pattern.", + "default": 3, + "minimum": 0 + }, + "vote_threshold_for_detect_marker": { + "type": "number", + "description": "Threshold for the number of rings matching the pattern needed to detect it as a marker.", + "default": 20, + "minimum": 0 + }, + "marker_height_from_ground": { + "type": "number", + "description": "Height from the ground to the center of the marker. [m]", + "default": 1.075 + }, + "self_pose_timeout_sec": { + "type": "number", + "description": "Timeout for self pose. [sec]", + "default": 1.0, + "minimum": 0.0 + }, + "self_pose_distance_tolerance_m": { + "type": "number", + "description": "Tolerance for the distance between two points when performing linear interpolation. [m]", + "default": 1.0, + "minimum": 0.0 + }, + "limit_distance_from_self_pose_to_nearest_marker": { + "type": "number", + "description": "Distance limit for the purpose of determining whether the node should detect a marker. [m]", + "default": 2.0, + "minimum": 0.0 + }, + "limit_distance_from_self_pose_to_marker": { + "type": "number", + "description": "Distance limit for avoiding miss detection. [m]", + "default": 2.0, + "minimum": 0.0 + }, + "base_covariance": { + "type": "array", + "description": "Output covariance in the base_link coordinate.", + "default": [ + 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.00007569, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00007569, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.00030625 + ] + }, + "marker_width": { + "type": "number", + "description": "Width of a marker. This param is used for visualizing the detected marker pointcloud[m]", + "default": 0.8, + "minimum": 0.0 + }, + "enable_save_log": { + "type": "boolean", + "description": "", + "default": false + }, + "save_file_directory_path": { + "type": "string", + "description": "", + "default": "$(env HOME)/detected_reflector_intensity" + }, + "save_file_name": { + "type": "string", + "description": "", + "default": "detected_reflector_intensity" + }, + "save_frame_id": { + "type": "string", + "description": "", + "default": "velodyne_top" + } + }, + "required": [ + "resolution", + "intensity_pattern", + "match_intensity_difference_threshold", + "positive_match_num_threshold", + "negative_match_num_threshold", + "vote_threshold_for_detect_marker", + "self_pose_timeout_sec", + "self_pose_distance_tolerance_m", + "limit_distance_from_self_pose_to_nearest_marker", + "limit_distance_from_self_pose_to_marker", + "base_covariance", + "marker_width", + "enable_save_log", + "save_file_directory_path", + "save_file_name", + "save_frame_id" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lidar_marker_localizer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp new file mode 100644 index 0000000000000..83fcae2352f51 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -0,0 +1,618 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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 "lidar_marker_localizer.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::lidar_marker_localizer +{ + +landmark_manager::Landmark get_nearest_landmark( + const geometry_msgs::msg::Pose & self_pose, + const std::vector & landmarks); +std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation); + +LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_options) +: Node("lidar_marker_localizer", node_options), is_activated_(false) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + param_.marker_name = this->declare_parameter("marker_name"); + param_.resolution = this->declare_parameter("resolution"); + param_.intensity_pattern = this->declare_parameter>("intensity_pattern"); + param_.match_intensity_difference_threshold = + this->declare_parameter("match_intensity_difference_threshold"); + param_.positive_match_num_threshold = + this->declare_parameter("positive_match_num_threshold"); + param_.negative_match_num_threshold = + this->declare_parameter("negative_match_num_threshold"); + param_.vote_threshold_for_detect_marker = + this->declare_parameter("vote_threshold_for_detect_marker"); + param_.marker_height_from_ground = this->declare_parameter("marker_height_from_ground"); + param_.self_pose_timeout_sec = this->declare_parameter("self_pose_timeout_sec"); + param_.self_pose_distance_tolerance_m = + this->declare_parameter("self_pose_distance_tolerance_m"); + param_.limit_distance_from_self_pose_to_nearest_marker = + this->declare_parameter("limit_distance_from_self_pose_to_nearest_marker"); + param_.limit_distance_from_self_pose_to_marker = + this->declare_parameter("limit_distance_from_self_pose_to_marker"); + std::vector base_covariance = + this->declare_parameter>("base_covariance"); + for (std::size_t i = 0; i < base_covariance.size(); ++i) { + param_.base_covariance[i] = base_covariance[i]; + } + param_.marker_width = this->declare_parameter("marker_width"); + param_.enable_save_log = this->declare_parameter("enable_save_log"); + param_.save_file_directory_path = + this->declare_parameter("save_file_directory_path"); + param_.save_file_name = this->declare_parameter("save_file_name"); + param_.save_frame_id = this->declare_parameter("save_frame_id"); + + ekf_pose_buffer_ = std::make_unique( + this->get_logger(), param_.self_pose_timeout_sec, param_.self_pose_distance_tolerance_m); + + rclcpp::CallbackGroup::SharedPtr points_callback_group; + points_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto points_sub_opt = rclcpp::SubscriptionOptions(); + points_sub_opt.callback_group = points_callback_group; + sub_points_ = this->create_subscription( + "~/input/pointcloud", rclcpp::QoS(1).best_effort(), + std::bind(&LidarMarkerLocalizer::points_callback, this, _1), points_sub_opt); + + rclcpp::CallbackGroup::SharedPtr self_pose_callback_group; + self_pose_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto self_pose_sub_opt = rclcpp::SubscriptionOptions(); + self_pose_sub_opt.callback_group = self_pose_callback_group; + sub_self_pose_ = this->create_subscription( + "~/input/ekf_pose", rclcpp::QoS(1), + std::bind(&LidarMarkerLocalizer::self_pose_callback, this, _1), self_pose_sub_opt); + sub_map_bin_ = this->create_subscription( + "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), + std::bind(&LidarMarkerLocalizer::map_bin_callback, this, _1)); + + pub_base_link_pose_with_covariance_on_map_ = + this->create_publisher("~/output/pose_with_covariance", 10); + rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); + qos_marker.transient_local(); + qos_marker.reliable(); + pub_marker_mapped_ = this->create_publisher("~/debug/marker_mapped", qos_marker); + pub_marker_detected_ = this->create_publisher("~/debug/marker_detected", 10); + pub_debug_pose_with_covariance_ = + this->create_publisher("~/debug/pose_with_covariance", 10); + pub_marker_pointcloud_ = this->create_publisher("~/debug/marker_pointcloud", 10); + service_trigger_node_ = this->create_service( + "~/service/trigger_node_srv", + std::bind(&LidarMarkerLocalizer::service_trigger_node, this, _1, _2), + rclcpp::ServicesQoS().get_rmw_qos_profile(), points_callback_group); + + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_, this, false); + + diagnostics_module_.reset(new DiagnosticsModule(this, "marker_detection_status")); +} + +void LidarMarkerLocalizer::initialize_diagnostics() +{ + diagnostics_module_->clear(); + diagnostics_module_->add_key_value("is_received_map", false); + diagnostics_module_->add_key_value("is_received_self_pose", false); + diagnostics_module_->add_key_value("detect_marker_num", 0); + diagnostics_module_->add_key_value("distance_self_pose_to_nearest_marker", 0.0); + diagnostics_module_->add_key_value( + "limit_distance_from_self_pose_to_nearest_marker", + param_.limit_distance_from_self_pose_to_nearest_marker); + diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", 0.0); + diagnostics_module_->add_key_value( + "limit_distance_from_lanelet2_marker_to_detected_marker", + param_.limit_distance_from_self_pose_to_marker); +} + +void LidarMarkerLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & map_bin_msg_ptr) +{ + landmark_manager_.parse_landmarks(map_bin_msg_ptr, param_.marker_name); + const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); + pub_marker_mapped_->publish(marker_msg); +} + +void LidarMarkerLocalizer::self_pose_callback( + const PoseWithCovarianceStamped::ConstSharedPtr & self_pose_msg_ptr) +{ + // TODO(YamatoAndo) + // if (!is_activated_) return; + + if (self_pose_msg_ptr->header.frame_id == "map") { + ekf_pose_buffer_->push_back(self_pose_msg_ptr); + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 1000, + "Received initial pose message with frame_id " + << self_pose_msg_ptr->header.frame_id << ", but expected map. " + << "Please check the frame_id in the input topic and ensure it is correct."); + } +} + +void LidarMarkerLocalizer::points_callback(const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + const auto sensor_ros_time = points_msg_ptr->header.stamp; + + initialize_diagnostics(); + + main_process(points_msg_ptr); + + diagnostics_module_->publish(sensor_ros_time); +} + +void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + const builtin_interfaces::msg::Time sensor_ros_time = points_msg_ptr->header.stamp; + + // (1) check if the map have be received + const std::vector map_landmarks = landmark_manager_.get_landmarks(); + const bool is_received_map = !map_landmarks.empty(); + diagnostics_module_->add_key_value("is_received_map", is_received_map); + if (!is_received_map) { + std::stringstream message; + message << "Not receive the landmark information. Please check if the vector map is being " + << "published and if the landmark information is correctly specified."; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + // (2) get Self Pose + const std::optional interpolate_result = + ekf_pose_buffer_->interpolate(sensor_ros_time); + + const bool is_received_self_pose = interpolate_result != std::nullopt; + diagnostics_module_->add_key_value("is_received_self_pose", is_received_self_pose); + if (!is_received_self_pose) { + std::stringstream message; + message << "Could not get self_pose. Please check if the self pose is being published and if " + << "the timestamp of the self pose is correctly specified"; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + ekf_pose_buffer_->pop_old(sensor_ros_time); + const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose; + + // (3) detect marker + const std::vector detected_landmarks = + detect_landmarks(points_msg_ptr); + + const bool is_detected_marker = !detected_landmarks.empty(); + diagnostics_module_->add_key_value("detect_marker_num", detected_landmarks.size()); + + // (4) check distance to the nearest marker + const landmark_manager::Landmark nearest_marker = get_nearest_landmark(self_pose, map_landmarks); + const Pose nearest_marker_pose_on_base_link = + autoware::universe_utils::inverseTransformPose(nearest_marker.pose, self_pose); + + const double distance_from_self_pose_to_nearest_marker = + std::abs(nearest_marker_pose_on_base_link.position.x); + diagnostics_module_->add_key_value( + "distance_self_pose_to_nearest_marker", distance_from_self_pose_to_nearest_marker); + + const bool is_exist_marker_within_self_pose = + distance_from_self_pose_to_nearest_marker < + param_.limit_distance_from_self_pose_to_nearest_marker; + + if (!is_detected_marker) { + if (!is_exist_marker_within_self_pose) { + std::stringstream message; + message << "Could not detect marker, because the distance from self_pose to nearest_marker " + << "is too far (" << distance_from_self_pose_to_nearest_marker << " [m])."; + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::OK, message.str()); + } else { + std::stringstream message; + message << "Could not detect marker, although the distance from self_pose to nearest_marker " + << "is near (" << distance_from_self_pose_to_nearest_marker << " [m])."; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } + return; + } + + // for debug + if (pub_marker_detected_->get_subscription_count() > 0) { + PoseArray pose_array_msg; + pose_array_msg.header.stamp = sensor_ros_time; + pose_array_msg.header.frame_id = "map"; + for (const landmark_manager::Landmark & landmark : detected_landmarks) { + const Pose detected_marker_on_map = + autoware::universe_utils::transformPose(landmark.pose, self_pose); + pose_array_msg.poses.push_back(detected_marker_on_map); + } + pub_marker_detected_->publish(pose_array_msg); + } + + // (4) calculate diff pose + const Pose new_self_pose = + landmark_manager_.calculate_new_self_pose(detected_landmarks, self_pose, false); + + const double diff_x = new_self_pose.position.x - self_pose.position.x; + const double diff_y = new_self_pose.position.y - self_pose.position.y; + + const double diff_norm = std::hypot(diff_x, diff_y); + const bool is_exist_marker_within_lanelet2_map = + diff_norm < param_.limit_distance_from_self_pose_to_marker; + + diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", diff_norm); + if (!is_exist_marker_within_lanelet2_map) { + std::stringstream message; + message << "The distance from lanelet2 to the detect marker is too far(" << diff_norm + << " [m]). The limit is " << param_.limit_distance_from_self_pose_to_marker << "."; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + // (5) Apply diff pose to self pose + // only x and y is changed + PoseWithCovarianceStamped result; + result.header.stamp = sensor_ros_time; + result.header.frame_id = "map"; + result.pose.pose.position.x = new_self_pose.position.x; + result.pose.pose.position.y = new_self_pose.position.y; + result.pose.pose.position.z = self_pose.position.z; + result.pose.pose.orientation = self_pose.orientation; + + // set covariance + const Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond( + result.pose.pose.orientation.w, result.pose.pose.orientation.x, result.pose.pose.orientation.y, + result.pose.pose.orientation.z); + const Eigen::Matrix3d map_to_base_link_rotation = + map_to_base_link_quat.normalized().toRotationMatrix(); + result.pose.covariance = rotate_covariance(param_.base_covariance, map_to_base_link_rotation); + + pub_base_link_pose_with_covariance_on_map_->publish(result); + pub_debug_pose_with_covariance_->publish(result); + + // for debug + const landmark_manager::Landmark nearest_detected_landmark = + get_nearest_landmark(self_pose, detected_landmarks); + const auto marker_pointcloud_msg_ptr = + extract_marker_pointcloud(points_msg_ptr, nearest_detected_landmark.pose); + pub_marker_pointcloud_->publish(*marker_pointcloud_msg_ptr); + + // save log + if (param_.enable_save_log) { + save_detected_marker_log(marker_pointcloud_msg_ptr); + } +} + +void LidarMarkerLocalizer::service_trigger_node( + const SetBool::Request::SharedPtr req, SetBool::Response::SharedPtr res) +{ + is_activated_ = req->data; + if (is_activated_) { + ekf_pose_buffer_->clear(); + } + res->success = true; +} + +std::vector LidarMarkerLocalizer::detect_landmarks( + const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + // TODO(YamatoAndo) + // Transform sensor_frame to base_link + + pcl::PointCloud::Ptr points_ptr( + new pcl::PointCloud); + pcl::fromROSMsg(*points_msg_ptr, *points_ptr); + + if (points_ptr->empty()) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No points!"); + return std::vector{}; + } + + std::vector> ring_points(128); + + float min_x = std::numeric_limits::max(); + float max_x = std::numeric_limits::lowest(); + for (const autoware_point_types::PointXYZIRC & point : points_ptr->points) { + ring_points[point.channel].push_back(point); + min_x = std::min(min_x, point.x); + max_x = std::max(max_x, point.x); + } + + // Check that the leaf size is not too small, given the size of the data + const int bin_num = static_cast((max_x - min_x) / param_.resolution + 1); + + // initialize variables + std::vector vote(bin_num, 0); + std::vector min_y(bin_num, std::numeric_limits::max()); + + // for each channel + for (const pcl::PointCloud & one_ring : ring_points) { + std::vector intensity_sum(bin_num, 0.0); + std::vector intensity_num(bin_num, 0); + std::vector average_intensity(bin_num, 0.0); + + for (const autoware_point_types::PointXYZIRC & point : one_ring.points) { + const int bin_index = static_cast((point.x - min_x) / param_.resolution); + intensity_sum[bin_index] += point.intensity; + intensity_num[bin_index]++; + min_y[bin_index] = std::min(min_y[bin_index], point.y); + } + + // calc average + for (int bin_index = 0; bin_index < bin_num; bin_index++) { + if (intensity_num[bin_index] == 0) { + continue; + } + + average_intensity[bin_index] = intensity_sum[bin_index] / intensity_num[bin_index]; + } + + // pattern matching + for (size_t i = 0; i <= bin_num - param_.intensity_pattern.size(); i++) { + int64_t pos = 0; + int64_t neg = 0; + double min_intensity = std::numeric_limits::max(); + double max_intensity = std::numeric_limits::lowest(); + + // find max_min + for (size_t j = 0; j < param_.intensity_pattern.size(); j++) { + if (intensity_num[i + j] == 0) { + continue; + } + + min_intensity = std::min(min_intensity, average_intensity[i + j]); + max_intensity = std::max(max_intensity, average_intensity[i + j]); + } + + if (max_intensity <= min_intensity) { + continue; + } + + const double center_intensity = (max_intensity - min_intensity) / 2.0 + min_intensity; + + for (size_t j = 0; j < param_.intensity_pattern.size(); j++) { + if (intensity_num[i + j] == 0) { + continue; + } + + if (param_.intensity_pattern[j] == 1) { + // check positive + if ( + average_intensity[i + j] > + center_intensity + static_cast(param_.match_intensity_difference_threshold)) { + pos++; + } + } else if (param_.intensity_pattern[j] == -1) { + // check negative + if ( + average_intensity[i + j] < + center_intensity - static_cast(param_.match_intensity_difference_threshold)) { + neg++; + } + } else { + // ignore param_.intensity_pattern[j] == 0 + } + } + + if ( + pos >= param_.positive_match_num_threshold && neg >= param_.negative_match_num_threshold) { + vote[i]++; + } + } + } + + std::vector detected_landmarks; + + for (size_t i = 0; i < bin_num - param_.intensity_pattern.size(); i++) { + if (vote[i] > param_.vote_threshold_for_detect_marker) { + const double bin_position = + static_cast(i) + static_cast(param_.intensity_pattern.size()) / 2.0; + Pose marker_pose_on_base_link; + marker_pose_on_base_link.position.x = bin_position * param_.resolution + min_x; + marker_pose_on_base_link.position.y = min_y[i]; + marker_pose_on_base_link.position.z = param_.marker_height_from_ground; + marker_pose_on_base_link.orientation = + autoware::universe_utils::createQuaternionFromRPY(M_PI_2, 0.0, 0.0); // TODO(YamatoAndo) + detected_landmarks.push_back(landmark_manager::Landmark{"0", marker_pose_on_base_link}); + } + } + + return detected_landmarks; +} + +landmark_manager::Landmark get_nearest_landmark( + const geometry_msgs::msg::Pose & self_pose, + const std::vector & landmarks) +{ + landmark_manager::Landmark nearest_landmark; + double min_distance = std::numeric_limits::max(); + + for (const auto & landmark : landmarks) { + const double curr_distance = + autoware::universe_utils::calcDistance3d(landmark.pose.position, self_pose.position); + + if (curr_distance > min_distance) { + continue; + } + + min_distance = curr_distance; + nearest_landmark = landmark; + } + return nearest_landmark; +} + +std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) +{ + std::array ret_covariance = src_covariance; + + Eigen::Matrix3d src_cov; + src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], + src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], + src_covariance[14]; + + Eigen::Matrix3d ret_cov; + ret_cov = rotation * src_cov * rotation.transpose(); + + for (Eigen::Index i = 0; i < 3; ++i) { + ret_covariance[i] = ret_cov(0, i); + ret_covariance[i + 6] = ret_cov(1, i); + ret_covariance[i + 12] = ret_cov(2, i); + } + + return ret_covariance; +} + +sensor_msgs::msg::PointCloud2::SharedPtr LidarMarkerLocalizer::extract_marker_pointcloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg_ptr, + const geometry_msgs::msg::Pose marker_pose) const +{ + // convert from ROSMsg to PCL + pcl::shared_ptr> points_ptr( + new pcl::PointCloud); + pcl::fromROSMsg(*points_msg_ptr, *points_ptr); + + pcl::shared_ptr> marker_points_ptr( + new pcl::PointCloud); + + // extract marker pointcloud + for (const autoware_point_types::PointXYZIRC & point : points_ptr->points) { + const double xy_distance = std::sqrt( + std::pow(point.x - marker_pose.position.x, 2.0) + + std::pow(point.y - marker_pose.position.y, 2.0)); + if (xy_distance < param_.marker_width / 2.0) { + marker_points_ptr->push_back(point); + } + } + + marker_points_ptr->width = marker_points_ptr->size(); + marker_points_ptr->height = 1; + marker_points_ptr->is_dense = false; + + sensor_msgs::msg::PointCloud2::SharedPtr marker_points_msg_ptr(new sensor_msgs::msg::PointCloud2); + pcl::toROSMsg(*marker_points_ptr, *marker_points_msg_ptr); + marker_points_msg_ptr->header = points_msg_ptr->header; + + return marker_points_msg_ptr; +} + +void LidarMarkerLocalizer::save_detected_marker_log( + const sensor_msgs::msg::PointCloud2::SharedPtr & points_msg_ptr) +{ + // transform input_frame to save_frame_id + sensor_msgs::msg::PointCloud2::SharedPtr marker_points_msg_sensor_frame_ptr( + new sensor_msgs::msg::PointCloud2); + transform_sensor_measurement( + param_.save_frame_id, points_msg_ptr->header.frame_id, points_msg_ptr, + marker_points_msg_sensor_frame_ptr); + + // convert from ROSMsg to PCL + pcl::shared_ptr> + marker_points_sensor_frame_ptr(new pcl::PointCloud); + pcl::fromROSMsg(*marker_points_msg_sensor_frame_ptr, *marker_points_sensor_frame_ptr); + + // to csv format + std::stringstream log_message; + log_message << "point.position.x,point.position.y,point.position.z,point.intensity,point.ring" + << std::endl; + for (const auto & point : marker_points_sensor_frame_ptr->points) { + log_message << point.x; + log_message << "," << point.y; + log_message << "," << point.z; + log_message << "," << point.intensity; + log_message << "," << point.channel; + log_message << std::endl; + } + + // create file name + const double times_seconds = rclcpp::Time(points_msg_ptr->header.stamp).seconds(); + double time_integer_tmp = 0.0; + double time_decimal = std::modf(times_seconds, &time_integer_tmp); + auto time_integer = static_cast(time_integer_tmp); + struct tm * time_info{}; + time_info = std::localtime(&time_integer); + std::stringstream file_name; + file_name << param_.save_file_name << std::put_time(time_info, "%Y%m%d-%H%M%S") << "-" + << std::setw(3) << std::setfill('0') << static_cast((time_decimal) * 1000) + << ".csv"; + + // write log_message to file + std::filesystem::path save_file_directory_path = param_.save_file_directory_path; + std::filesystem::create_directories(save_file_directory_path); + std::ofstream csv_file(save_file_directory_path.append(file_name.str())); + csv_file << log_message.str(); + csv_file.close(); +} + +void LidarMarkerLocalizer::transform_sensor_measurement( + const std::string & source_frame, const std::string & target_frame, + const sensor_msgs::msg::PointCloud2::SharedPtr & sensor_points_input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & sensor_points_output_ptr) +{ + if (source_frame == target_frame) { + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_->lookupTransform(source_frame, target_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + // Since there is no clear error handling policy, temporarily return as is. + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = + autoware::universe_utils::transform2pose(transform); + const Eigen::Matrix4f base_to_sensor_matrix = + pose_to_matrix4f(target_to_source_pose_stamped.pose); + pcl_ros::transformPointCloud( + base_to_sensor_matrix, *sensor_points_input_ptr, *sensor_points_output_ptr); +} + +} // namespace autoware::lidar_marker_localizer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lidar_marker_localizer::LidarMarkerLocalizer) diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp new file mode 100644 index 0000000000000..11a6e0b34abdc --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -0,0 +1,148 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef LIDAR_MARKER_LOCALIZER_HPP_ +#define LIDAR_MARKER_LOCALIZER_HPP_ + +#include "localization_util/diagnostics_module.hpp" +#include "localization_util/smart_pose_buffer.hpp" + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#include +#else +#include + +#include +#include +#endif +#include + +#include + +#include +#include +#include + +namespace autoware::lidar_marker_localizer +{ + +class LidarMarkerLocalizer : public rclcpp::Node +{ + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PoseArray = geometry_msgs::msg::PoseArray; + using TransformStamped = geometry_msgs::msg::TransformStamped; + using Vector3 = geometry_msgs::msg::Vector3; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using SetBool = std_srvs::srv::SetBool; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + + struct Param + { + std::string marker_name; + + double resolution; + std::vector intensity_pattern; + int64_t match_intensity_difference_threshold; + int64_t positive_match_num_threshold; + int64_t negative_match_num_threshold; + int64_t vote_threshold_for_detect_marker; + double marker_height_from_ground; + + double self_pose_timeout_sec; + double self_pose_distance_tolerance_m; + + double limit_distance_from_self_pose_to_nearest_marker; + double limit_distance_from_self_pose_to_marker; + std::array base_covariance; + + double marker_width; + + bool enable_save_log; + std::string save_file_directory_path; + std::string save_file_name; + std::string save_frame_id; + }; + +public: + explicit LidarMarkerLocalizer(const rclcpp::NodeOptions & node_options); + +private: + void self_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & self_pose_msg_ptr); + void points_callback(const PointCloud2::ConstSharedPtr & points_msg_ptr); + void map_bin_callback(const HADMapBin::ConstSharedPtr & map_bin_msg_ptr); + void service_trigger_node( + const SetBool::Request::SharedPtr req, SetBool::Response::SharedPtr res); + + void initialize_diagnostics(); + void main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr); + std::vector detect_landmarks( + const PointCloud2::ConstSharedPtr & points_msg_ptr); + sensor_msgs::msg::PointCloud2::SharedPtr extract_marker_pointcloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg_ptr, + const geometry_msgs::msg::Pose marker_pose) const; + void save_detected_marker_log(const sensor_msgs::msg::PointCloud2::SharedPtr & points_msg_ptr); + + void transform_sensor_measurement( + const std::string & source_frame, const std::string & target_frame, + const sensor_msgs::msg::PointCloud2::SharedPtr & sensor_points_input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & sensor_points_output_ptr); + + std::shared_ptr tf_listener_; + std::shared_ptr tf_buffer_; + + rclcpp::Subscription::SharedPtr sub_points_; + rclcpp::Subscription::SharedPtr sub_self_pose_; + rclcpp::Subscription::SharedPtr sub_map_bin_; + + rclcpp::Publisher::SharedPtr + pub_base_link_pose_with_covariance_on_map_; + rclcpp::Service::SharedPtr service_trigger_node_; + rclcpp::Publisher::SharedPtr pub_marker_mapped_; + rclcpp::Publisher::SharedPtr pub_marker_detected_; + rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; + rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; + + std::shared_ptr diagnostics_module_; + + Param param_; + bool is_activated_; + std::unique_ptr ekf_pose_buffer_; + + landmark_manager::LandmarkManager landmark_manager_; +}; + +} // namespace autoware::lidar_marker_localizer + +#endif // LIDAR_MARKER_LOCALIZER_HPP_ diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/autoware_pose_estimator_arbiter/CMakeLists.txt similarity index 83% rename from localization/pose_estimator_arbiter/CMakeLists.txt rename to localization/autoware_pose_estimator_arbiter/CMakeLists.txt index eefb7fc9a6879..a45da8767b72b 100644 --- a/localization/pose_estimator_arbiter/CMakeLists.txt +++ b/localization/autoware_pose_estimator_arbiter/CMakeLists.txt @@ -1,22 +1,25 @@ cmake_minimum_required(VERSION 3.14) -project(pose_estimator_arbiter) +project(autoware_pose_estimator_arbiter) find_package(autoware_cmake REQUIRED) autoware_package() find_package(PCL REQUIRED COMPONENTS common) -include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) +include_directories( + SYSTEM ${PCL_INCLUDE_DIRS} + src +) # ============================== # pose estimator arbiter node ament_auto_add_library(${PROJECT_NAME} SHARED - src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp - src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp + src/pose_estimator_arbiter_core.cpp + src/switch_rule/enable_all_rule.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC src) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "pose_estimator_arbiter::PoseEstimatorArbiter" + PLUGIN "autoware::pose_estimator_arbiter::PoseEstimatorArbiter" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR MultiThreadedExecutor ) diff --git a/localization/pose_estimator_arbiter/README.md b/localization/autoware_pose_estimator_arbiter/README.md similarity index 94% rename from localization/pose_estimator_arbiter/README.md rename to localization/autoware_pose_estimator_arbiter/README.md index 7b9397dfdba47..938bb20e252ac 100644 --- a/localization/pose_estimator_arbiter/README.md +++ b/localization/autoware_pose_estimator_arbiter/README.md @@ -1,4 +1,4 @@ -# pose_estimator_arbiter +# autoware_pose_estimator_arbiter Table of contents: @@ -35,7 +35,7 @@ Also, even if both can be activated at the same time, the Kalman Filter may be a - [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) - [eagleye](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/launch-autoware/localization/eagleye/) - [yabloc](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc) -- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/landmark_based_localizer) +- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_landmark_based_localizer) ### Demonstration @@ -117,8 +117,8 @@ If it does not seems to work, users can get more information in the following wa > [!TIP] > > ```bash -> ros2 service call /localization/pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \ -> '{logger_name: localization.pose_estimator_arbiter, level: debug}' +> ros2 service call /localization/autoware_pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \ +> '{logger_name: localization.autoware_pose_estimator_arbiter, level: debug}' > ``` ## Architecture @@ -135,7 +135,7 @@ Following figure shows the node configuration when NDT, YabLoc Eagleye and AR-Ta ### Case of running multiple pose estimators -When running multiple pose_estimators, pose_estimator_arbiter is executed. +When running multiple pose_estimators, autoware_pose_estimator_arbiter is executed. It comprises a **switching rule** and **stoppers** corresponding to each pose_estimator. - Stoppers control the pose_estimator activity by relaying inputs or outputs, or by requesting a suspend service. @@ -187,14 +187,14 @@ ros2 launch autoware_launch logging_simulator.launch.xml \ Even if `pose_source` includes an unexpected string, it will be filtered appropriately. Please see the table below for details. -| given runtime argument | parsed pose_estimator_arbiter's param (pose_sources) | -| ------------------------------------------- | ---------------------------------------------------- | -| `pose_source:=ndt` | `["ndt"]` | -| `pose_source:=nan` | `[]` | -| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` | -| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` | -| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` | -| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` | +| given runtime argument | parsed autoware_pose_estimator_arbiter's param (pose_sources) | +| ------------------------------------------- | ------------------------------------------------------------- | +| `pose_source:=ndt` | `["ndt"]` | +| `pose_source:=nan` | `[]` | +| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` | +| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` | +| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` | +| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` | diff --git a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt b/localization/autoware_pose_estimator_arbiter/example_rule/CMakeLists.txt similarity index 67% rename from localization/pose_estimator_arbiter/example_rule/CMakeLists.txt rename to localization/autoware_pose_estimator_arbiter/example_rule/CMakeLists.txt index b2b5a828e42e7..5125829ffd69c 100644 --- a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt +++ b/localization/autoware_pose_estimator_arbiter/example_rule/CMakeLists.txt @@ -1,13 +1,13 @@ # example switch rule library ament_auto_add_library(example_rule SHARED - src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp - src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp - src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp - src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp + src/switch_rule/pcd_map_based_rule.cpp + src/switch_rule/vector_map_based_rule.cpp + src/rule_helper/pcd_occupancy.cpp + src/rule_helper/pose_estimator_area.cpp ) target_include_directories(example_rule PUBLIC src example_rule/src) -target_link_libraries(example_rule pose_estimator_arbiter) +target_link_libraries(example_rule autoware_pose_estimator_arbiter) # ============================== # define test definition macro @@ -16,7 +16,7 @@ function(add_testcase filepath) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" pose_estimator_arbiter example_rule) + target_link_libraries("${test_name}" autoware_pose_estimator_arbiter example_rule) target_include_directories(${test_name} PUBLIC src) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/localization/pose_estimator_arbiter/example_rule/README.md b/localization/autoware_pose_estimator_arbiter/example_rule/README.md similarity index 100% rename from localization/pose_estimator_arbiter/example_rule/README.md rename to localization/autoware_pose_estimator_arbiter/example_rule/README.md diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/grid_key.hpp similarity index 78% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/grid_key.hpp index b4ad7111e4ba0..d4d4409ade979 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/grid_key.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ -#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#ifndef RULE_HELPER__GRID_KEY_HPP_ +#define RULE_HELPER__GRID_KEY_HPP_ #include #include #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { struct GridKey { @@ -46,16 +46,16 @@ struct GridKey friend bool operator!=(const GridKey & one, const GridKey & other) { return !(one == other); } }; -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper // This is for unordered_map and unordered_set namespace std { template <> -struct hash +struct hash { public: - size_t operator()(const pose_estimator_arbiter::rule_helper::GridKey & grid) const + size_t operator()(const autoware::pose_estimator_arbiter::rule_helper::GridKey & grid) const { std::size_t seed = 0; boost::hash_combine(seed, grid.x); @@ -65,4 +65,4 @@ struct hash }; } // namespace std -#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#endif // RULE_HELPER__GRID_KEY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.cpp similarity index 93% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.cpp index 4b0188a1638f6..197bff459530d 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" +#include "rule_helper/pcd_occupancy.hpp" -#include "pose_estimator_arbiter/rule_helper/grid_key.hpp" +#include "rule_helper/grid_key.hpp" #include @@ -25,7 +25,7 @@ #include #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold) : pcd_density_upper_threshold_(pcd_density_upper_threshold), @@ -114,4 +114,4 @@ void PcdOccupancy::init(PointCloud2::ConstSharedPtr msg) kdtree_->setInputCloud(occupied_areas_); } -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.hpp similarity index 83% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.hpp index 098704e11aba9..e38cb1bd1ee38 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ -#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ +#ifndef RULE_HELPER__PCD_OCCUPANCY_HPP_ +#define RULE_HELPER__PCD_OCCUPANCY_HPP_ #include @@ -26,7 +26,7 @@ #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { class PcdOccupancy { @@ -49,6 +49,6 @@ class PcdOccupancy pcl::KdTreeFLANN::Ptr kdtree_{nullptr}; }; -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper -#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ +#endif // RULE_HELPER__PCD_OCCUPANCY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp similarity index 96% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp index 1bf359b6ab54d..41ad45430d238 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" +#include "rule_helper/pose_estimator_area.hpp" #include #include @@ -26,7 +26,7 @@ #include #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { using BoostPoint = boost::geometry::model::d2::point_xy; using BoostPolygon = boost::geometry::model::polygon; @@ -141,4 +141,4 @@ bool PoseEstimatorArea::Impl::within( } return false; } -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.hpp similarity index 83% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.hpp index 74843c66a4eba..262b064615bc1 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ -#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ +#ifndef RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ +#define RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ #include #include @@ -25,7 +25,7 @@ #include #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { class PoseEstimatorArea { @@ -51,6 +51,6 @@ class PoseEstimatorArea rclcpp::Logger logger_; }; -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper -#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ +#endif // RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp similarity index 93% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp index 3a565e7f2e4df..cc5414ebd0a68 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" +#include "switch_rule/pcd_map_based_rule.hpp" #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { PcdMapBasedRule::PcdMapBasedRule( rclcpp::Node & node, std::unordered_set running_estimator_list, @@ -77,4 +77,4 @@ std::unordered_map PcdMapBasedRule::update() {PoseEstimatorType::artag, false}}; } -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.hpp similarity index 71% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.hpp index ab4d18dcad66a..45559fe37e606 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ -#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ +#ifndef SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ +#define SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ -#include "pose_estimator_arbiter/pose_estimator_type.hpp" -#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" -#include "pose_estimator_arbiter/shared_data.hpp" -#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include "pose_estimator_type.hpp" +#include "rule_helper/pcd_occupancy.hpp" +#include "shared_data.hpp" +#include "switch_rule/base_switch_rule.hpp" #include #include #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { class PcdMapBasedRule : public BaseSwitchRule { @@ -49,6 +49,6 @@ class PcdMapBasedRule : public BaseSwitchRule // Store the reason why which pose estimator is enabled mutable std::string debug_string_; }; -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule -#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ +#endif // SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp similarity index 93% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp index dffb738e87685..58cf9ab09e9a7 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" +#include "switch_rule/vector_map_based_rule.hpp" #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { VectorMapBasedRule::VectorMapBasedRule( rclcpp::Node & node, std::unordered_set running_estimator_list, @@ -76,4 +76,4 @@ std::unordered_map VectorMapBasedRule::update() return enable_list; } -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.hpp similarity index 71% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.hpp index 83723a346054b..356651af223f3 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ -#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ +#ifndef SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ +#define SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ -#include "pose_estimator_arbiter/pose_estimator_type.hpp" -#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" -#include "pose_estimator_arbiter/shared_data.hpp" -#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include "pose_estimator_type.hpp" +#include "rule_helper/pose_estimator_area.hpp" +#include "shared_data.hpp" +#include "switch_rule/base_switch_rule.hpp" #include #include @@ -26,7 +26,7 @@ #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { class VectorMapBasedRule : public BaseSwitchRule { @@ -50,6 +50,6 @@ class VectorMapBasedRule : public BaseSwitchRule std::unique_ptr pose_estimator_area_{nullptr}; }; -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule -#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ +#endif // SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp similarity index 62% rename from localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp index f53e01dc9c548..0d1f245123806 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" +#include "switch_rule/pcd_map_based_rule.hpp" #include #include @@ -30,21 +30,21 @@ class PcdMapBasedRuleMockNode : public ::testing::Test node->declare_parameter("pcd_density_upper_threshold", 5); const auto running_estimator_list = - std::unordered_set{ - pose_estimator_arbiter::PoseEstimatorType::ndt, - pose_estimator_arbiter::PoseEstimatorType::yabloc, - pose_estimator_arbiter::PoseEstimatorType::eagleye, - pose_estimator_arbiter::PoseEstimatorType::artag}; + std::unordered_set{ + autoware::pose_estimator_arbiter::PoseEstimatorType::ndt, + autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc, + autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye, + autoware::pose_estimator_arbiter::PoseEstimatorType::artag}; - shared_data_ = std::make_shared(); + shared_data_ = std::make_shared(); - rule_ = std::make_shared( + rule_ = std::make_shared( *node, running_estimator_list, shared_data_); } std::shared_ptr node; - std::shared_ptr shared_data_; - std::shared_ptr rule_; + std::shared_ptr shared_data_; + std::shared_ptr rule_; void TearDown() override { rclcpp::shutdown(); } }; @@ -81,10 +81,10 @@ TEST_F(PcdMapBasedRuleMockNode, pcdMapBasedRule) shared_data_->localization_pose_cov.set_and_invoke( std::make_shared(position)); auto ret = rule_->update(); - EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + EXPECT_TRUE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::artag)); } { @@ -92,9 +92,9 @@ TEST_F(PcdMapBasedRuleMockNode, pcdMapBasedRule) shared_data_->localization_pose_cov.set_and_invoke( std::make_shared(position)); auto ret = rule_->update(); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); - EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_TRUE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::artag)); } } diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp similarity index 87% rename from localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp index 3258a1be34fda..10833b5498b89 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/rule_helper/grid_key.hpp" -#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" -#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" +#include "rule_helper/grid_key.hpp" +#include "rule_helper/pcd_occupancy.hpp" +#include "rule_helper/pose_estimator_area.hpp" #include @@ -64,7 +64,7 @@ TEST_F(RuleHelperMockNode, poseEstimatorArea) HADMapBin msg; lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); - pose_estimator_arbiter::rule_helper::PoseEstimatorArea pose_estimator_area(&(*node)); + autoware::pose_estimator_arbiter::rule_helper::PoseEstimatorArea pose_estimator_area(&(*node)); pose_estimator_area.init(std::make_shared(msg)); EXPECT_TRUE(pose_estimator_area.within(Point().set__x(5).set__y(5).set__z(0), "ndt")); @@ -75,11 +75,11 @@ TEST_F(RuleHelperMockNode, poseEstimatorArea) TEST_F(RuleHelperMockNode, pcdOccupancy) { - using pose_estimator_arbiter::rule_helper::PcdOccupancy; + using autoware::pose_estimator_arbiter::rule_helper::PcdOccupancy; const int pcd_density_upper_threshold = 20; const int pcd_density_lower_threshold = 10; - pose_estimator_arbiter::rule_helper::PcdOccupancy pcd_occupancy( + autoware::pose_estimator_arbiter::rule_helper::PcdOccupancy pcd_occupancy( pcd_density_upper_threshold, pcd_density_lower_threshold); geometry_msgs::msg::Point point; @@ -91,7 +91,7 @@ TEST_F(RuleHelperMockNode, pcdOccupancy) TEST_F(RuleHelperMockNode, gridKey) { - using pose_estimator_arbiter::rule_helper::GridKey; + using autoware::pose_estimator_arbiter::rule_helper::GridKey; EXPECT_TRUE(GridKey(10., -5.) == GridKey(10., -10.)); EXPECT_TRUE(GridKey(10., -5.) != GridKey(10., -15.)); diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp similarity index 65% rename from localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp index ec8c905bf8311..9d537f2048176 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" +#include "switch_rule/vector_map_based_rule.hpp" #include @@ -33,21 +33,21 @@ class VectorMapBasedRuleMockNode : public ::testing::Test node = std::make_shared("test_node"); const auto running_estimator_list = - std::unordered_set{ - pose_estimator_arbiter::PoseEstimatorType::ndt, - pose_estimator_arbiter::PoseEstimatorType::yabloc, - pose_estimator_arbiter::PoseEstimatorType::eagleye, - pose_estimator_arbiter::PoseEstimatorType::artag}; + std::unordered_set{ + autoware::pose_estimator_arbiter::PoseEstimatorType::ndt, + autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc, + autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye, + autoware::pose_estimator_arbiter::PoseEstimatorType::artag}; - shared_data_ = std::make_shared(); + shared_data_ = std::make_shared(); - rule_ = std::make_shared( + rule_ = std::make_shared( *node, running_estimator_list, shared_data_); } std::shared_ptr node; - std::shared_ptr shared_data_; - std::shared_ptr rule_; + std::shared_ptr shared_data_; + std::shared_ptr rule_; void TearDown() override { rclcpp::shutdown(); } }; @@ -90,17 +90,17 @@ TEST_F(VectorMapBasedRuleMockNode, vectorMapBasedRule) { shared_data_->localization_pose_cov.set_and_invoke(create_pose(5, 5)); auto ret = rule_->update(); - EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + EXPECT_TRUE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::artag)); } { shared_data_->localization_pose_cov.set_and_invoke(create_pose(15, 15)); auto ret = rule_->update(); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::artag)); } } diff --git a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml b/localization/autoware_pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml similarity index 93% rename from localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml rename to localization/autoware_pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml index b5be96fc3ce44..d38f0e2a104a8 100644 --- a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml +++ b/localization/autoware_pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/pose_estimator_arbiter/media/architecture.drawio.svg b/localization/autoware_pose_estimator_arbiter/media/architecture.drawio.svg similarity index 100% rename from localization/pose_estimator_arbiter/media/architecture.drawio.svg rename to localization/autoware_pose_estimator_arbiter/media/architecture.drawio.svg diff --git a/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg b/localization/autoware_pose_estimator_arbiter/media/pcd_occupancy.drawio.svg similarity index 100% rename from localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg rename to localization/autoware_pose_estimator_arbiter/media/pcd_occupancy.drawio.svg diff --git a/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png b/localization/autoware_pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png similarity index 100% rename from localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png rename to localization/autoware_pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png diff --git a/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg b/localization/autoware_pose_estimator_arbiter/media/single_pose_estimator.drawio.svg similarity index 100% rename from localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg rename to localization/autoware_pose_estimator_arbiter/media/single_pose_estimator.drawio.svg diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/autoware_pose_estimator_arbiter/package.xml similarity index 97% rename from localization/pose_estimator_arbiter/package.xml rename to localization/autoware_pose_estimator_arbiter/package.xml index d97c9f15fc8ae..f3aa30a2c6425 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/autoware_pose_estimator_arbiter/package.xml @@ -1,7 +1,7 @@ - pose_estimator_arbiter + autoware_pose_estimator_arbiter 0.1.0 The arbiter of multiple pose estimators Yamato Ando diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp similarity index 89% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp rename to localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp index 013b4b38f9ef6..b70ab378eac67 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ -#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ +#ifndef POSE_ESTIMATOR_ARBITER_HPP_ +#define POSE_ESTIMATOR_ARBITER_HPP_ -#include "pose_estimator_arbiter/shared_data.hpp" -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" -#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include "shared_data.hpp" +#include "stopper/base_stopper.hpp" +#include "switch_rule/base_switch_rule.hpp" #include #include @@ -32,7 +32,7 @@ #include #include -namespace pose_estimator_arbiter +namespace autoware::pose_estimator_arbiter { class PoseEstimatorArbiter : public rclcpp::Node { @@ -95,6 +95,6 @@ class PoseEstimatorArbiter : public rclcpp::Node // Timer callback void on_timer(); }; -} // namespace pose_estimator_arbiter +} // namespace autoware::pose_estimator_arbiter -#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ +#endif // POSE_ESTIMATOR_ARBITER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp similarity index 90% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp rename to localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp index 8e25628d6e0fc..9108d44e82fb5 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp +++ b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/pose_estimator_arbiter.hpp" -#include "pose_estimator_arbiter/pose_estimator_type.hpp" -#include "pose_estimator_arbiter/stopper/stopper_artag.hpp" -#include "pose_estimator_arbiter/stopper/stopper_eagleye.hpp" -#include "pose_estimator_arbiter/stopper/stopper_ndt.hpp" -#include "pose_estimator_arbiter/stopper/stopper_yabloc.hpp" -#include "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" +#include "pose_estimator_arbiter.hpp" +#include "pose_estimator_type.hpp" +#include "stopper/stopper_artag.hpp" +#include "stopper/stopper_eagleye.hpp" +#include "stopper/stopper_ndt.hpp" +#include "stopper/stopper_yabloc.hpp" +#include "switch_rule/enable_all_rule.hpp" #include -namespace pose_estimator_arbiter +namespace autoware::pose_estimator_arbiter { // Parses ros param to get the estimator set that is running static std::unordered_set parse_estimator_name_args( @@ -111,9 +111,9 @@ PoseEstimatorArbiter::PoseEstimatorArbiter(const rclcpp::NodeOptions & options) load_switch_rule(); // Timer callback - auto on_timer = std::bind(&PoseEstimatorArbiter::on_timer, this); - timer_ = - rclcpp::create_timer(this, this->get_clock(), rclcpp::Rate(1).period(), std::move(on_timer)); + auto on_timer_callback = std::bind(&PoseEstimatorArbiter::on_timer, this); + timer_ = rclcpp::create_timer( + this, this->get_clock(), rclcpp::Rate(1).period(), std::move(on_timer_callback)); // Enable all pose estimators at the first toggle_all(true); @@ -210,7 +210,7 @@ void PoseEstimatorArbiter::on_timer() publish_diagnostics(); } -} // namespace pose_estimator_arbiter +} // namespace autoware::pose_estimator_arbiter #include -RCLCPP_COMPONENTS_REGISTER_NODE(pose_estimator_arbiter::PoseEstimatorArbiter) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pose_estimator_arbiter::PoseEstimatorArbiter) diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_type.hpp similarity index 73% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp rename to localization/autoware_pose_estimator_arbiter/src/pose_estimator_type.hpp index d78bfeb05b4f0..98f92612f8bc7 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_type.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ -#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ +#ifndef POSE_ESTIMATOR_TYPE_HPP_ +#define POSE_ESTIMATOR_TYPE_HPP_ -namespace pose_estimator_arbiter +namespace autoware::pose_estimator_arbiter { enum class PoseEstimatorType : int { ndt = 1, yabloc = 2, eagleye = 4, artag = 8 }; -} // namespace pose_estimator_arbiter +} // namespace autoware::pose_estimator_arbiter -#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ +#endif // POSE_ESTIMATOR_TYPE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp b/localization/autoware_pose_estimator_arbiter/src/shared_data.hpp similarity index 93% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp rename to localization/autoware_pose_estimator_arbiter/src/shared_data.hpp index 0d3dbfab11cbe..6332b77a9ed31 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/shared_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ -#define POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ +#ifndef SHARED_DATA_HPP_ +#define SHARED_DATA_HPP_ #include #include @@ -25,7 +25,7 @@ #include #include -namespace pose_estimator_arbiter +namespace autoware::pose_estimator_arbiter { template struct CallbackInvokingVariable @@ -97,5 +97,5 @@ struct SharedData InitializationState{}.set__state(InitializationState::UNINITIALIZED))}; }; -} // namespace pose_estimator_arbiter -#endif // POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ +} // namespace autoware::pose_estimator_arbiter +#endif // SHARED_DATA_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/base_stopper.hpp similarity index 81% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/base_stopper.hpp index d64592a12308e..8b47b0e3b3a9a 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/base_stopper.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ +#ifndef STOPPER__BASE_STOPPER_HPP_ +#define STOPPER__BASE_STOPPER_HPP_ -#include "pose_estimator_arbiter/shared_data.hpp" +#include "shared_data.hpp" #include #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class BaseStopper { @@ -48,6 +48,6 @@ class BaseStopper rclcpp::Logger logger_; std::shared_ptr shared_data_{nullptr}; }; -} // namespace pose_estimator_arbiter::stopper +} // namespace autoware::pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ +#endif // STOPPER__BASE_STOPPER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_artag.hpp similarity index 82% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/stopper_artag.hpp index f334983f33aad..3635ebd39c545 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_artag.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ +#ifndef STOPPER__STOPPER_ARTAG_HPP_ +#define STOPPER__STOPPER_ARTAG_HPP_ -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#include "stopper/base_stopper.hpp" #include #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class StopperArTag : public BaseStopper { @@ -52,6 +52,6 @@ class StopperArTag : public BaseStopper bool ar_tag_is_enabled_; rclcpp::Publisher::SharedPtr pub_image_; }; -} // namespace pose_estimator_arbiter::stopper +} // namespace autoware::pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ +#endif // STOPPER__STOPPER_ARTAG_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_eagleye.hpp similarity index 81% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/stopper_eagleye.hpp index cc800ad6bdee4..cfddf3e13014f 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_eagleye.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#ifndef STOPPER__STOPPER_EAGLEYE_HPP_ +#define STOPPER__STOPPER_EAGLEYE_HPP_ +#include "stopper/base_stopper.hpp" #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class StopperEagleye : public BaseStopper { @@ -49,6 +49,6 @@ class StopperEagleye : public BaseStopper bool eagleye_is_enabled_; rclcpp::Publisher::SharedPtr pub_pose_; }; -} // namespace pose_estimator_arbiter::stopper +} // namespace autoware::pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ +#endif // STOPPER__STOPPER_EAGLEYE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_ndt.hpp similarity index 81% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/stopper_ndt.hpp index 3b7c083ea31e3..eba43118c7860 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_ndt.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#ifndef STOPPER__STOPPER_NDT_HPP_ +#define STOPPER__STOPPER_NDT_HPP_ +#include "stopper/base_stopper.hpp" #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class StopperNdt : public BaseStopper { @@ -49,6 +49,6 @@ class StopperNdt : public BaseStopper rclcpp::Publisher::SharedPtr pub_pointcloud_; bool ndt_is_enabled_; }; -} // namespace pose_estimator_arbiter::stopper +} // namespace autoware::pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ +#endif // STOPPER__STOPPER_NDT_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_yabloc.hpp similarity index 89% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/stopper_yabloc.hpp index 808a5bf9407ca..77aa9603ac06a 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_yabloc.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#ifndef STOPPER__STOPPER_YABLOC_HPP_ +#define STOPPER__STOPPER_YABLOC_HPP_ +#include "stopper/base_stopper.hpp" #include #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class StopperYabLoc : public BaseStopper { @@ -86,5 +86,5 @@ class StopperYabLoc : public BaseStopper rclcpp::Client::SharedPtr enable_service_client_; rclcpp::Publisher::SharedPtr pub_image_; }; -} // namespace pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ +} // namespace autoware::pose_estimator_arbiter::stopper +#endif // STOPPER__STOPPER_YABLOC_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp b/localization/autoware_pose_estimator_arbiter/src/switch_rule/base_switch_rule.hpp similarity index 78% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp rename to localization/autoware_pose_estimator_arbiter/src/switch_rule/base_switch_rule.hpp index 95d10c2b741a8..0462ce6e7d340 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/switch_rule/base_switch_rule.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ -#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ +#ifndef SWITCH_RULE__BASE_SWITCH_RULE_HPP_ +#define SWITCH_RULE__BASE_SWITCH_RULE_HPP_ -#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_type.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { class BaseSwitchRule { @@ -44,7 +44,8 @@ class BaseSwitchRule BaseSwitchRule(BaseSwitchRule && other) noexcept = default; BaseSwitchRule & operator=(const BaseSwitchRule & other) = default; BaseSwitchRule & operator=(BaseSwitchRule && other) noexcept = default; - virtual std::unordered_map update() = 0; + virtual std::unordered_map + update() = 0; virtual std::string debug_string() { return std::string{}; } virtual MarkerArray debug_marker_array() { return MarkerArray{}; } @@ -53,6 +54,6 @@ class BaseSwitchRule std::shared_ptr logger_ptr_{nullptr}; }; -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule -#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ +#endif // SWITCH_RULE__BASE_SWITCH_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp similarity index 88% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp rename to localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp index aebad094a0eca..9767e9ef4fba4 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" +#include "switch_rule/enable_all_rule.hpp" #include @@ -22,7 +22,7 @@ #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { EnableAllRule::EnableAllRule( rclcpp::Node & node, std::unordered_set running_estimator_list, @@ -41,4 +41,4 @@ std::unordered_map EnableAllRule::update() }; } -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.hpp similarity index 71% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp rename to localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.hpp index 63142b0e662e1..bf2aa68df35d5 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ -#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ +#ifndef SWITCH_RULE__ENABLE_ALL_RULE_HPP_ +#define SWITCH_RULE__ENABLE_ALL_RULE_HPP_ -#include "pose_estimator_arbiter/shared_data.hpp" -#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include "shared_data.hpp" +#include "switch_rule/base_switch_rule.hpp" #include #include #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { class EnableAllRule : public BaseSwitchRule { @@ -38,6 +38,6 @@ class EnableAllRule : public BaseSwitchRule const std::unordered_set running_estimator_list_; }; -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule -#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ +#endif // SWITCH_RULE__ENABLE_ALL_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py b/localization/autoware_pose_estimator_arbiter/test/test_pose_estimator_arbiter.py similarity index 98% rename from localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py rename to localization/autoware_pose_estimator_arbiter/test/test_pose_estimator_arbiter.py index c419fb68e0571..a5ff04c002004 100644 --- a/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py +++ b/localization/autoware_pose_estimator_arbiter/test/test_pose_estimator_arbiter.py @@ -38,7 +38,7 @@ @pytest.mark.launch_test def generate_test_description(): test_pose_estimator_arbiter_launch_file = os.path.join( - get_package_share_directory("pose_estimator_arbiter"), + get_package_share_directory("autoware_pose_estimator_arbiter"), "launch", "pose_estimator_arbiter.launch.xml", ) diff --git a/localization/pose_estimator_arbiter/test/test_shared_data.cpp b/localization/autoware_pose_estimator_arbiter/test/test_shared_data.cpp similarity index 90% rename from localization/pose_estimator_arbiter/test/test_shared_data.cpp rename to localization/autoware_pose_estimator_arbiter/test/test_shared_data.cpp index 3c1fa50b502a1..c0f7044442d71 100644 --- a/localization/pose_estimator_arbiter/test/test_shared_data.cpp +++ b/localization/autoware_pose_estimator_arbiter/test/test_shared_data.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/shared_data.hpp" +#include "shared_data.hpp" #include TEST(SharedData, callback_invoked_correctly) { - pose_estimator_arbiter::CallbackInvokingVariable variable; + autoware::pose_estimator_arbiter::CallbackInvokingVariable variable; // register callback bool callback_invoked = false; @@ -44,7 +44,7 @@ TEST(SharedData, callback_invoked_correctly) TEST(SharedData, multiple_callback_invoked_correctly) { - pose_estimator_arbiter::CallbackInvokingVariable variable; + autoware::pose_estimator_arbiter::CallbackInvokingVariable variable; // register callback int callback_invoked_num = 0; diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/autoware_pose_instability_detector/CMakeLists.txt similarity index 82% rename from localization/pose_instability_detector/CMakeLists.txt rename to localization/autoware_pose_instability_detector/CMakeLists.txt index c6f94ab7df16e..9e7a1d15ee0f2 100644 --- a/localization/pose_instability_detector/CMakeLists.txt +++ b/localization/autoware_pose_instability_detector/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(pose_instability_detector) +project(autoware_pose_instability_detector) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "PoseInstabilityDetector" + PLUGIN "autoware::pose_instability_detector::PoseInstabilityDetector" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR SingleThreadedExecutor ) diff --git a/localization/pose_instability_detector/README.md b/localization/autoware_pose_instability_detector/README.md similarity index 98% rename from localization/pose_instability_detector/README.md rename to localization/autoware_pose_instability_detector/README.md index 4ced0fa8eb97b..5beb796f46e12 100644 --- a/localization/pose_instability_detector/README.md +++ b/localization/autoware_pose_instability_detector/README.md @@ -1,4 +1,4 @@ -# pose_instability_detector +# autoware_pose_instability_detector The `pose_instability_detector` is a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). @@ -109,7 +109,7 @@ $$ ## Parameters -{{ json_to_markdown("localization/pose_instability_detector/schema/pose_instability_detector.schema.json") }} +{{ json_to_markdown("localization/autoware_pose_instability_detector/schema/pose_instability_detector.schema.json") }} ## Input diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/autoware_pose_instability_detector/config/pose_instability_detector.param.yaml similarity index 100% rename from localization/pose_instability_detector/config/pose_instability_detector.param.yaml rename to localization/autoware_pose_instability_detector/config/pose_instability_detector.param.yaml diff --git a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp b/localization/autoware_pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp similarity index 97% rename from localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp rename to localization/autoware_pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp index febff8c2ee244..615b818a2bde0 100644 --- a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp +++ b/localization/autoware_pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp @@ -26,6 +26,8 @@ #include #include +namespace autoware::pose_instability_detector +{ class PoseInstabilityDetector : public rclcpp::Node { using Quaternion = geometry_msgs::msg::Quaternion; @@ -93,5 +95,6 @@ class PoseInstabilityDetector : public rclcpp::Node std::optional prev_odometry_ = std::nullopt; std::deque twist_buffer_; }; +} // namespace autoware::pose_instability_detector #endif // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/autoware_pose_instability_detector/launch/pose_instability_detector.launch.xml similarity index 58% rename from localization/pose_instability_detector/launch/pose_instability_detector.launch.xml rename to localization/autoware_pose_instability_detector/launch/pose_instability_detector.launch.xml index 5ebe7d7e429e0..716f2b165bb9b 100644 --- a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml +++ b/localization/autoware_pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -1,12 +1,12 @@ - + - + diff --git a/localization/pose_instability_detector/media/how_to_snip_twist.png b/localization/autoware_pose_instability_detector/media/how_to_snip_twist.png similarity index 100% rename from localization/pose_instability_detector/media/how_to_snip_twist.png rename to localization/autoware_pose_instability_detector/media/how_to_snip_twist.png diff --git a/localization/pose_instability_detector/media/lateral_threshold_calculation.png b/localization/autoware_pose_instability_detector/media/lateral_threshold_calculation.png similarity index 100% rename from localization/pose_instability_detector/media/lateral_threshold_calculation.png rename to localization/autoware_pose_instability_detector/media/lateral_threshold_calculation.png diff --git a/localization/pose_instability_detector/media/pose_instability_detector_overview.png b/localization/autoware_pose_instability_detector/media/pose_instability_detector_overview.png similarity index 100% rename from localization/pose_instability_detector/media/pose_instability_detector_overview.png rename to localization/autoware_pose_instability_detector/media/pose_instability_detector_overview.png diff --git a/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg b/localization/autoware_pose_instability_detector/media/pose_instabilty_detector_procedure.svg similarity index 100% rename from localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg rename to localization/autoware_pose_instability_detector/media/pose_instabilty_detector_procedure.svg diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/autoware_pose_instability_detector/media/rqt_runtime_monitor.png similarity index 100% rename from localization/pose_instability_detector/media/rqt_runtime_monitor.png rename to localization/autoware_pose_instability_detector/media/rqt_runtime_monitor.png diff --git a/localization/pose_instability_detector/media/timeline.drawio.svg b/localization/autoware_pose_instability_detector/media/timeline.drawio.svg similarity index 100% rename from localization/pose_instability_detector/media/timeline.drawio.svg rename to localization/autoware_pose_instability_detector/media/timeline.drawio.svg diff --git a/localization/pose_instability_detector/package.xml b/localization/autoware_pose_instability_detector/package.xml similarity index 91% rename from localization/pose_instability_detector/package.xml rename to localization/autoware_pose_instability_detector/package.xml index 7e0237cbaf780..d59baf5817233 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/autoware_pose_instability_detector/package.xml @@ -1,9 +1,9 @@ - pose_instability_detector + autoware_pose_instability_detector 0.1.0 - The pose_instability_detector package + The autoware_pose_instability_detector package Yamato Ando Kento Yabuuchi Masahiro Sakamoto diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/autoware_pose_instability_detector/schema/pose_instability_detector.schema.json similarity index 100% rename from localization/pose_instability_detector/schema/pose_instability_detector.schema.json rename to localization/autoware_pose_instability_detector/schema/pose_instability_detector.schema.json diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp similarity index 98% rename from localization/pose_instability_detector/src/pose_instability_detector.cpp rename to localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp index 23362dd13c6bc..36aa4f6d7d73e 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp @@ -25,6 +25,8 @@ #include #include +namespace autoware::pose_instability_detector +{ PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_instability_detector", options), timer_period_(this->declare_parameter("timer_period")), @@ -409,6 +411,7 @@ PoseInstabilityDetector::clip_out_necessary_twist( } return result_deque; } +} // namespace autoware::pose_instability_detector #include -RCLCPP_COMPONENTS_REGISTER_NODE(PoseInstabilityDetector) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pose_instability_detector::PoseInstabilityDetector) diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/autoware_pose_instability_detector/test/test.cpp similarity index 96% rename from localization/pose_instability_detector/test/test.cpp rename to localization/autoware_pose_instability_detector/test/test.cpp index 482e659e7a13c..bd663a2406903 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/autoware_pose_instability_detector/test/test.cpp @@ -36,7 +36,7 @@ class TestPoseInstabilityDetector : public ::testing::Test void SetUp() override { const std::string yaml_path = - ament_index_cpp::get_package_share_directory("pose_instability_detector") + + ament_index_cpp::get_package_share_directory("autoware_pose_instability_detector") + "/config/pose_instability_detector.param.yaml"; rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); @@ -53,7 +53,8 @@ class TestPoseInstabilityDetector : public ::testing::Test } } - subject_ = std::make_shared(node_options); + subject_ = + std::make_shared(node_options); executor_.add_node(subject_); helper_ = std::make_shared(); @@ -69,7 +70,7 @@ class TestPoseInstabilityDetector : public ::testing::Test } rclcpp::executors::SingleThreadedExecutor executor_; - std::shared_ptr subject_; + std::shared_ptr subject_; std::shared_ptr helper_; }; diff --git a/localization/pose_instability_detector/test/test_message_helper_node.hpp b/localization/autoware_pose_instability_detector/test/test_message_helper_node.hpp similarity index 100% rename from localization/pose_instability_detector/test/test_message_helper_node.hpp rename to localization/autoware_pose_instability_detector/test/test_message_helper_node.hpp diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 0b3f64caabe41..f3830c303ab48 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -56,43 +56,44 @@ class Simple1DFilter { initialized_ = false; x_ = 0; - dev_ = 1e9; - proc_dev_x_c_ = 0.0; + var_ = 1e9; + proc_var_x_c_ = 0.0; }; - void init(const double init_obs, const double obs_dev, const rclcpp::Time & time) + void init(const double init_obs, const double obs_var, const rclcpp::Time & time) { x_ = init_obs; - dev_ = obs_dev; + var_ = obs_var; latest_time_ = time; initialized_ = true; }; - void update(const double obs, const double obs_dev, const rclcpp::Time & time) + void update(const double obs, const double obs_var, const rclcpp::Time & time) { if (!initialized_) { - init(obs, obs_dev, time); + init(obs, obs_var, time); return; } - // Prediction step (current stddev_) + // Prediction step (current variance) double dt = (time - latest_time_).seconds(); - double proc_dev_x_d = proc_dev_x_c_ * dt * dt; - dev_ = dev_ + proc_dev_x_d; + double proc_var_x_d = proc_var_x_c_ * dt * dt; + var_ = var_ + proc_var_x_d; // Update step - double kalman_gain = dev_ / (dev_ + obs_dev); + double kalman_gain = var_ / (var_ + obs_var); x_ = x_ + kalman_gain * (obs - x_); - dev_ = (1 - kalman_gain) * dev_; + var_ = (1 - kalman_gain) * var_; latest_time_ = time; }; - void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } + void set_proc_var(const double proc_var) { proc_var_x_c_ = proc_var; } [[nodiscard]] double get_x() const { return x_; } + [[nodiscard]] double get_var() const { return var_; } private: bool initialized_; double x_; - double dev_; - double proc_dev_x_c_; + double var_; + double proc_var_x_c_; rclcpp::Time latest_time_; }; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 37880f4e4139a..707d5e585cb0c 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -100,9 +100,9 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) ekf_module_ = std::make_unique(warning_, params_); logger_configure_ = std::make_unique(this); - z_filter_.set_proc_dev(params_.z_filter_proc_dev); - roll_filter_.set_proc_dev(params_.roll_filter_proc_dev); - pitch_filter_.set_proc_dev(params_.pitch_filter_proc_dev); + z_filter_.set_proc_var(params_.z_filter_proc_dev * params_.z_filter_proc_dev); + roll_filter_.set_proc_var(params_.roll_filter_proc_dev * params_.roll_filter_proc_dev); + pitch_filter_.set_proc_var(params_.pitch_filter_proc_dev * params_.pitch_filter_proc_dev); } /* @@ -365,6 +365,12 @@ void EKFLocalizer::publish_estimate_result( pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance(); + + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + pose_cov.pose.covariance[COV_IDX::Z_Z] = z_filter_.get_var(); + pose_cov.pose.covariance[COV_IDX::ROLL_ROLL] = roll_filter_.get_var(); + pose_cov.pose.covariance[COV_IDX::PITCH_PITCH] = pitch_filter_.get_var(); + pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; @@ -460,14 +466,14 @@ void EKFLocalizer::update_simple_1d_filters( const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); - double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); - double pitch_dev = + double z_var = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); + double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); + double pitch_var = pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast(smoothing_step); - z_filter_.update(z, z_dev, pose.header.stamp); - roll_filter_.update(rpy.x, roll_dev, pose.header.stamp); - pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp); + z_filter_.update(z, z_var, pose.header.stamp); + roll_filter_.update(rpy.x, roll_var, pose.header.stamp); + pitch_filter_.update(rpy.y, pitch_var, pose.header.stamp); } void EKFLocalizer::init_simple_1d_filters( @@ -478,13 +484,13 @@ void EKFLocalizer::init_simple_1d_filters( const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; - double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL]; - double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; + double z_var = pose.pose.covariance[COV_IDX::Z_Z]; + double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL]; + double pitch_var = pose.pose.covariance[COV_IDX::PITCH_PITCH]; - z_filter_.init(z, z_dev, pose.header.stamp); - roll_filter_.init(rpy.x, roll_dev, pose.header.stamp); - pitch_filter_.init(rpy.y, pitch_dev, pose.header.stamp); + z_filter_.init(z, z_var, pose.header.stamp); + roll_filter_.init(rpy.x, roll_var, pose.header.stamp); + pitch_filter_.init(rpy.y, pitch_var, pose.header.stamp); } /** diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 74e3a35b0ee10..2928f51f7256d 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -7,7 +7,7 @@ It receives roughly estimated initial pose from GNSS/user. Passing the pose to `ndt_scan_matcher`, and it gets a calculated ego pose from `ndt_scan_matcher` via service. Finally, it publishes the initial pose to `ekf_localizer`. This node depends on the map height fitter library. -[See here for more details.](../../map/map_height_fitter/README.md) +[See here for more details.](../../map/autoware_map_height_fitter/README.md) ## Interfaces diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index ca4aa032fc7f9..e1f1233b8b4f5 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -19,13 +19,13 @@ ament_cmake autoware_cmake + autoware_map_height_fitter autoware_motion_utils autoware_universe_utils component_interface_specs component_interface_utils geometry_msgs localization_util - map_height_fitter rclcpp rclcpp_components std_srvs diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp index fd490b00d0f70..bb3f6933c2aaa 100644 --- a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp @@ -15,7 +15,7 @@ #ifndef POSE_INITIALIZER__GNSS_MODULE_HPP_ #define POSE_INITIALIZER__GNSS_MODULE_HPP_ -#include +#include #include #include @@ -32,7 +32,7 @@ class GnssModule private: void on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg); - map_height_fitter::MapHeightFitter fitter_; + autoware::map_height_fitter::MapHeightFitter fitter_; rclcpp::Clock::SharedPtr clock_; rclcpp::Subscription::SharedPtr sub_gnss_pose_; PoseWithCovarianceStamped::ConstSharedPtr pose_; diff --git a/map/map_height_fitter/CMakeLists.txt b/map/autoware_map_height_fitter/CMakeLists.txt similarity index 74% rename from map/map_height_fitter/CMakeLists.txt rename to map/autoware_map_height_fitter/CMakeLists.txt index 9485ed3fa54cd..5e26a25af3a1d 100644 --- a/map/map_height_fitter/CMakeLists.txt +++ b/map/autoware_map_height_fitter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(map_height_fitter) +project(autoware_map_height_fitter) find_package(autoware_cmake REQUIRED) find_package(PCL REQUIRED COMPONENTS common) @@ -9,14 +9,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/map_height_fitter.cpp src/map_height_fitter_node.cpp ) -target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) # When adding `autoware_lanelet2_extension` to package.xml, many warnings are generated. # These are treated as errors in compile, so pedantic warnings are disabled for this package. -target_compile_options(map_height_fitter PRIVATE -Wno-pedantic) +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-pedantic) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "MapHeightFitterNode" + PLUGIN "autoware::map_height_fitter::MapHeightFitterNode" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR MultiThreadedExecutor ) diff --git a/map/map_height_fitter/README.md b/map/autoware_map_height_fitter/README.md similarity index 87% rename from map/map_height_fitter/README.md rename to map/autoware_map_height_fitter/README.md index 7da70a0ff7766..a61760b142615 100644 --- a/map/map_height_fitter/README.md +++ b/map/autoware_map_height_fitter/README.md @@ -1,4 +1,4 @@ -# map_height_fitter +# autoware_map_height_fitter This library fits the given point with the ground of the point cloud map. The map loading operation is switched by the parameter `enable_partial_load` of the node specified by `map_loader_name`. @@ -6,7 +6,7 @@ The node using this library must use multi thread executor. ## Parameters -{{ json_to_markdown("map/map_height_fitter/schema/map_height_fitter.schema.json") }} +{{ json_to_markdown("map/autoware_map_height_fitter/schema/map_height_fitter.schema.json") }} ## Topic subscription diff --git a/map/map_height_fitter/config/map_height_fitter.param.yaml b/map/autoware_map_height_fitter/config/map_height_fitter.param.yaml similarity index 100% rename from map/map_height_fitter/config/map_height_fitter.param.yaml rename to map/autoware_map_height_fitter/config/map_height_fitter.param.yaml diff --git a/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp b/map/autoware_map_height_fitter/include/autoware/map_height_fitter/map_height_fitter.hpp similarity index 82% rename from map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp rename to map/autoware_map_height_fitter/include/autoware/map_height_fitter/map_height_fitter.hpp index 46baffa270108..e8ab0eef931a3 100644 --- a/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp +++ b/map/autoware_map_height_fitter/include/autoware/map_height_fitter/map_height_fitter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ -#define MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ +#ifndef AUTOWARE__MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ +#define AUTOWARE__MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace map_height_fitter +namespace autoware::map_height_fitter { using geometry_msgs::msg::Point; @@ -44,6 +44,6 @@ class MapHeightFitter final std::unique_ptr impl_; }; -} // namespace map_height_fitter +} // namespace autoware::map_height_fitter -#endif // MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ +#endif // AUTOWARE__MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ diff --git a/map/map_height_fitter/launch/map_height_fitter.launch.xml b/map/autoware_map_height_fitter/launch/map_height_fitter.launch.xml similarity index 60% rename from map/map_height_fitter/launch/map_height_fitter.launch.xml rename to map/autoware_map_height_fitter/launch/map_height_fitter.launch.xml index 3e01a35a8e519..fa613d102b496 100644 --- a/map/map_height_fitter/launch/map_height_fitter.launch.xml +++ b/map/autoware_map_height_fitter/launch/map_height_fitter.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/map/map_height_fitter/package.xml b/map/autoware_map_height_fitter/package.xml similarity index 93% rename from map/map_height_fitter/package.xml rename to map/autoware_map_height_fitter/package.xml index 36e5814365c3f..17f2e95edb159 100644 --- a/map/map_height_fitter/package.xml +++ b/map/autoware_map_height_fitter/package.xml @@ -1,9 +1,9 @@ - map_height_fitter + autoware_map_height_fitter 0.1.0 - The map_height_fitter package + The autoware_map_height_fitter package Takagi, Isamu Yamato Ando Masahiro Sakamoto diff --git a/map/map_height_fitter/schema/map_height_fitter.schema.json b/map/autoware_map_height_fitter/schema/map_height_fitter.schema.json similarity index 100% rename from map/map_height_fitter/schema/map_height_fitter.schema.json rename to map/autoware_map_height_fitter/schema/map_height_fitter.schema.json diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/autoware_map_height_fitter/src/map_height_fitter.cpp similarity index 98% rename from map/map_height_fitter/src/map_height_fitter.cpp rename to map/autoware_map_height_fitter/src/map_height_fitter.cpp index c9e199ad88422..732f13e375f05 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/autoware_map_height_fitter/src/map_height_fitter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "map_height_fitter/map_height_fitter.hpp" +#include "autoware/map_height_fitter/map_height_fitter.hpp" #include #include @@ -31,7 +31,7 @@ #include -namespace map_height_fitter +namespace autoware::map_height_fitter { struct MapHeightFitter::Impl @@ -285,4 +285,4 @@ std::optional MapHeightFitter::fit(const Point & position, const std::str return impl_->fit(position, frame); } -} // namespace map_height_fitter +} // namespace autoware::map_height_fitter diff --git a/map/map_height_fitter/src/map_height_fitter_node.cpp b/map/autoware_map_height_fitter/src/map_height_fitter_node.cpp similarity index 88% rename from map/map_height_fitter/src/map_height_fitter_node.cpp rename to map/autoware_map_height_fitter/src/map_height_fitter_node.cpp index fdc7604b68855..99e55c26094b7 100644 --- a/map/map_height_fitter/src/map_height_fitter_node.cpp +++ b/map/autoware_map_height_fitter/src/map_height_fitter_node.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "map_height_fitter/map_height_fitter.hpp" +#include "autoware/map_height_fitter/map_height_fitter.hpp" #include #include +namespace autoware::map_height_fitter +{ using tier4_localization_msgs::srv::PoseWithCovarianceStamped; class MapHeightFitterNode : public rclcpp::Node @@ -46,6 +48,7 @@ class MapHeightFitterNode : public rclcpp::Node map_height_fitter::MapHeightFitter fitter_; rclcpp::Service::SharedPtr srv_; }; +} // namespace autoware::map_height_fitter #include -RCLCPP_COMPONENTS_REGISTER_NODE(MapHeightFitterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_height_fitter::MapHeightFitterNode) diff --git a/map/map_tf_generator/CMakeLists.txt b/map/autoware_map_tf_generator/CMakeLists.txt similarity index 80% rename from map/map_tf_generator/CMakeLists.txt rename to map/autoware_map_tf_generator/CMakeLists.txt index e20289766cdda..eb67ea7330312 100644 --- a/map/map_tf_generator/CMakeLists.txt +++ b/map/autoware_map_tf_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(map_tf_generator) +project(autoware_map_tf_generator) find_package(autoware_cmake REQUIRED) autoware_package() @@ -18,8 +18,8 @@ ament_auto_add_library(pcd_map_tf_generator_node SHARED target_link_libraries(pcd_map_tf_generator_node ${PCL_LIBRARIES}) rclcpp_components_register_node(pcd_map_tf_generator_node - PLUGIN "PcdMapTFGeneratorNode" - EXECUTABLE pcd_map_tf_generator + PLUGIN "autoware::map_tf_generator::PcdMapTFGeneratorNode" + EXECUTABLE autoware_pcd_map_tf_generator ) ament_auto_add_library(vector_map_tf_generator_node SHARED @@ -27,8 +27,8 @@ ament_auto_add_library(vector_map_tf_generator_node SHARED ) rclcpp_components_register_node(vector_map_tf_generator_node - PLUGIN "VectorMapTFGeneratorNode" - EXECUTABLE vector_map_tf_generator + PLUGIN "autoware::map_tf_generator::VectorMapTFGeneratorNode" + EXECUTABLE autoware_vector_map_tf_generator ) if(BUILD_TESTING) @@ -37,7 +37,7 @@ if(BUILD_TESTING) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) - target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/map/map_tf_generator/README.md b/map/autoware_map_tf_generator/README.md similarity index 89% rename from map/map_tf_generator/README.md rename to map/autoware_map_tf_generator/README.md index eef36c34750ca..ed63e35e37b85 100644 --- a/map/map_tf_generator/README.md +++ b/map/autoware_map_tf_generator/README.md @@ -1,4 +1,4 @@ -# map_tf_generator +# autoware_map_tf_generator ## Purpose @@ -17,13 +17,13 @@ The following are the supported methods to calculate the position of the `viewer ### Input -#### pcd_map_tf_generator +#### autoware_pcd_map_tf_generator | Name | Type | Description | | --------------------- | ------------------------------- | ----------------------------------------------------------------- | | `/map/pointcloud_map` | `sensor_msgs::msg::PointCloud2` | Subscribe pointcloud map to calculate position of `viewer` frames | -#### vector_map_tf_generator +#### autoware_vector_map_tf_generator | Name | Type | Description | | ----------------- | --------------------------------------- | ------------------------------------------------------------- | @@ -43,7 +43,7 @@ None ### Core Parameters -{{ json_to_markdown("map/map_tf_generator/schema/map_tf_generator.schema.json") }} +{{ json_to_markdown("map/autoware_map_tf_generator/schema/map_tf_generator.schema.json") }} ## Assumptions / Known limits diff --git a/map/map_tf_generator/config/map_tf_generator.param.yaml b/map/autoware_map_tf_generator/config/map_tf_generator.param.yaml similarity index 100% rename from map/map_tf_generator/config/map_tf_generator.param.yaml rename to map/autoware_map_tf_generator/config/map_tf_generator.param.yaml diff --git a/map/autoware_map_tf_generator/launch/map_tf_generator.launch.xml b/map/autoware_map_tf_generator/launch/map_tf_generator.launch.xml new file mode 100644 index 0000000000000..00a90f10a17b0 --- /dev/null +++ b/map/autoware_map_tf_generator/launch/map_tf_generator.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/map/map_tf_generator/package.xml b/map/autoware_map_tf_generator/package.xml similarity index 97% rename from map/map_tf_generator/package.xml rename to map/autoware_map_tf_generator/package.xml index 41c3364358f74..eff7ec88adfa0 100644 --- a/map/map_tf_generator/package.xml +++ b/map/autoware_map_tf_generator/package.xml @@ -1,7 +1,7 @@ - map_tf_generator + autoware_map_tf_generator 0.1.0 map_tf_generator package as a ROS 2 node Yamato Ando diff --git a/map/map_tf_generator/schema/map_tf_generator.schema.json b/map/autoware_map_tf_generator/schema/map_tf_generator.schema.json similarity index 100% rename from map/map_tf_generator/schema/map_tf_generator.schema.json rename to map/autoware_map_tf_generator/schema/map_tf_generator.schema.json diff --git a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp b/map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp similarity index 95% rename from map/map_tf_generator/src/pcd_map_tf_generator_node.cpp rename to map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp index d4b8ccb797224..e6edbe99dd210 100644 --- a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp +++ b/map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "map_tf_generator/uniform_random.hpp" +#include "uniform_random.hpp" #include @@ -28,6 +28,8 @@ #include #include +namespace autoware::map_tf_generator +{ constexpr size_t n_samples = 20; class PcdMapTFGeneratorNode : public rclcpp::Node @@ -95,6 +97,7 @@ class PcdMapTFGeneratorNode : public rclcpp::Node << ", y:" << coordinate[1] << ", z:" << coordinate[2]); } }; +} // namespace autoware::map_tf_generator #include -RCLCPP_COMPONENTS_REGISTER_NODE(PcdMapTFGeneratorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_tf_generator::PcdMapTFGeneratorNode) diff --git a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp b/map/autoware_map_tf_generator/src/uniform_random.hpp similarity index 84% rename from map/map_tf_generator/include/map_tf_generator/uniform_random.hpp rename to map/autoware_map_tf_generator/src/uniform_random.hpp index cda3bc5c49a0c..73de77458d09d 100644 --- a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp +++ b/map/autoware_map_tf_generator/src/uniform_random.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_ -#define MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_ +#ifndef UNIFORM_RANDOM_HPP_ +#define UNIFORM_RANDOM_HPP_ #include #include +namespace autoware::map_tf_generator +{ std::vector inline uniform_random(const size_t max_exclusive, const size_t n) { std::default_random_engine generator; @@ -29,5 +31,6 @@ std::vector inline uniform_random(const size_t max_exclusive, const size } return v; } +} // namespace autoware::map_tf_generator -#endif // MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_ +#endif // UNIFORM_RANDOM_HPP_ diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp similarity index 95% rename from map/map_tf_generator/src/vector_map_tf_generator_node.cpp rename to map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp index 093b42037ed72..e4c397bd04cf1 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -25,6 +25,8 @@ #include #include +namespace autoware::map_tf_generator +{ class VectorMapTFGeneratorNode : public rclcpp::Node { public: @@ -93,6 +95,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node << ", y:" << coordinate_y << ", z:" << coordinate_z); } }; +} // namespace autoware::map_tf_generator #include -RCLCPP_COMPONENTS_REGISTER_NODE(VectorMapTFGeneratorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_tf_generator::VectorMapTFGeneratorNode) diff --git a/map/map_tf_generator/test/test_uniform_random.cpp b/map/autoware_map_tf_generator/test/test_uniform_random.cpp similarity index 85% rename from map/map_tf_generator/test/test_uniform_random.cpp rename to map/autoware_map_tf_generator/test/test_uniform_random.cpp index 19e558ebb2add..481f5c6d86859 100644 --- a/map/map_tf_generator/test/test_uniform_random.cpp +++ b/map/autoware_map_tf_generator/test/test_uniform_random.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "map_tf_generator/uniform_random.hpp" +#include "uniform_random.hpp" #include @@ -24,7 +24,7 @@ using testing::Lt; TEST(uniform_random, uniform_random) { { - const std::vector random = uniform_random(4, 0); + const std::vector random = autoware::map_tf_generator::uniform_random(4, 0); ASSERT_EQ(random.size(), static_cast(0)); } @@ -35,7 +35,7 @@ TEST(uniform_random, uniform_random) const size_t max_exclusive = 4; for (int i = 0; i < 50; i++) { - const std::vector random = uniform_random(4, 10); + const std::vector random = autoware::map_tf_generator::uniform_random(4, 10); ASSERT_EQ(random.size(), 10U); ASSERT_THAT(random, Each(AllOf(Ge(min_inclusive), Lt(max_exclusive)))); // in range [0, 4) } diff --git a/map/map_tf_generator/launch/map_tf_generator.launch.xml b/map/map_tf_generator/launch/map_tf_generator.launch.xml deleted file mode 100644 index 197085f31d9c4..0000000000000 --- a/map/map_tf_generator/launch/map_tf_generator.launch.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp index 4e3728be53171..195e09e4b406e 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp @@ -31,15 +31,6 @@ VoxelGridMapLoader::VoxelGridMapLoader( debug_ = node->declare_parameter("publish_debug_pcd"); } -bool VoxelGridMapLoader::is_close_points( - const pcl::PointXYZ point, const pcl::PointXYZ target_point, const double distance_threshold) -{ - if (distance3D(point, target_point) < distance_threshold * distance_threshold) { - return true; - } - return false; -} - void VoxelGridMapLoader::publish_downsampled_map( const pcl::PointCloud & downsampled_pc) { diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 6560ae82f8bce..1bcdff228f40b 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -88,7 +88,6 @@ class VoxelGridEx : public pcl::VoxelGrid inline Eigen::Vector4i get_max_b() const { return max_b_; } inline Eigen::Vector4i get_div_b() const { return div_b_; } inline Eigen::Array4f get_inverse_leaf_size() const { return inverse_leaf_size_; } - inline std::vector getLeafLayout() { return (leaf_layout_); } }; class VoxelGridMapLoader @@ -122,8 +121,6 @@ class VoxelGridMapLoader const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const; void publish_downsampled_map(const pcl::PointCloud & downsampled_pc); - static bool is_close_points( - const pcl::PointXYZ point, const pcl::PointXYZ target_point, const double distance_threshold); std::string * tf_map_input_frame_; }; diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index 94ef16042ff33..e87fd0c341a6f 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -143,11 +143,6 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt float getMinHeight() const { return height_min; } - uint16_t getGridId() const { return grid_id; } - - pcl::PointIndices getIndices() const { return pcl_indices; } - std::vector getHeightList() const { return height_list; } - pcl::PointIndices & getIndicesRef() { return pcl_indices; } std::vector & getHeightListRef() { return height_list; } }; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 9a0f4838d1fac..058a5994ab8ad 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -74,9 +74,6 @@ std::optional getTransformStamped( Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); -void convertCluster2FeatureObject( - const std_msgs::msg::Header & header, const PointCloud & cluster, - DetectedObjectWithFeature & feature_obj); void closest_cluster( const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, const pcl::PointXYZ & center, PointCloud2 & out_cluster); @@ -90,11 +87,6 @@ void updateOutputFusedObjects( geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); -pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster); -void addShapeAndKinematic( - const pcl::PointCloud & cluster, - tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); - } // namespace autoware::image_projection_based_fusion #endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 1b303e3f01ff5..1f67d2e1cc86f 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -72,18 +72,6 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) return a; } -void convertCluster2FeatureObject( - const std_msgs::msg::Header & header, const PointCloud & cluster, - DetectedObjectWithFeature & feature_obj) -{ - PointCloud2 ros_cluster; - pcl::toROSMsg(cluster, ros_cluster); - ros_cluster.header = header; - feature_obj.feature.cluster = ros_cluster; - feature_obj.object.kinematics.pose_with_covariance.pose.position = getCentroid(ros_cluster); - feature_obj.object.existence_probability = 1.0f; -} - void closest_cluster( const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, const pcl::PointXYZ & center, PointCloud2 & out_cluster) @@ -198,73 +186,6 @@ void updateOutputFusedObjects( } } -void addShapeAndKinematic( - const pcl::PointCloud & cluster, - tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj) -{ - if (cluster.empty()) { - return; - } - pcl::PointXYZ centroid = pcl::PointXYZ(0.0, 0.0, 0.0); - float max_z = -1e6; - float min_z = 1e6; - for (const auto & point : cluster) { - centroid.x += point.x; - centroid.y += point.y; - centroid.z += point.z; - max_z = max_z < point.z ? point.z : max_z; - min_z = min_z > point.z ? point.z : min_z; - } - centroid.x = centroid.x / static_cast(cluster.size()); - centroid.y = centroid.y / static_cast(cluster.size()); - centroid.z = centroid.z / static_cast(cluster.size()); - - std::vector cluster2d; - std::vector cluster2d_convex; - - for (size_t i = 0; i < cluster.size(); ++i) { - cluster2d.push_back( - cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.)); - } - cv::convexHull(cluster2d, cluster2d_convex); - if (cluster2d_convex.empty()) { - return; - } - pcl::PointXYZ polygon_centroid = pcl::PointXYZ(0.0, 0.0, 0.0); - for (size_t i = 0; i < cluster2d_convex.size(); ++i) { - polygon_centroid.x += static_cast(cluster2d_convex.at(i).x) / 1000.0; - polygon_centroid.y += static_cast(cluster2d_convex.at(i).y) / 1000.0; - } - polygon_centroid.x = polygon_centroid.x / static_cast(cluster2d_convex.size()); - polygon_centroid.y = polygon_centroid.y / static_cast(cluster2d_convex.size()); - - autoware_perception_msgs::msg::Shape shape; - for (size_t i = 0; i < cluster2d_convex.size(); ++i) { - geometry_msgs::msg::Point32 point; - point.x = cluster2d_convex.at(i).x / 1000.0; - point.y = cluster2d_convex.at(i).y / 1000.0; - point.z = 0.0; - shape.footprint.points.push_back(point); - } - shape.type = autoware_perception_msgs::msg::Shape::POLYGON; - constexpr float eps = 0.01; - shape.dimensions.x = 0; - shape.dimensions.y = 0; - shape.dimensions.z = std::max((max_z - min_z), eps); - feature_obj.object.shape = shape; - feature_obj.object.kinematics.pose_with_covariance.pose.position.x = - centroid.x + polygon_centroid.x; - feature_obj.object.kinematics.pose_with_covariance.pose.position.y = - centroid.y + polygon_centroid.y; - feature_obj.object.kinematics.pose_with_covariance.pose.position.z = - min_z + shape.dimensions.z * 0.5; - feature_obj.object.existence_probability = 1.0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.x = 0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.y = 0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.z = 0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.w = 1; -} - geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) { geometry_msgs::msg::Point centroid; @@ -285,20 +206,4 @@ geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & poin return centroid; } -pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) -{ - pcl::PointXYZ closest_point; - double min_dist = 1e6; - pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); - for (std::size_t i = 0; i < cluster.points.size(); ++i) { - pcl::PointXYZ point = cluster.points.at(i); - double dist_closest_point = autoware::universe_utils::calcDistance2d(point, orig_point); - if (min_dist > dist_closest_point) { - min_dist = dist_closest_point; - closest_point = pcl::PointXYZ(point.x, point.y, point.z); - } - } - return closest_point; -} - } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_lidar_centerpoint/lib/utils.cpp b/perception/autoware_lidar_centerpoint/lib/utils.cpp index a9e7a68f6adaa..0abdf0f452fe4 100644 --- a/perception/autoware_lidar_centerpoint/lib/utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/utils.cpp @@ -19,7 +19,7 @@ namespace autoware::lidar_centerpoint { // cspell: ignore divup -std::size_t divup(const std::size_t a, const std::size_t b) +std::size_t divup(const std::size_t a, const std::size_t b) // cppcheck-suppress unusedFunction { if (a == 0) { throw std::runtime_error("A dividend of divup isn't positive."); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 5172cc5062450..decd2139c5b81 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -46,17 +46,6 @@ void InputStream::init(const InputChannel & input_channel) latest_message_time_ = node_.now(); } -bool InputStream::getTimestamps( - rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const -{ - if (!isTimeInitialized()) { - return false; - } - latest_measurement_time = latest_measurement_time_; - latest_message_time = latest_message_time_; - return true; -} - void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index ee2c6910159b5..f8b221f733faa 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -46,7 +46,6 @@ class InputStream void init(const InputChannel & input_channel); - void setQueueSize(size_t que_size) { que_size_ = que_size; } void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; @@ -60,15 +59,8 @@ class InputStream void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, ObjectsList & objects_list); - void getNames(std::string & long_name, std::string & short_name) const - { - long_name = long_name_; - short_name = short_name_; - } bool isSpawnEnabled() const { return is_spawn_enabled_; } - std::string getLongName() const { return long_name_; } - size_t getObjectsCount() const { return objects_que_.size(); } void getTimeStatistics( double & latency_mean, double & latency_var, double & interval_mean, double & interval_var) const @@ -78,13 +70,6 @@ class InputStream interval_mean = interval_mean_; interval_var = interval_var_; } - void getLatencyStatistics(double & latency_mean, double & latency_var) const - { - latency_mean = latency_mean_; - latency_var = latency_var_; - } - bool getTimestamps( - rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const; rclcpp::Time getLatestMeasurementTime() const { return latest_measurement_time_; } private: diff --git a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp index 381fa79ed090c..fec26b762dfe9 100644 --- a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp @@ -49,11 +49,6 @@ float calWeightedIou( namespace autoware::traffic_light { -inline std::vector toFloatVector(const std::vector & double_vector) -{ - return std::vector(double_vector.begin(), double_vector.end()); -} - TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOptions & options) : Node("traffic_light_fine_detector_node", options) { diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index db48154ffa214..56e096944e739 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -82,9 +82,11 @@ None | `use_back` | bool | whether using backward trajectory | | `adapt_expansion_distance` | bool | if true, adapt expansion distance based on environment | | `expansion_distance` | double | length of expansion for node transitions | +| `near_goal_distance` | double | near goal distance threshold | | `distance_heuristic_weight` | double | heuristic weight for estimating node's cost | | `smoothness_weight` | double | cost factor for change in curvature | | `obstacle_distance_weight` | double | cost factor for distance to obstacle | +| `goal_lat_distance_weight` | double | cost factor for lateral distance from goal | #### RRT\* search parameters diff --git a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml index 6ea99b2c0c061..b34ffe698aa36 100644 --- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml +++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml @@ -22,7 +22,7 @@ theta_size: 144 angle_goal_range: 6.0 lateral_goal_range: 0.5 - longitudinal_goal_range: 2.0 + longitudinal_goal_range: 1.0 curve_weight: 0.5 reverse_weight: 1.0 direction_change_weight: 1.5 @@ -36,9 +36,11 @@ use_back: true adapt_expansion_distance: true expansion_distance: 0.5 + near_goal_distance: 4.0 distance_heuristic_weight: 1.5 smoothness_weight: 0.5 obstacle_distance_weight: 1.5 + goal_lat_distance_weight: 5.0 # -- RRT* search Configurations -- rrtstar: diff --git a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json index 4494878849897..e42995718974a 100644 --- a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json +++ b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json @@ -146,6 +146,11 @@ "default": 0.5, "description": "Distance for expanding nodes in A* search." }, + "near_goal_distance": { + "type": "number", + "default": 4.0, + "description": "Distance threshold to consider near goal." + }, "distance_heuristic_weight": { "type": "number", "default": 1.0, @@ -160,6 +165,11 @@ "type": "number", "default": 0.5, "description": "Weight for distance to obstacle in A* search." + }, + "goal_lat_distance_weight": { + "type": "number", + "default": 0.5, + "description": "Weight for lateral distance from original goal." } }, "required": [ diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index 9bff2cfddd2c6..8e8a962ed1071 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -377,8 +377,7 @@ void FreespacePlannerNode::updateTargetIndex() } else { // Switch to next partial trajectory prev_target_index_ = target_index_; - target_index_ = - getNextTargetIndex(trajectory_.points.size(), reversing_indices_, target_index_); + target_index_ = new_target_index; } } } diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp index 911144f3796e5..8a4d6bf2e42b5 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ #include +#include #include #include @@ -29,6 +30,10 @@ namespace autoware::freespace_planning_algorithms { +using autoware::universe_utils::normalizeRadian; + +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); int discretizeAngle(const double theta, const int theta_size); struct IndexXYT @@ -36,6 +41,11 @@ struct IndexXYT int x; int y; int theta; + + bool operator==(const IndexXYT & index) const + { + return (x == index.x && y == index.y && theta == index.theta); + } }; struct IndexXY @@ -138,6 +148,12 @@ struct PlannerWaypoints double compute_length() const; }; +struct EDTData +{ + double distance; + double angle; +}; + class AbstractPlanningAlgorithm { public: @@ -162,6 +178,9 @@ class AbstractPlanningAlgorithm virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap); virtual bool makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0; + virtual bool makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) = 0; virtual bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } double getDistanceToObstacle(const geometry_msgs::msg::Pose & pose) const; @@ -223,7 +242,7 @@ class AbstractPlanningAlgorithm } template - inline double getObstacleEDT(const IndexType & index) const + inline EDTData getObstacleEDT(const IndexType & index) const { return edt_map_[indexToId(index)]; } @@ -235,6 +254,19 @@ class AbstractPlanningAlgorithm return index.y * costmap_.info.width + index.x; } + inline double getVehicleBaseToFrameDistance(const double angle) const + { + const double normalized_angle = std::abs(normalizeRadian(angle)); + const double w = 0.5 * collision_vehicle_shape_.width; + const double l_b = collision_vehicle_shape_.base2back; + const double l_f = collision_vehicle_shape_.length - l_b; + + if (normalized_angle < atan(w / l_f)) return l_f / cos(normalized_angle); + if (normalized_angle < M_PI_2) return w / sin(normalized_angle); + if (normalized_angle < M_PI_2 + atan(l_b / w)) return w / cos(normalized_angle - M_PI_2); + return l_b / cos(M_PI - normalized_angle); + } + PlannerCommonParam planner_common_param_; VehicleShape collision_vehicle_shape_; @@ -250,8 +282,8 @@ class AbstractPlanningAlgorithm // is_obstacle's table std::vector is_obstacle_table_; - // Euclidean distance transform map (distance to nearest obstacle cell) - std::vector edt_map_; + // Euclidean distance transform map (distance & angle info to nearest obstacle cell) + std::vector edt_map_; // pose in costmap frame geometry_msgs::msg::Pose start_pose_; diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp index 8ac7c60e7ca4f..65ef53d820cba 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp @@ -23,6 +23,8 @@ #include #include +#include + #include #include #include @@ -46,11 +48,13 @@ struct AstarParam bool use_back; // backward search bool adapt_expansion_distance; double expansion_distance; + double near_goal_distance; // search configs double distance_heuristic_weight; // obstacle threshold on grid [0,255] double smoothness_weight; double obstacle_distance_weight; + double goal_lat_distance_weight; }; struct AstarNode @@ -105,15 +109,19 @@ class AstarSearch : public AbstractPlanningAlgorithm node.declare_parameter("astar.use_back"), node.declare_parameter("astar.adapt_expansion_distance"), node.declare_parameter("astar.expansion_distance"), + node.declare_parameter("astar.near_goal_distance"), node.declare_parameter("astar.distance_heuristic_weight"), node.declare_parameter("astar.smoothness_weight"), - node.declare_parameter("astar.obstacle_distance_weight")}) + node.declare_parameter("astar.obstacle_distance_weight"), + node.declare_parameter("astar.goal_lat_distance_weight")}) { } void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override; bool makePlan(const Pose & start_pose, const Pose & goal_pose) override; + bool makePlan(const Pose & start_pose, const std::vector & goal_candidates) override; + const PlannerWaypoints & getWaypoints() const { return waypoints_; } inline int getKey(const IndexXYT & index) @@ -127,16 +135,18 @@ class AstarSearch : public AbstractPlanningAlgorithm void expandNodes(AstarNode & current_node, const bool is_back = false); void resetData(); void setPath(const AstarNode & goal); - void setStartNode(); + void setStartNode(const double cost_offset = 0.0); double estimateCost(const Pose & pose, const IndexXYT & index) const; bool isGoal(const AstarNode & node) const; + void setShiftedGoalPose(const Pose & goal_pose, const double lat_offset) const; Pose node2pose(const AstarNode & node) const; double getExpansionDistance(const AstarNode & current_node) const; double getSteeringCost(const int steering_index) const; double getSteeringChangeCost(const int steering_index, const int prev_steering_index) const; double getDirectionChangeCost(const double dir_distance) const; - double getObsDistanceCost(const double obs_distance) const; + double getObsDistanceCost(const IndexXYT & index, const EDTData & obs_edt) const; + double getLatDistanceCost(const Pose & pose) const; // Algorithm specific param AstarParam astar_param_; @@ -150,6 +160,11 @@ class AstarSearch : public AbstractPlanningAlgorithm // goal node, which may helpful in testing and debugging AstarNode * goal_node_; + mutable boost::optional shifted_goal_pose_; + + // alternate goals for when multiple goal candidates are given + std::vector alternate_goals_; + // distance metric option (removed when the reeds_shepp gets stable) bool use_reeds_shepp_; @@ -158,7 +173,9 @@ class AstarSearch : public AbstractPlanningAlgorithm double avg_turning_radius_; double min_expansion_dist_; double max_expansion_dist_; + double near_goal_dist_; bool is_backward_search_; + bool is_multiple_goals_; // the following constexpr values were found to be best by trial and error, through multiple // tests, and are not expected to be changed regularly, therefore they were not made into ros @@ -169,8 +186,11 @@ class AstarSearch : public AbstractPlanningAlgorithm static constexpr double dist_to_goal_expansion_factor_ = 0.15; static constexpr double dist_to_obs_expansion_factor_ = 0.3; + // initial cost offset for multi goal backward search + static constexpr double multi_goal_backward_cost_offset = 5.0; + // cost free obstacle distance - static constexpr double cost_free_obs_dist = 5.0; + static constexpr double cost_free_obs_dist = 1.0; }; } // namespace autoware::freespace_planning_algorithms diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp index 17e7f1f1ebf56..b30692ce8da2d 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp @@ -58,6 +58,9 @@ class RRTStar : public AbstractPlanningAlgorithm bool makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) override; + bool makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) override; bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const override; private: diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index 1c04fd65447b2..c35081bd97f84 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -120,6 +120,8 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) &freespace_planning_algorithms::AstarParam::adapt_expansion_distance) .def_readwrite( "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance) + .def_readwrite( + "near_goal_distance", &freespace_planning_algorithms::AstarParam::near_goal_distance) .def_readwrite( "distance_heuristic_weight", &freespace_planning_algorithms::AstarParam::distance_heuristic_weight) @@ -127,7 +129,10 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight) .def_readwrite( "obstacle_distance_weight", - &freespace_planning_algorithms::AstarParam::obstacle_distance_weight); + &freespace_planning_algorithms::AstarParam::obstacle_distance_weight) + .def_readwrite( + "goal_lat_distance_weight", + &freespace_planning_algorithms::AstarParam::goal_lat_distance_weight); auto pyPlannerCommonParam = py::class_( p, "PlannerCommonParam", py::dynamic_attr()) diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py index b4af9ed87a3b9..9f18dc799c9d0 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py +++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py @@ -49,9 +49,11 @@ astar_param.use_back = True astar_param.adapt_expansion_distance = True astar_param.expansion_distance = 0.4 +astar_param.near_goal_distance = 3.0 astar_param.distance_heuristic_weight = 1.0 astar_param.smoothness_weight = 1.0 astar_param.obstacle_distance_weight = 1.0 +astar_param.goal_lat_distance_weight = 1.0 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param) diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index 97fd93e31d5cd..b968e70bd2c01 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -19,12 +19,12 @@ #include #include +#include #include namespace autoware::freespace_planning_algorithms { using autoware::universe_utils::createQuaternionFromYaw; -using autoware::universe_utils::normalizeRadian; geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) @@ -145,19 +145,19 @@ double AbstractPlanningAlgorithm::getDistanceToObstacle(const geometry_msgs::msg if (indexToId(index) >= static_cast(edt_map_.size())) { return std::numeric_limits::max(); } - return getObstacleEDT(index); + return getObstacleEDT(index).distance; } void AbstractPlanningAlgorithm::computeEDTMap() { + edt_map_.clear(); const int height = costmap_.info.height; const int width = costmap_.info.width; const double resolution_m = costmap_.info.resolution; - const double diagonal_m = std::hypot(resolution_m * height, resolution_m * width); - std::vector edt_map; + std::vector> edt_map; edt_map.reserve(costmap_.data.size()); - std::vector temporary_storage(width); + std::vector> temporary_storage(width); // scan rows for (int i = 0; i < height; ++i) { double distance = resolution_m; @@ -165,14 +165,16 @@ void AbstractPlanningAlgorithm::computeEDTMap() // forward scan for (int j = 0; j < width; ++j) { if (isObs(IndexXY{j, i})) { - temporary_storage[j] = 0.0; + temporary_storage[j].first = 0.0; + temporary_storage[j].second.x = 0.0; distance = resolution_m; found_obstacle = true; } else if (found_obstacle) { - temporary_storage[j] = distance; + temporary_storage[j].first = distance; + temporary_storage[j].second.x = -distance; distance += resolution_m; } else { - temporary_storage[j] = diagonal_m; + temporary_storage[j].first = std::numeric_limits::infinity(); } } @@ -183,8 +185,9 @@ void AbstractPlanningAlgorithm::computeEDTMap() if (isObs(IndexXY{j, i})) { distance = resolution_m; found_obstacle = true; - } else if (found_obstacle && temporary_storage[j] > distance) { - temporary_storage[j] = distance; + } else if (found_obstacle && temporary_storage[j].first > distance) { + temporary_storage[j].first = distance; + temporary_storage[j].second.x = distance; distance += resolution_m; } } @@ -197,22 +200,29 @@ void AbstractPlanningAlgorithm::computeEDTMap() for (int j = 0; j < width; ++j) { for (int i = 0; i < height; ++i) { int id = indexToId(IndexXY{j, i}); - double min_value = edt_map[id] * edt_map[id]; + double min_value = edt_map[id].first * edt_map[id].first; + geometry_msgs::msg::Point rel_pos = edt_map[id].second; for (int k = 0; k < height; ++k) { id = indexToId(IndexXY{j, k}); double dist = resolution_m * std::abs(static_cast(i - k)); - double value = edt_map[id] * edt_map[id] + dist * dist; + double value = edt_map[id].first * edt_map[id].first + dist * dist; if (value < min_value) { min_value = value; + rel_pos.x = edt_map[id].second.x; + rel_pos.y = dist; } } - temporary_storage[i] = sqrt(min_value); + temporary_storage[i].first = sqrt(min_value); + temporary_storage[i].second = rel_pos; } for (int i = 0; i < height; ++i) { edt_map[indexToId(IndexXY{j, i})] = temporary_storage[i]; } } - edt_map_ = edt_map; + for (const auto & edt : edt_map) { + const double angle = std::atan2(edt.second.y, edt.second.x); + edt_map_.push_back({edt.first, angle}); + } } void AbstractPlanningAlgorithm::computeCollisionIndexes( @@ -312,7 +322,7 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con if (detectBoundaryExit(base_index)) return true; - double obstacle_edt = getObstacleEDT(base_index); + double obstacle_edt = getObstacleEDT(base_index).distance; // if nearest obstacle is further than largest dimension, no collision is guaranteed // if nearest obstacle is closer than smallest dimension, collision is guaranteed diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index b0be1da75caec..805f375af4276 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -14,11 +14,13 @@ #include "autoware/freespace_planning_algorithms/astar_search.hpp" +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" #include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" #include #include +#include #include #include @@ -88,6 +90,9 @@ AstarSearch::AstarSearch( min_expansion_dist_ = astar_param_.expansion_distance; max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_; + + near_goal_dist_ = + std::max(astar_param.near_goal_distance, planner_common_param.longitudinal_goal_range); } void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) @@ -100,6 +105,18 @@ void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) collision_vehicle_shape_.base_length * base_length_max_expansion_factor_, min_expansion_dist_); } +void AstarSearch::resetData() +{ + // clearing openlist is necessary because otherwise remaining elements of openlist + // point to deleted node. + openlist_ = std::priority_queue, NodeComparison>(); + const int nb_of_grid_nodes = costmap_.info.width * costmap_.info.height; + const int total_astar_node_count = nb_of_grid_nodes * planner_common_param_.theta_size; + graph_.assign(total_astar_node_count, AstarNode{}); + col_free_distance_map_.assign(nb_of_grid_nodes, std::numeric_limits::max()); + shifted_goal_pose_ = {}; +} + bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) { resetData(); @@ -115,6 +132,8 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) setCollisionFreeDistanceMap(); + is_multiple_goals_ = false; + setStartNode(); if (!search()) { @@ -124,16 +143,54 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) return true; } -void AstarSearch::resetData() +bool AstarSearch::makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) { - // clearing openlist is necessary because otherwise remaining elements of openlist - // point to deleted node. - openlist_ = std::priority_queue, NodeComparison>(); - const int nb_of_grid_nodes = costmap_.info.width * costmap_.info.height; - const int total_astar_node_count = nb_of_grid_nodes * planner_common_param_.theta_size; - graph_ = std::vector(total_astar_node_count); - col_free_distance_map_ = - std::vector(nb_of_grid_nodes, std::numeric_limits::max()); + if (goal_candidates.empty()) return false; + + if (goal_candidates.size() == 1) { + return makePlan(start_pose, goal_candidates.front()); + } + + resetData(); + + start_pose_ = global2local(costmap_, start_pose); + + std::vector goals_local; + for (const auto & goal : goal_candidates) { + const auto goal_local = global2local(costmap_, goal); + if (detectCollision(goal_local)) continue; + goals_local.push_back(goal_local); + } + + if (detectCollision(start_pose_) || goals_local.empty()) { + throw std::logic_error("Invalid start or goal pose"); + } + + goal_pose_ = is_backward_search_ ? start_pose_ : goals_local.front(); + + setCollisionFreeDistanceMap(); + + is_multiple_goals_ = true; + + if (is_backward_search_) { + double cost_offset = 0.0; + for (const auto & pose : goals_local) { + start_pose_ = pose; + setStartNode(cost_offset); + cost_offset += multi_goal_backward_cost_offset; + } + } else { + setStartNode(); + alternate_goals_ = goals_local; + } + + if (!search()) { + throw std::logic_error("HA* failed to find path to goal"); + } + + return true; } void AstarSearch::setCollisionFreeDistanceMap() @@ -166,7 +223,7 @@ void AstarSearch::setCollisionFreeDistanceMap() const IndexXY n_index{x, y}; const double offset = std::abs(offset_x) + std::abs(offset_y); if (isOutOfRange(n_index) || isObs(n_index) || offset < 1) continue; - if (getObstacleEDT(n_index) < 0.5 * collision_vehicle_shape_.width) continue; + if (getObstacleEDT(n_index).distance < 0.5 * collision_vehicle_shape_.width) continue; const int n_id = indexToId(n_index); const double dist = current.second + (sqrt(offset) * costmap_.info.resolution); if (closed[n_id] || col_free_distance_map_[n_id] < dist) continue; @@ -177,15 +234,16 @@ void AstarSearch::setCollisionFreeDistanceMap() } } -void AstarSearch::setStartNode() +void AstarSearch::setStartNode(const double cost_offset) { const auto index = pose2index(costmap_, start_pose_, planner_common_param_.theta_size); // Set start node AstarNode * start_node = &graph_[getKey(index)]; - start_node->set(start_pose_, 0.0, estimateCost(start_pose_, index), 0, false); + const double initial_cost = estimateCost(start_pose_, index) + cost_offset; + start_node->set(start_pose_, 0.0, initial_cost, 0, false); start_node->dir_distance = 0.0; start_node->dist_to_goal = calcDistance2d(start_pose_, goal_pose_); - start_node->dist_to_obs = getObstacleEDT(index); + start_node->dist_to_obs = getObstacleEDT(index).distance; start_node->status = NodeStatus::Open; start_node->parent = nullptr; @@ -261,7 +319,7 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) AstarNode * next_node = &graph_[getKey(next_index)]; if (next_node->status == NodeStatus::Closed || detectCollision(next_index)) continue; - const double distance_to_obs = getObstacleEDT(next_index); + const auto obs_edt = getObstacleEDT(next_index); const bool is_direction_switch = (current_node.parent != nullptr) && (is_back != current_node.is_back); @@ -271,7 +329,8 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) double move_cost = current_node.gc + (total_weight * std::abs(distance)); move_cost += getSteeringChangeCost(steering_index, current_node.steering_index); - move_cost += getObsDistanceCost(distance_to_obs); + move_cost += getObsDistanceCost(next_index, obs_edt); + move_cost += getLatDistanceCost(next_pose); if (is_direction_switch) move_cost += getDirectionChangeCost(current_node.dir_distance); double total_cost = move_cost + estimateCost(next_pose, next_index); @@ -282,7 +341,7 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) next_node->dir_distance = std::abs(distance) + (is_direction_switch ? 0.0 : current_node.dir_distance); next_node->dist_to_goal = calcDistance2d(next_pose, goal_pose_); - next_node->dist_to_obs = distance_to_obs; + next_node->dist_to_obs = obs_edt.distance; next_node->parent = ¤t_node; openlist_.push(next_node); continue; @@ -320,10 +379,26 @@ double AstarSearch::getDirectionChangeCost(const double dir_distance) const return planner_common_param_.direction_change_weight * (1.0 + (1.0 / (1.0 + dir_distance))); } -double AstarSearch::getObsDistanceCost(const double obs_distance) const +double AstarSearch::getObsDistanceCost(const IndexXYT & index, const EDTData & obs_edt) const { + if (obs_edt.distance > collision_vehicle_shape_.max_dimension + cost_free_obs_dist) { + return 0.0; + } + const double yaw = index.theta * (2.0 * M_PI / planner_common_param_.theta_size); + const double base_to_frame_dist = getVehicleBaseToFrameDistance(yaw - obs_edt.angle); + const double vehicle_to_obs_dist = std::max(obs_edt.distance - base_to_frame_dist, 0.0); return astar_param_.obstacle_distance_weight * - std::max(1.0 - (obs_distance / cost_free_obs_dist), 0.0); + std::max(1.0 - (vehicle_to_obs_dist / cost_free_obs_dist), 0.0); +} + +double AstarSearch::getLatDistanceCost(const Pose & pose) const +{ + if (is_multiple_goals_) return 0.0; + const auto ref_pose = is_backward_search_ ? start_pose_ : goal_pose_; + const double distance_to_goal = calcDistance2d(pose, ref_pose); + if (distance_to_goal > near_goal_dist_) return 0.0; + const double lat_distance = std::abs(calcRelativePose(ref_pose, pose).position.y); + return astar_param_.goal_lat_distance_weight * lat_distance; } void AstarSearch::setPath(const AstarNode & goal_node) @@ -340,6 +415,11 @@ void AstarSearch::setPath(const AstarNode & goal_node) geometry_msgs::msg::PoseStamped pose; pose.header = header; + if (shifted_goal_pose_) { + pose.pose = local2global(costmap_, shifted_goal_pose_.get()); + waypoints.push_back({pose, goal_node.is_back}); + } + const auto interpolate = [this, &waypoints, &pose](const AstarNode & node) { if (node.parent == nullptr || !astar_param_.adapt_expansion_distance) return; const auto parent_pose = node2pose(*node.parent); @@ -375,22 +455,19 @@ void AstarSearch::setPath(const AstarNode & goal_node) } waypoints_.header = header; - waypoints_.waypoints.clear(); + waypoints_.waypoints = waypoints; - for (size_t i = 0; i < waypoints.size() - 1; ++i) { - const auto & current = waypoints[i]; - const auto & next = waypoints[i + 1]; + if (!is_backward_search_) return; - waypoints_.waypoints.push_back(current); + for (size_t i = 0; i < waypoints_.waypoints.size() - 1; ++i) { + const auto & current = waypoints_.waypoints[i]; + auto & next = waypoints_.waypoints[i + 1]; if (current.is_back != next.is_back) { - waypoints_.waypoints.push_back( - {is_backward_search_ ? next.pose : current.pose, - is_backward_search_ ? current.is_back : next.is_back}); + next.is_back = current.is_back; + ++i; // skip next waypoint } } - - waypoints_.waypoints.push_back(waypoints.back()); } bool AstarSearch::isGoal(const AstarNode & node) const @@ -400,26 +477,65 @@ bool AstarSearch::isGoal(const AstarNode & node) const const double goal_angle = autoware::universe_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); - const auto relative_pose = calcRelativePose(goal_pose_, node2pose(node)); + const auto node_pose = node2pose(node); - // Check conditions - if (astar_param_.only_behind_solutions && relative_pose.position.x > 0) { - return false; - } + auto checkGoal = [this, &node_pose, &lateral_goal_range, &longitudinal_goal_range, &goal_angle, + &is_back = node.is_back](const Pose & pose) { + const auto node_index = pose2index(costmap_, node_pose, planner_common_param_.theta_size); + const auto goal_index = pose2index(costmap_, pose, planner_common_param_.theta_size); - if ( - std::fabs(relative_pose.position.x) > longitudinal_goal_range || - std::fabs(relative_pose.position.y) > lateral_goal_range) { - return false; - } + if (node_index == goal_index) return true; - const auto angle_diff = - autoware::universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); - if (std::abs(angle_diff) > goal_angle) { - return false; - } + const auto relative_pose = calcRelativePose(pose, node_pose); - return true; + bool is_behind_goal = relative_pose.position.x <= 0.0; + + if (astar_param_.only_behind_solutions && !is_behind_goal) { + return false; + } + + if ( + std::fabs(relative_pose.position.x) > longitudinal_goal_range || + std::fabs(relative_pose.position.y) > lateral_goal_range) { + return false; + } + + const auto angle_diff = + autoware::universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); + if (std::abs(angle_diff) > goal_angle) { + return false; + } + + const bool is_set_shifted_goal_pose = + is_backward_search_ ? is_behind_goal == is_back : is_behind_goal != is_back; + if (is_set_shifted_goal_pose) { + setShiftedGoalPose(pose, relative_pose.position.y); + } + + return true; + }; + + if (checkGoal(goal_pose_)) return true; + + return std::any_of( + alternate_goals_.begin(), alternate_goals_.end(), + [&checkGoal](const Pose & pose) { return checkGoal(pose); }); +} + +void AstarSearch::setShiftedGoalPose(const Pose & goal_pose, const double lat_offset) const +{ + tf2::Transform tf; + tf2::convert(goal_pose, tf); + + geometry_msgs::msg::TransformStamped transform; + transform.transform = tf2::toMsg(tf); + + Pose lat_pose; + lat_pose.position = geometry_msgs::build().x(0.0).y(lat_offset).z(0.0); + lat_pose.orientation = + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0); + + shifted_goal_pose_ = transformPose(lat_pose, transform); } Pose AstarSearch::node2pose(const AstarNode & node) const diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index 20a59048e59d1..b9343f680fa1a 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -41,6 +41,13 @@ RRTStar::RRTStar( } } +bool RRTStar::makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) +{ + return makePlan(start_pose, goal_candidates.front()); +} + bool RRTStar::makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) { diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 82806bb500b0e..2b2254d66c844 100644 --- a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -214,12 +214,22 @@ std::unique_ptr configure_astar(bool use_multi) const bool use_back = true; const bool adapt_expansion_distance = true; const double expansion_distance = 0.4; + const double near_goal_distance = 4.0; const double distance_heuristic_weight = 2.0; const double smoothness_weight = 0.5; const double obstacle_distance_weight = 1.7; + const double goal_lat_distance_weight = 1.0; const auto astar_param = fpa::AstarParam{ - search_method, only_behind_solutions, use_back, adapt_expansion_distance, - expansion_distance, distance_heuristic_weight, smoothness_weight, obstacle_distance_weight}; + search_method, + only_behind_solutions, + use_back, + adapt_expansion_distance, + expansion_distance, + near_goal_distance, + distance_heuristic_weight, + smoothness_weight, + obstacle_distance_weight, + goal_lat_distance_weight}; auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); return algo; diff --git a/planning/autoware_obstacle_stop_planner/README.md b/planning/autoware_obstacle_stop_planner/README.md index 10870222aee09..d3965192cd4c3 100644 --- a/planning/autoware_obstacle_stop_planner/README.md +++ b/planning/autoware_obstacle_stop_planner/README.md @@ -31,6 +31,8 @@ ### Common Parameter +{{ json_to_markdown("planning/autoware_obstacle_stop_planner/schema/common.schema.json") }} + | Parameter | Type | Description | | -------------------------------------- | ------ | ----------------------------------------------------------------------------------------- | | `enable_slow_down` | bool | enable slow down planner [-] | @@ -103,6 +105,8 @@ stopped due to other factors. ### Parameters +{{ json_to_markdown("planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json") }} + #### Stop position | Parameter | Type | Description | @@ -186,6 +190,8 @@ down section. ### Parameters +{{ json_to_markdown("planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json") }} + #### Slow down section | Parameter | Type | Description | diff --git a/planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json b/planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json new file mode 100644 index 0000000000000..25a6a9338987b --- /dev/null +++ b/planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json @@ -0,0 +1,216 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for adaptive cruise control", + "type": "object", + "definitions": { + "adaptive_cruise_control": { + "type": "object", + "properties": { + "adaptive_cruise_control": { + "type": "object", + "properties": { + "use_object_to_estimate_vel": { + "type": "boolean", + "description": "use tracking objects for estimating object velocity or not", + "default": "true" + }, + "use_pcl_to_estimate_vel": { + "type": "boolean", + "description": "use pcl for estimating object velocity or not", + "default": "true" + }, + "consider_obj_velocity": { + "type": "boolean", + "description": "consider forward vehicle velocity to ACC or not", + "default": "true" + }, + "obstacle_velocity_thresh_to_start_acc": { + "type": "number", + "description": "start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s]", + "default": "1.5" + }, + "obstacle_velocity_thresh_to_stop_acc": { + "type": "number", + "description": "stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s]", + "default": "1.0" + }, + "emergency_stop_acceleration": { + "type": "number", + "description": "supposed minimum acceleration (deceleration) in emergency stop [m/ss]", + "default": "-5.0" + }, + "emergency_stop_idling_time": { + "type": "number", + "description": "supposed idling time to start emergency stop [s]", + "default": "0.5" + }, + "min_dist_stop": { + "type": "number", + "description": "minimum distance of emergency stop [m]", + "default": "4.0" + }, + "obstacle_emergency_stop_acceleration": { + "type": "number", + "description": "supposed minimum acceleration (deceleration) in emergency stop [m/ss]", + "default": "-5.0" + }, + "max_standard_acceleration": { + "type": "number", + "description": "supposed maximum acceleration in active cruise control [m/ss]", + "default": "0.5" + }, + "min_standard_acceleration": { + "type": "number", + "description": "supposed minimum acceleration (deceleration) in active cruise control", + "default": "-1.0" + }, + "standard_idling_time": { + "type": "number", + "description": "supposed idling time to react object in active cruise control [s]", + "default": "0.5" + }, + "min_dist_standard": { + "type": "number", + "description": "minimum distance in active cruise control [m]", + "default": "4.0" + }, + "obstacle_min_standard_acceleration": { + "type": "number", + "description": "supposed minimum acceleration of forward obstacle [m/ss]", + "default": "-1.5" + }, + "margin_rate_to_change_vel": { + "type": "number", + "description": "margin to insert upper velocity [-]", + "default": "0.3" + }, + "use_time_compensation_to_calc_distance": { + "type": "boolean", + "description": "use time-compensation to calculate distance to forward vehicle", + "default": "true" + }, + "p_coefficient_positive": { + "type": "number", + "description": "coefficient P in PID control (used when target dist -current_dist >=0) [-]", + "default": "0.1" + }, + "p_coefficient_negative": { + "type": "number", + "description": "coefficient P in PID control (used when target dist -current_dist <0) [-]", + "default": "0.3" + }, + "d_coefficient_positive": { + "type": "number", + "description": "coefficient D in PID control (used when delta_dist >=0) [-]", + "default": "0.0" + }, + "d_coefficient_negative": { + "type": "number", + "description": "coefficient D in PID control (used when delta_dist <0) [-]", + "default": "0.2" + }, + "object_polygon_length_margin": { + "type": "number", + "description": "The distance to extend the polygon length the object in pointcloud-object matching [m]", + "default": "2.0" + }, + "object_polygon_width_margin": { + "type": "number", + "description": "The distance to extend the polygon width the object in pointcloud-object matching [m]", + "default": "0.5" + }, + "valid_estimated_vel_diff_time": { + "type": "number", + "description": "Maximum time difference treated as continuous points in speed estimation using a point cloud [s]", + "default": "1.0" + }, + "valid_vel_que_time": { + "type": "number", + "description": "Time width of information used for speed estimation in speed estimation using a point cloud [s]", + "default": "0.5" + }, + "valid_estimated_vel_max": { + "type": "number", + "description": "Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s]", + "default": "20.0" + }, + "valid_estimated_vel_min": { + "type": "number", + "description": "Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s]", + "default": "-20.0" + }, + "thresh_vel_to_stop": { + "type": "number", + "description": "Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s]", + "default": "1.5" + }, + "lowpass_gain_of_upper_velocity": { + "type": "number", + "description": "Lowpass-gain of upper velocity", + "default": "0.75" + }, + "use_rough_velocity_estimation": { + "type": "boolean", + "description": "Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####)", + "default": "false" + }, + "rough_velocity_rate": { + "type": "number", + "description": "In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value", + "default": "0.9" + } + }, + "required": [ + "use_object_to_estimate_vel", + "use_pcl_to_estimate_vel", + "consider_obj_velocity", + "obstacle_velocity_thresh_to_start_acc", + "obstacle_velocity_thresh_to_stop_acc", + "emergency_stop_acceleration", + "emergency_stop_idling_time", + "min_dist_stop", + "obstacle_emergency_stop_acceleration", + "max_standard_acceleration", + "min_standard_acceleration", + "standard_idling_time", + "min_dist_standard", + "obstacle_min_standard_acceleration", + "margin_rate_to_change_vel", + "use_time_compensation_to_calc_distance", + "p_coefficient_positive", + "p_coefficient_negative", + "d_coefficient_positive", + "d_coefficient_negative", + "object_polygon_length_margin", + "object_polygon_width_margin", + "valid_estimated_vel_diff_time", + "valid_vel_que_time", + "valid_estimated_vel_max", + "valid_estimated_vel_min", + "thresh_vel_to_stop", + "lowpass_gain_of_upper_velocity", + "use_rough_velocity_estimation", + "rough_velocity_rate" + ], + "additionalProperties": false + } + }, + "required": ["adaptive_cruise_control"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/adaptive_cruise_control" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/autoware_obstacle_stop_planner/schema/common.schema.json b/planning/autoware_obstacle_stop_planner/schema/common.schema.json new file mode 100644 index 0000000000000..d8360f48c1617 --- /dev/null +++ b/planning/autoware_obstacle_stop_planner/schema/common.schema.json @@ -0,0 +1,85 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for common in autoware_obstacle_stop_planner", + "type": "object", + "definitions": { + "common": { + "type": "object", + "properties": { + "max_vel": { + "type": "number", + "description": "max velocity limit [m/s]", + "default": "11.1" + }, + "normal": { + "type": "object", + "properties": { + "min_acc": { + "type": "number", + "description": "min deceleration [m/ss]", + "default": "-0.5" + }, + "max_acc": { + "type": "number", + "description": "max acceleration [m/ss]", + "default": "1.0" + }, + "min_jerk": { + "type": "number", + "description": "min jerk [m/sss]", + "default": "-0.5" + }, + "max_jerk": { + "type": "number", + "description": "max jerk [m/sss]", + "default": "1.0" + } + }, + "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] + }, + "limit": { + "type": "object", + "properties": { + "min_acc": { + "type": "number", + "description": "min deceleration limit [m/ss]", + "default": "-2.5" + }, + "max_acc": { + "type": "number", + "description": "max acceleration [m/ss]", + "default": "1.0" + }, + "min_jerk": { + "type": "number", + "description": "min jerk [m/sss]", + "default": "-1.5" + }, + "max_jerk": { + "type": "number", + "description": "max jerk [m/sss]", + "default": "1.5" + } + }, + "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] + } + }, + "required": ["max_vel", "normal", "limit"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/common" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json b/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json new file mode 100644 index 0000000000000..868f669ad2004 --- /dev/null +++ b/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json @@ -0,0 +1,322 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for obstacle stop planner", + "type": "object", + "definitions": { + "obstacle_stop_planner": { + "type": "object", + "properties": { + "chattering_threshold": { + "type": "number", + "description": "even if the obstacle disappears, the stop judgment continues for chattering_threshold [s]", + "default": "0.5" + }, + "max_velocity": { + "type": "number", + "description": "max velocity [m/s]", + "default": "20.0" + }, + "ego_nearest_dist_threshold": { + "type": "number", + "description": "[m]", + "default": "3.0" + }, + "ego_nearest_yaw_threshold": { + "type": "number", + "description": "[rad] = 60 [deg]", + "default": "1.046" + }, + "enable_slow_down": { + "type": "boolean", + "description": "whether to use slow down planner [-]", + "default": "false" + }, + "enable_z_axis_obstacle_filtering": { + "type": "boolean", + "description": "filter obstacles in z axis (height) [-]", + "default": "true" + }, + "z_axis_filtering_buffer": { + "type": "number", + "description": "additional buffer for z axis filtering [m]", + "default": "0.0" + }, + "voxel_grid_x": { + "type": "number", + "description": "voxel grid x parameter for filtering pointcloud [m]", + "default": "0.05" + }, + "voxel_grid_y": { + "type": "number", + "description": "voxel grid y parameter for filtering pointcloud [m]", + "default": "0.05" + }, + "voxel_grid_z": { + "type": "number", + "description": "voxel grid z parameter for filtering pointcloud [m]", + "default": "100000.0" + }, + "use_predicted_objects": { + "type": "boolean", + "description": "whether to use predicted objects [-]", + "default": "false" + }, + "publish_obstacle_polygon": { + "type": "boolean", + "description": "whether to publish obstacle polygon [-]", + "default": "false" + }, + "predicted_object_filtering_threshold": { + "type": "number", + "description": "threshold for filtering predicted objects (valid only publish_obstacle_polygon true) [m]", + "default": "1.5" + }, + "stop_planner": { + "type": "object", + "properties": { + "stop_position": { + "type": "object", + "properties": { + "max_longitudinal_margin": { + "type": "number", + "description": "stop margin distance from obstacle on the path [m]", + "default": "5.0" + }, + "max_longitudinal_margin_behind_goal": { + "type": "number", + "description": "stop margin distance from obstacle behind the goal on the path [m]", + "default": "3.0" + }, + "min_longitudinal_margin": { + "type": "number", + "description": "stop margin distance when any other stop point is inserted in stop margin [m]", + "default": "2.0" + }, + "hold_stop_margin_distance": { + "type": "number", + "description": "the ego keeps stopping if the ego is in this margin [m]", + "default": "0.0" + } + }, + "required": [ + "max_longitudinal_margin", + "max_longitudinal_margin_behind_goal", + "min_longitudinal_margin", + "hold_stop_margin_distance" + ] + }, + "detection_area": { + "type": "object", + "properties": { + "lateral_margin": { + "type": "number", + "description": "margin [m]", + "default": "0.0" + }, + "vehicle_lateral_margin": { + "type": "number", + "description": "margin of vehicle footprint [m]", + "default": "0.0" + }, + "pedestrian_lateral_margin": { + "type": "number", + "description": "margin of pedestrian footprint [m]", + "default": "0.0" + }, + "unknown_lateral_margin": { + "type": "number", + "description": "margin of unknown footprint [m]", + "default": "0.0" + }, + "step_length": { + "type": "number", + "description": "step length for pointcloud search range [m]", + "default": "1.0" + }, + "enable_stop_behind_goal_for_obstacle": { + "type": "boolean", + "description": "enable extend trajectory after goal lane for obstacle detection", + "default": "true" + } + }, + "required": [ + "lateral_margin", + "vehicle_lateral_margin", + "pedestrian_lateral_margin", + "unknown_lateral_margin", + "step_length", + "enable_stop_behind_goal_for_obstacle" + ] + } + }, + "required": ["stop_position", "detection_area"] + }, + "slow_down_planner": { + "type": "object", + "properties": { + "slow_down_section": { + "type": "object", + "properties": { + "longitudinal_forward_margin": { + "type": "number", + "description": "margin distance from slow down point to vehicle front [m]", + "default": "5.0" + }, + "longitudinal_backward_margin": { + "type": "number", + "description": "margin distance from slow down point to vehicle rear [m]", + "default": "5.0" + }, + "longitudinal_margin_span": { + "type": "number", + "description": "fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s]", + "default": "-0.1" + }, + "min_longitudinal_forward_margin": { + "type": "number", + "description": "min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s]", + "default": "1.0" + } + }, + "required": [ + "longitudinal_forward_margin", + "longitudinal_backward_margin", + "longitudinal_margin_span", + "min_longitudinal_forward_margin" + ] + }, + "detection_area": { + "type": "object", + "properties": { + "lateral_margin": { + "type": "number", + "description": "offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + }, + "vehicle_lateral_margin": { + "type": "number", + "description": "offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + }, + "pedestrian_lateral_margin": { + "type": "number", + "description": "offset from pedestrian side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + }, + "unknown_lateral_margin": { + "type": "number", + "description": "offset from unknown side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + } + }, + "required": [ + "lateral_margin", + "vehicle_lateral_margin", + "pedestrian_lateral_margin", + "unknown_lateral_margin" + ] + }, + "target_velocity": { + "type": "object", + "properties": { + "max_slow_down_velocity": { + "type": "number", + "description": "max slow down velocity (use this param if consider_constraints is False)[m/s]", + "default": "1.38" + }, + "min_slow_down_velocity": { + "type": "number", + "description": "offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]", + "default": "0.28" + }, + "slow_down_velocity": { + "type": "number", + "description": "target slow down velocity (use this param if consider_constraints is True)[m/s]", + "default": "1.38" + } + }, + "required": ["max_slow_down_velocity", "min_slow_down_velocity", "slow_down_velocity"] + }, + "constraints": { + "type": "object", + "properties": { + "jerk_min_slow_down": { + "type": "number", + "description": "min slow down jerk constraint [m/sss]", + "default": "-0.3" + }, + "jerk_span": { + "type": "number", + "description": "fineness param for planning deceleration jerk [m/sss]", + "default": "-0.01" + }, + "jerk_start": { + "type": "number", + "description": "init jerk used for deceleration planning [m/sss]", + "default": "-0.1" + } + }, + "required": ["jerk_min_slow_down", "jerk_span", "jerk_start"] + }, + "consider_constraints": { + "type": "boolean", + "description": "set 'True', if no decel plan found under jerk/dec constrains, relax target slow down vel [-]", + "default": "false" + }, + "velocity_threshold_decel_complete": { + "type": "number", + "description": "use for judge whether the ego velocity converges the target slow down velocity [m/s]", + "default": "0.2" + }, + "acceleration_threshold_decel_complete": { + "type": "number", + "description": "use for judge whether the ego velocity converges the target slow down velocity [m/ss]", + "default": "0.1" + } + }, + "required": [ + "slow_down_section", + "detection_area", + "target_velocity", + "constraints", + "consider_constraints", + "velocity_threshold_decel_complete", + "acceleration_threshold_decel_complete" + ] + } + }, + "required": [ + "chattering_threshold", + "max_velocity", + "ego_nearest_dist_threshold", + "ego_nearest_yaw_threshold", + "enable_slow_down", + "enable_z_axis_obstacle_filtering", + "z_axis_filtering_buffer", + "voxel_grid_x", + "voxel_grid_y", + "voxel_grid_z", + "use_predicted_objects", + "publish_obstacle_polygon", + "predicted_object_filtering_threshold", + "stop_planner", + "slow_down_planner" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/obstacle_stop_planner" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 79c422ac283d1..e4a482def7b3d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -14,6 +14,7 @@ #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" #include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" @@ -988,30 +989,41 @@ bool passed_parked_objects( } const auto & current_path_end = current_lane_path.points.back().point.pose.position; - double min_dist_to_end_of_current_lane = std::numeric_limits::max(); - const auto & target_lanes = common_data_ptr->lanes_ptr->target; - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); - const double dist = autoware::motion_utils::calcSignedArcLength( - current_lane_path.points, obj_p, current_path_end); - min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); - if (is_goal_in_route) { + const auto dist_to_path_end = [&](const auto & src_point) { + if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) { const auto goal_pose = route_handler.getGoalPose(); - const double dist_to_goal = autoware::motion_utils::calcSignedArcLength( - current_lane_path.points, obj_p, goal_pose.position); - min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal); + return motion_utils::calcSignedArcLength( + current_lane_path.points, src_point, goal_pose.position); } - } + return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end); + }; + + const auto min_dist_to_end_of_current_lane = std::invoke([&]() { + auto min_dist_to_end_of_current_lane = std::numeric_limits::max(); + for (const auto & point : leading_obj_poly.outer()) { + const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0); + const auto dist = dist_to_path_end(obj_p); + min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); + } + return min_dist_to_end_of_current_lane; + }); // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { - debug.second.unsafe_reason = "delay lane change"; - utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return false; + if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) { + return true; } - return true; + const auto current_pose = common_data_ptr->get_ego_pose(); + const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( + current_lane_path.points, current_pose.position, leading_obj.initial_pose.pose.position); + + if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { + return true; + } + + debug.second.unsafe_reason = "delay lane change"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return false; } std::optional getLeadingStaticObjectIdx( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index ca476481ddde5..f30ed9d7a68da 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -320,8 +320,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double ego_vel = planner_data_->current_velocity->twist.linear.x; const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - const std::optional ego_crosswalk_passage_direction = - findEgoPassageDirectionAlongPath(sparse_resample_path); const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position); @@ -357,6 +355,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( }; std::optional> nearest_stop_info; std::vector stop_factor_points; + const std::optional ego_crosswalk_passage_direction = + findEgoPassageDirectionAlongPath(sparse_resample_path); for (const auto & object : object_info_manager_.getObject()) { const auto & collision_point_opt = object.collision_point; if (collision_point_opt) { @@ -618,9 +618,17 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( if (!intersect_pt1 || !intersect_pt2) { return std::nullopt; } - const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; - const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; - const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + + const auto idx1 = intersect_pt1.value().first; + const auto idx2 = intersect_pt2.value().first; + + const auto min_idx = std::min(idx1, idx2); + const auto dist1 = calcSignedArcLength(path.points, min_idx, intersect_pt1.value().second); + const auto dist2 = calcSignedArcLength(path.points, min_idx, intersect_pt2.value().second); + + const auto & front = dist1 > dist2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = dist1 > dist2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); } diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 60c082cba53da..5d7aba4d0567b 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -71,14 +71,14 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/downsample_filter/approximate_downsample_filter_nodelet.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp src/outlier_filter/ring_outlier_filter_nodelet.cpp - src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp + src/outlier_filter/voxel_grid_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp src/outlier_filter/dual_return_outlier_filter_nodelet.cpp src/passthrough_filter/passthrough_filter_nodelet.cpp src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp src/passthrough_filter/passthrough_uint16.cpp src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp - src/vector_map_filter/lanelet2_map_filter_nodelet.cpp + src/vector_map_filter/lanelet2_map_filter_node.cpp src/distortion_corrector/distortion_corrector.cpp src/distortion_corrector/distortion_corrector_node.cpp src/blockage_diag/blockage_diag_node.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml new file mode 100644 index 0000000000000..2aae4c5e886ea --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + voxel_size_x: 0.04 + voxel_size_y: 0.04 diff --git a/sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..2ff4c64d5f835 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + voxel_size_x: 0.3 + voxel_size_y: 0.3 + voxel_size_z: 0.1 + voxel_points_threshold: 2 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md index c38a4c719fcf3..8e978adb81657 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md @@ -25,10 +25,7 @@ The `vector_map_filter` is a node that removes points on the outside of lane by ### Core Parameters -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ----------- | -| `voxel_size_x` | double | 0.04 | voxel size | -| `voxel_size_y` | double | 0.04 | voxel size | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md index d99b54ef09a73..920d550c2426b 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md @@ -23,12 +23,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------ | ------ | ------------- | ------------------------------------------ | -| `voxel_size_x` | double | 0.3 | the voxel size along x-axis [m] | -| `voxel_size_y` | double | 0.3 | the voxel size along y-axis [m] | -| `voxel_size_z` | double | 0.1 | the voxel size along z-axis [m] | -| `voxel_points_threshold` | int | 2 | the minimum number of points in each voxel | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp similarity index 92% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp index 4ec50c53c2393..c9c6d1fd2dbad 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -50,4 +50,4 @@ class VoxelGridOutlierFilterComponent : public autoware::pointcloud_preprocessor }; } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp similarity index 96% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp index 4ba773ed618ac..5572a9a6f4588 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ #include #include @@ -97,4 +97,4 @@ class Lanelet2MapFilterComponent : public rclcpp::Node } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml new file mode 100644 index 0000000000000..745bda3119404 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..380a46eed3159 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json new file mode 100644 index 0000000000000..be7c90b5fd4f7 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json @@ -0,0 +1,40 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Lanelet2 Map Filter Node", + "type": "object", + "definitions": { + "lanelet2_map_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "voxel size along x-axis [m]", + "default": "0.04", + "minimum": 0 + }, + "voxel_size_y": { + "type": "number", + "description": "voxel size along y-axis [m]", + "default": "0.04", + "minimum": 0 + } + }, + "required": ["voxel_size_x", "voxel_size_y"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lanelet2_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..586b577f8540d --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json @@ -0,0 +1,52 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Grid Outlier Filter Node", + "type": "object", + "definitions": { + "voxel_grid_outlier_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "the voxel size along x-axis [m]", + "default": "0.3", + "minimum": 0 + }, + "voxel_size_y": { + "type": "number", + "description": "the voxel size along y-axis [m]", + "default": "0.3", + "minimum": 0 + }, + "voxel_size_z": { + "type": "number", + "description": "the voxel size along z-axis [m]", + "default": "0.1", + "minimum": 0 + }, + "voxel_points_threshold": { + "type": "integer", + "description": "the minimum number of points in each voxel", + "default": "2", + "minimum": 1 + } + }, + "required": ["voxel_size_x", "voxel_size_y", "voxel_size_z", "voxel_points_threshold"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_grid_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_node.cpp similarity index 90% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_node.cpp index 94dabf7ee5d9b..1f5cdc80a9c2c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp" #include #include @@ -28,10 +28,10 @@ VoxelGridOutlierFilterComponent::VoxelGridOutlierFilterComponent( { // set initial parameters { - voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); - voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); - voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); - voxel_points_threshold_ = static_cast(declare_parameter("voxel_points_threshold", 2)); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); + voxel_points_threshold_ = declare_parameter("voxel_points_threshold"); } using std::placeholders::_1; diff --git a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_node.cpp similarity index 98% rename from sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_node.cpp index b6de8bb87d5b1..ee788a2fc807a 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp" #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -39,8 +39,8 @@ Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions // Set parameters { - voxel_size_x_ = declare_parameter("voxel_size_x", 0.04); - voxel_size_y_ = declare_parameter("voxel_size_y", 0.04); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); } // Set publisher diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index 2e299fd7a2e51..c04503bee70fd 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -5,7 +5,7 @@ This node makes it easy to use the localization AD API from RViz. When a initial pose topic is received, call the localization initialize API. This node depends on the map height fitter library. -[See here for more details.](../../../map/map_height_fitter/README.md) +[See here for more details.](../../../map/autoware_map_height_fitter/README.md) | Interface | Local Name | Global Name | Description | | ------------ | ----------- | ---------------------------- | ----------------------------------------- | diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index b070131f1d567..a395eadbe3d2d 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -14,8 +14,8 @@ autoware_ad_api_specs autoware_adapi_v1_msgs + autoware_map_height_fitter component_interface_utils - map_height_fitter rclcpp rclcpp_components diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp index 340bc3b0a3058..e5b8d0e33dd3b 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp @@ -15,9 +15,9 @@ #ifndef INITIAL_POSE_ADAPTOR_HPP_ #define INITIAL_POSE_ADAPTOR_HPP_ +#include #include #include -#include #include #include @@ -36,7 +36,7 @@ class InitialPoseAdaptor : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_initial_pose_; component_interface_utils::Client::SharedPtr cli_initialize_; std::array rviz_particle_covariance_; - map_height_fitter::MapHeightFitter fitter_; + autoware::map_height_fitter::MapHeightFitter fitter_; void on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg); }; diff --git a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp b/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp index 0e64f25860fda..14a27dae9a539 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp @@ -34,7 +34,6 @@ class Graph void update(const rclcpp::Time & stamp); // cppcheck-suppress functionConst bool update(const rclcpp::Time & stamp, const DiagnosticStatus & status); const auto & nodes() const { return nodes_; } - const auto & diags() const { return diags_; } const auto & units() const { return units_; } DiagGraphStruct create_struct(const rclcpp::Time & stamp) const; DiagGraphStatus create_status(const rclcpp::Time & stamp) const; diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp index c9bc07cffde72..5c698a0a01860 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -42,19 +42,6 @@ DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_op this, get_clock(), period_ns, std::bind(&DuplicatedNodeChecker::onTimer, this)); } -std::string get_fullname_from_name_ns_pair(std::pair name_and_ns_pair) -{ - std::string full_name; - const std::string & name = name_and_ns_pair.first; - const std::string & ns = name_and_ns_pair.second; - if (ns.back() == '/') { - full_name = ns + name; - } else { - full_name = ns + "/" + name; - } - return full_name; -} - void DuplicatedNodeChecker::onTimer() { updater_.force_update(); diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml b/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml index e7e503d6d7eee..4eb9f0ab6d442 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml @@ -30,4 +30,4 @@ vgr_coef_a: 15.713 vgr_coef_b: 0.053 vgr_coef_c: 0.042 - convert_actuation_to_steering_status: true + convert_actuation_to_steering_status: false