Merge branch 'main' into fix/remove_unused_variable

.github/CODEOWNERS

@@ -25,6 +25,7 @@ common/autoware_test_utils/** [email protected] [email protected] ta
 common/autoware_testing/** [email protected] [email protected] [email protected] [email protected]
 common/autoware_time_utils/** [email protected] [email protected] [email protected]
 common/autoware_trajectory/** [email protected] [email protected] [email protected]
+common/autoware_traffic_light_utils/** [email protected] [email protected] [email protected]
 common/autoware_universe_utils/** [email protected] [email protected] [email protected]
 common/autoware_vehicle_info_utils/** [email protected] [email protected] [email protected] [email protected]
 common/bag_time_manager_rviz_plugin/** [email protected]
@@ -41,7 +42,6 @@ common/tier4_system_rviz_plugin/** [email protected]
 common/tier4_traffic_light_rviz_plugin/** [email protected]
 common/tier4_vehicle_rviz_plugin/** [email protected]
 common/traffic_light_recognition_marker_publisher/** [email protected] [email protected] [email protected]
-common/traffic_light_utils/** [email protected] [email protected] [email protected]
 control/autoware_autonomous_emergency_braking/** [email protected] [email protected] [email protected] [email protected] [email protected]
 control/autoware_collision_detector/** [email protected] [email protected] [email protected]
 control/autoware_control_validator/** [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -97,6 +97,7 @@ localization/yabloc/yabloc_image_processing/** [email protected] kento.yabuu
 localization/yabloc/yabloc_monitor/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_particle_filter/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_pose_initializer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+map/autoware_lanelet2_map_visualizer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 map/autoware_map_height_fitter/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 map/autoware_map_projection_loader/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 map/autoware_map_tf_generator/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]

common/.pages

@@ -22,8 +22,8 @@ nav:
     - 'Signal Processing':
       - 'Introduction': common/autoware_signal_processing
       - 'Butterworth Filter': common/autoware_signal_processing/documentation/ButterworthFilter
+    - 'autoware_traffic_light_utils': common/autoware_traffic_light_utils
     - 'autoware_universe_utils': common/autoware_universe_utils
-    - 'traffic_light_utils': common/traffic_light_utils
   - 'RVIZ2 Plugins':
     - 'autoware_perception_rviz_plugin': common/autoware_perception_rviz_plugin
     - 'autoware_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin

common/traffic_light_utils/CMakeLists.txt → common/autoware_traffic_light_utils/CMakeLists.txt

@@ -1,13 +1,13 @@
 cmake_minimum_required(VERSION 3.14)
-project(traffic_light_utils)
+project(autoware_traffic_light_utils)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 find_package(Boost REQUIRED)
 find_package(autoware_cmake REQUIRED)
 
-ament_auto_add_library(traffic_light_utils SHARED
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/traffic_light_utils.cpp
 )
 
@@ -16,7 +16,7 @@ if(BUILD_TESTING)
   file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
   ament_add_ros_isolated_gtest(test_traffic_light_utils ${TEST_SOURCES})
   target_include_directories(test_traffic_light_utils PRIVATE src/include)
-  target_link_libraries(test_traffic_light_utils traffic_light_utils)
+  target_link_libraries(test_traffic_light_utils ${PROJECT_NAME})
 endif()
 
 ament_auto_package()

common/traffic_light_utils/README.md → common/autoware_traffic_light_utils/README.md

@@ -1,4 +1,4 @@
-# traffic_light_utils
+# autoware_traffic_light_utils
 
 ## Purpose
 

common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp → common/autoware_traffic_light_utils/include/autoware/traffic_light_utils/traffic_light_utils.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_
-#define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_
+#ifndef AUTOWARE__TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_
+#define AUTOWARE__TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_
 
 #include "autoware_perception_msgs/msg/traffic_light_element.hpp"
 #include "autoware_perception_msgs/msg/traffic_light_group.hpp"
@@ -27,7 +27,7 @@
 #include <tf2/LinearMath/Matrix3x3.h>
 #include <tf2/LinearMath/Transform.h>
 
-namespace traffic_light_utils
+namespace autoware::traffic_light_utils
 {
 
 void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence = -1);
@@ -79,6 +79,6 @@ tf2::Vector3 getTrafficLightBottomRight(const lanelet::ConstLineString3d & traff
 
 tf2::Vector3 getTrafficLightCenter(const lanelet::ConstLineString3d & traffic_light);
 
-}  // namespace traffic_light_utils
+}  // namespace autoware::traffic_light_utils
 
-#endif  // TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_
+#endif  // AUTOWARE__TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_

common/traffic_light_utils/package.xml → common/autoware_traffic_light_utils/package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>traffic_light_utils</name>
+  <name>autoware_traffic_light_utils</name>
   <version>0.38.0</version>
-  <description>The traffic_light_utils package</description>
+  <description>The autoware_traffic_light_utils package</description>
   <maintainer email="[email protected]">Kotaro Uetake</maintainer>
   <maintainer email="[email protected]">Shunsuke Miura</maintainer>
   <maintainer email="[email protected]">Satoshi Ota</maintainer>

common/traffic_light_utils/src/traffic_light_utils.cpp → common/autoware_traffic_light_utils/src/traffic_light_utils.cpp

@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "traffic_light_utils/traffic_light_utils.hpp"
+#include "autoware/traffic_light_utils/traffic_light_utils.hpp"
 
-namespace traffic_light_utils
+namespace autoware::traffic_light_utils
 {
 
 void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence)
@@ -105,4 +105,4 @@ tf2::Vector3 getTrafficLightCenter(const lanelet::ConstLineString3d & traffic_li
   return (top_left + bottom_right) / 2;
 }
 
-}  // namespace traffic_light_utils
+}  // namespace autoware::traffic_light_utils

common/traffic_light_utils/test/test_traffic_light_utils.cpp → common/autoware_traffic_light_utils/test/test_traffic_light_utils.cpp

@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "autoware/traffic_light_utils/traffic_light_utils.hpp"
 #include "gtest/gtest.h"
-#include "traffic_light_utils/traffic_light_utils.hpp"
 
-namespace traffic_light_utils
+namespace autoware::traffic_light_utils
 {
 
 TEST(setSignalUnknown, set_signal_element)
@@ -53,4 +53,4 @@ TEST(getTrafficLightCenter, get_signal)
   EXPECT_FLOAT_EQ(getTrafficLightCenter(test_light).z(), (float)1.5);
 }
 
-}  // namespace traffic_light_utils
+}  // namespace autoware::traffic_light_utils

common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp

@@ -194,13 +194,16 @@ class InterpolatedArray
    * @param x The position to compute the value at.
    * @return The interpolated value.
    */
-  T compute(const double & x) const { return interpolator_->compute(x); }
+  [[nodiscard]] T compute(const double & x) const { return interpolator_->compute(x); }
 
   /**
    * @brief Get the underlying data of the array.
    * @return A pair containing the axis and values.
    */
-  std::pair<std::vector<double>, std::vector<T>> get_data() const { return {bases_, values_}; }
+  [[nodiscard]] std::pair<std::vector<double>, std::vector<T>> get_data() const
+  {
+    return {bases_, values_};
+  }
 };
 
 }  // namespace autoware::trajectory::detail

common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_
+#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_
 
 #include <Eigen/Dense>
 #include <rclcpp/logging.hpp>
@@ -74,17 +72,33 @@ class InterpolatorCommonInterface
    * 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.
+   * @return The input value, clamped to the range of the interpolator.
    */
-  void validate_compute_input(const double & s) const
+  [[nodiscard]] double 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());
     }
+    return std::clamp(s, start(), end());
   }
 
+  /**
+   * @brief Get the index of the interval containing the input value.
+   *
+   * This method determines the index of the interval in the bases array that contains the given
+   * value. It assumes that the bases array is sorted in ascending order.
+   *
+   * If `end_inclusive` is true and the input value matches the end of the bases array,
+   * the method returns the index of the second-to-last interval.
+   *
+   * @param s The input value for which to find the interval index.
+   * @param end_inclusive Whether to include the end value in the last interval. Defaults to true.
+   * @return The index of the interval containing the input value.
+   *
+   * @throw std::out_of_range if the input value is outside the range of the bases array.
+   */
   [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const
   {
     if (end_inclusive && s == end()) {
@@ -96,6 +110,13 @@ class InterpolatorCommonInterface
   }
 
 public:
+  InterpolatorCommonInterface() = default;
+  virtual ~InterpolatorCommonInterface() = default;
+  InterpolatorCommonInterface(const InterpolatorCommonInterface & other) = default;
+  InterpolatorCommonInterface & operator=(const InterpolatorCommonInterface & other) = default;
+  InterpolatorCommonInterface(InterpolatorCommonInterface && other) noexcept = default;
+  InterpolatorCommonInterface & operator=(InterpolatorCommonInterface && other) noexcept = default;
+
   /**
    * @brief Build the interpolator with the given bases and values.
    *
@@ -132,12 +153,10 @@ class InterpolatorCommonInterface
    */
   [[nodiscard]] T compute(const double & s) const
   {
-    validate_compute_input(s);
-    return compute_impl(s);
+    double clamped_s = validate_compute_input(s);
+    return compute_impl(clamped_s);
   }
 };
 }  // namespace autoware::trajectory::interpolator::detail
 
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
-// clang-format on
+#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_

common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_
+#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_
 
 #include "autoware/trajectory/interpolator/interpolator.hpp"
 
@@ -40,7 +38,7 @@ namespace autoware::trajectory::interpolator::detail
 template <class InterpolatorType, class T>
 struct InterpolatorMixin : public InterpolatorInterface<T>
 {
-  std::shared_ptr<InterpolatorInterface<T>> clone() const override
+  [[nodiscard]] std::shared_ptr<InterpolatorInterface<T>> clone() const override
   {
     return std::make_shared<InterpolatorType>(static_cast<const InterpolatorType &>(*this));
   }
@@ -85,6 +83,4 @@ struct InterpolatorMixin : public InterpolatorInterface<T>
 
 }  // namespace autoware::trajectory::interpolator::detail
 
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_  // NOLINT
-// clang-format on
+#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_

common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_
+#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_
 
 #include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp"
 
@@ -80,6 +78,5 @@ class NearestNeighborCommonImpl : public detail::InterpolatorMixin<NearestNeighb
 
 }  // namespace detail
 }  // namespace autoware::trajectory::interpolator
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_  // NOLINT
-// clang-format on
+
+#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_

common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_
+#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_
 
 #include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp"
 
@@ -80,6 +78,4 @@ class StairstepCommonImpl : public detail::InterpolatorMixin<Stairstep<T>, T>
 }  // namespace detail
 }  // namespace autoware::trajectory::interpolator
 
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_  // NOLINT
-// clang-format on
+#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_

common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp

@@ -73,8 +73,8 @@ class InterpolatorInterface<double> : public detail::InterpolatorCommonInterface
    */
   [[nodiscard]] double compute_first_derivative(const double & s) const
   {
-    this->validate_compute_input(s);
-    return compute_first_derivative_impl(s);
+    double clamped_s = this->validate_compute_input(s);
+    return compute_first_derivative_impl(clamped_s);
   }
 
   /**
@@ -85,8 +85,8 @@ class InterpolatorInterface<double> : public detail::InterpolatorCommonInterface
    */
   [[nodiscard]] double compute_second_derivative(const double & s) const
   {
-    this->validate_compute_input(s);
-    return compute_second_derivative_impl(s);
+    double clamped_s = this->validate_compute_input(s);
+    return compute_second_derivative_impl(clamped_s);
   }
 
   [[nodiscard]] virtual std::shared_ptr<InterpolatorInterface<double>> clone() const = 0;

common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_
+#define AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_
 
 #include "autoware/trajectory/path_point.hpp"
 #include "autoware/trajectory/point.hpp"
@@ -149,6 +147,4 @@ class Trajectory<tier4_planning_msgs::msg::PathPointWithLaneId>
 };
 }  // namespace autoware::trajectory
 
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_  // NOLINT
-// clang-format on
+#endif  // AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_

common/autoware_trajectory/src/path_point_with_lane_id.cpp

@@ -16,6 +16,8 @@
 
 #include "autoware/trajectory/detail/utils.hpp"
 
+#include <vector>
+
 namespace autoware::trajectory
 {