Merge branch 'main' into port-autoware-route-handler

.github/CODEOWNERS

@@ -1,4 +1,5 @@
 ### Automatically generated from package.xml ###
+common/autoware_component_interface_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 common/autoware_geography_utils/** anh.nguyen.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 common/autoware_interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-core-global-codeowners
@@ -11,6 +12,8 @@ common/autoware_trajectory/** mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yu
 demos/autoware_test_node/** mfc@autoware.org @autowarefoundation/autoware-core-global-codeowners
 localization/autoware_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 @autowarefoundation/autoware-core-global-codeowners
 localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners
+sensing/autoware_gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners
+testing/autoware_pyplot/** mamoru.sobue@tier4.jp yukinari.hisaki.2@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 testing/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 
 ### Copied from .github/CODEOWNERS-manual ###

common/autoware_trajectory/examples/example_find_intervals.cpp

@@ -22,10 +22,10 @@
 using Trajectory =
   autoware::trajectory::Trajectory<autoware_internal_planning_msgs::msg::PathPointWithLaneId>;
 
-autoware_internal::msg::PathPointWithLaneId path_point_with_lane_id(
+autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id(
   double x, double y, uint8_t lane_id)
 {
-  autoware_internal::msg::PathPointWithLaneId point;
+  autoware_internal_planning_msgs::msg::PathPointWithLaneId point;
   point.point.pose.position.x = x;
   point.point.pose.position.y = y;
   point.lane_ids.emplace_back(lane_id);
@@ -37,7 +37,7 @@ int main()
   pybind11::scoped_interpreter guard{};
   auto plt = matplotlibcpp17::pyplot::import();
 
-  std::vector<autoware_internal::msg::PathPointWithLaneId> points{
+  std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> points{
     path_point_with_lane_id(0.41, 0.69, 0), path_point_with_lane_id(0.66, 1.09, 0),
     path_point_with_lane_id(0.93, 1.41, 0), path_point_with_lane_id(1.26, 1.71, 0),
     path_point_with_lane_id(1.62, 1.90, 0), path_point_with_lane_id(1.96, 1.98, 0),
@@ -56,7 +56,7 @@ int main()
   }
 
   const auto intervals = autoware::trajectory::find_intervals(
-    *trajectory, [](const autoware_internal::msg::PathPointWithLaneId & point) {
+    *trajectory, [](const autoware_internal_planning_msgs::msg::PathPointWithLaneId & point) {
       return point.lane_ids[0] == 1;
     });
 

common/autoware_trajectory/examples/example_interpolator.cpp

@@ -35,6 +35,7 @@ int main()
   std::random_device seed_gen;
   std::mt19937 engine(seed_gen());
   std::uniform_real_distribution<> dist(-1.0, 1.0);
+  values.reserve(bases.size());
   for (size_t i = 0; i < bases.size(); ++i) {
     values.push_back(dist(engine));
   }

common/autoware_trajectory/examples/example_trajectory_point.cpp

@@ -16,6 +16,7 @@
 #include "autoware/trajectory/point.hpp"
 #include "autoware/trajectory/utils/closest.hpp"
 #include "autoware/trajectory/utils/crossed.hpp"
+#include "autoware/trajectory/utils/curvature_utils.hpp"
 #include "lanelet2_core/primitives/LineString.h"
 
 #include <geometry_msgs/msg/point.hpp>
@@ -133,6 +134,10 @@ int main()
     }
     plt.scatter(Args(x, y), Kwargs("label"_a = "Restored", "color"_a = "orange", "marker"_a = "x"));
   }
+  {
+    auto max_curvature = autoware::trajectory::max_curvature(*trajectory);
+    std::cout << "Max curvature: " << max_curvature << std::endl;
+  }
 
   plt.axis(Args("equal"));
   plt.legend();

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

@@ -36,6 +36,9 @@ namespace autoware::trajectory
 template <>
 class Trajectory<geometry_msgs::msg::Point>
 {
+  template <class PointType>
+  friend class Trajectory;
+
   using PointType = geometry_msgs::msg::Point;
 
 protected:

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

@@ -49,6 +49,9 @@ class Trajectory<geometry_msgs::msg::Pose> : public Trajectory<geometry_msgs::ms
   Trajectory & operator=(const Trajectory & rhs);
   Trajectory & operator=(Trajectory && rhs) = default;
 
+  // enable making trajectory from point trajectory
+  explicit Trajectory(const Trajectory<geometry_msgs::msg::Point> & point_trajectory);
+
   bool build(const std::vector<PointType> & points);
 
   /**

common/autoware_trajectory/include/autoware/trajectory/utils/curvature_utils.hpp

@@ -0,0 +1,37 @@
+// Copyright 2025 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__TRAJECTORY__UTILS__CURVATURE_UTILS_HPP_
+#define AUTOWARE__TRAJECTORY__UTILS__CURVATURE_UTILS_HPP_
+
+#include "autoware/trajectory/forward.hpp"
+
+#include <algorithm>
+
+namespace autoware::trajectory
+{
+template <class PointType>
+double max_curvature(const Trajectory<PointType> & trajectory)
+{
+  double max_curvature = 0.0;
+  for (const auto & base : trajectory.get_internal_bases()) {
+    const auto curvature = trajectory.curvature(base);
+    max_curvature = std::max(max_curvature, curvature);
+  }
+  return max_curvature;
+}
+
+}  // namespace autoware::trajectory
+
+#endif  // AUTOWARE__TRAJECTORY__UTILS__CURVATURE_UTILS_HPP_

common/autoware_trajectory/src/pose.cpp

@@ -15,6 +15,7 @@
 #include "autoware/trajectory/pose.hpp"
 
 #include "autoware/trajectory/detail/helpers.hpp"
+#include "autoware/trajectory/forward.hpp"
 #include "autoware/trajectory/interpolator/spherical_linear.hpp"
 
 #include <tf2/LinearMath/Quaternion.h>
@@ -45,6 +46,32 @@ Trajectory<PointType> & Trajectory<PointType>::operator=(const Trajectory & rhs)
   return *this;
 }
 
+Trajectory<PointType>::Trajectory(const Trajectory<geometry_msgs::msg::Point> & point_trajectory)
+: Trajectory()
+{
+  x_interpolator_ = point_trajectory.x_interpolator_->clone();
+  y_interpolator_ = point_trajectory.y_interpolator_->clone();
+  z_interpolator_ = point_trajectory.z_interpolator_->clone();
+  bases_ = point_trajectory.get_internal_bases();
+  start_ = point_trajectory.start_;
+  end_ = point_trajectory.end_;
+
+  // build mock orientations
+  std::vector<geometry_msgs::msg::Quaternion> orientations(bases_.size());
+  for (size_t i = 0; i < bases_.size(); ++i) {
+    orientations[i].w = 1.0;
+  }
+  bool success = orientation_interpolator_->build(bases_, orientations);
+
+  if (!success) {
+    throw std::runtime_error(
+      "Failed to build orientation interpolator.");  // This Exception should not be thrown.
+  }
+
+  // align orientation with trajectory direction
+  align_orientation_with_trajectory_direction();
+}
+
 bool Trajectory<PointType>::build(const std::vector<PointType> & points)
 {
   std::vector<geometry_msgs::msg::Point> path_points;

common/autoware_trajectory/test/test_trajectory_container.cpp

@@ -14,6 +14,7 @@
 #include "autoware/trajectory/path_point_with_lane_id.hpp"
 #include "autoware/trajectory/utils/closest.hpp"
 #include "autoware/trajectory/utils/crossed.hpp"
+#include "autoware/trajectory/utils/curvature_utils.hpp"
 #include "autoware/trajectory/utils/find_intervals.hpp"
 #include "lanelet2_core/primitives/LineString.h"
 
@@ -187,3 +188,9 @@ TEST_F(TrajectoryTest, find_interval)
   EXPECT_LT(intervals[0].start, intervals[0].end);
   EXPECT_NEAR(intervals[0].end, trajectory->length(), 0.1);
 }
+
+TEST_F(TrajectoryTest, max_curvature)
+{
+  double max_curvature = autoware::trajectory::max_curvature(*trajectory);
+  EXPECT_LT(0, max_curvature);
+}

planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_objects_of_interest_marker_interface)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(autoware_objects_of_interest_marker_interface SHARED
+  src/coloring.cpp
+  src/objects_of_interest_marker_interface.cpp
+  src/marker_utils.cpp
+)
+
+# Test
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

planning/autoware_objects_of_interest_marker_interface/README.md

@@ -0,0 +1,46 @@
+# Objects Of Interest Marker Interface
+
+## Overview
+
+`autoware_objects_of_interest_marker_interface` is a collection of object visualization function packages.
+
+## Design
+
+This package implement a library to manage and visualize the object information by construct and publish it as marker array to rviz.
+
+For a object to be visualized, it has three import characteristics.
+
+- pose the position of the object
+- shape the shape of the Bounding box of the object
+- color the color of the Bounding box of the object
+
+## Usage
+
+### init
+
+include the header file to use then init the library
+
+```cpp
+#include <autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
+
+autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface
+    objects_of_interest_marker_interface_{this, "obstacle_cruise_planner"};
+```
+
+### insert
+
+insert object information to the 'objects_of_interest_marker_interface' manager
+
+```cpp
+using autoware::objects_of_interest_marker_interface::ColorName;
+objects_of_interest_marker_interface_.insertObjectData(
+    stopped_obstacle.pose, stopped_obstacle.shape, ColorName::RED);
+```
+
+### publish
+
+publish object information to the rviz to visualize
+
+```cpp
+objects_of_interest_marker_interface_.publishMarkerArray();
+```

planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp

@@ -0,0 +1,31 @@
+// Copyright 2023 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__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_
+#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_
+#include "autoware/objects_of_interest_marker_interface/marker_data.hpp"
+
+#include <autoware_utils/ros/marker_helper.hpp>
+
+#include <std_msgs/msg/color_rgba.hpp>
+
+namespace autoware::objects_of_interest_marker_interface::coloring
+{
+std_msgs::msg::ColorRGBA getGreen(const float alpha);
+std_msgs::msg::ColorRGBA getAmber(const float alpha);
+std_msgs::msg::ColorRGBA getRed(const float alpha);
+std_msgs::msg::ColorRGBA getGray(const float alpha);
+}  // namespace autoware::objects_of_interest_marker_interface::coloring
+
+#endif  // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_

planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp

@@ -0,0 +1,34 @@
+// Copyright 2023 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__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_
+#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_
+
+#include <autoware_perception_msgs/msg/predicted_object.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+
+namespace autoware::objects_of_interest_marker_interface
+{
+struct ObjectMarkerData
+{
+  geometry_msgs::msg::Pose pose{};
+  autoware_perception_msgs::msg::Shape shape{};
+  std_msgs::msg::ColorRGBA color;
+};
+
+enum class ColorName { GRAY, GREEN, AMBER, RED };
+}  // namespace autoware::objects_of_interest_marker_interface
+
+#endif  // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_