feat(autoware_trajectory): update autoware_trajectory interfaces (#10034

) * feat(autoware_trajectory): update autoware_trajectory interfaces Signed-off-by: Y.Hisaki <[email protected]> * add const Signed-off-by: Y.Hisaki <[email protected]> * Update shift.hpp Signed-off-by: Y.Hisaki <[email protected]> * fix names Signed-off-by: Y.Hisaki <[email protected]> * fix copyright Signed-off-by: Y.Hisaki <[email protected]> --------- Signed-off-by: Y.Hisaki <[email protected]>
autowarefoundation · Jan 30, 2025 · d682048 · d682048
1 parent 0d030d7
commit d682048
Show file tree

Hide file tree

Showing 38 changed files with 1,951 additions and 442 deletions.
diff --git a/common/autoware_trajectory/examples/example_find_intervals.cpp b/common/autoware_trajectory/examples/example_find_intervals.cpp
@@ -0,0 +1,108 @@
+// 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.
+
+#include "autoware/trajectory/path_point_with_lane_id.hpp"
+#include "autoware/trajectory/utils/find_intervals.hpp"
+
+#include <matplotlibcpp17/pyplot.h>
+
+#include <vector>
+
+using Trajectory = autoware::trajectory::Trajectory<tier4_planning_msgs::msg::PathPointWithLaneId>;
+
+tier4_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id(
+  double x, double y, uint8_t lane_id)
+{
+  tier4_planning_msgs::msg::PathPointWithLaneId point;
+  point.point.pose.position.x = x;
+  point.point.pose.position.y = y;
+  point.lane_ids.emplace_back(lane_id);
+  return point;
+}
+
+int main()
+{
+  pybind11::scoped_interpreter guard{};
+  auto plt = matplotlibcpp17::pyplot::import();
+
+  std::vector<tier4_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),
+    path_point_with_lane_id(2.48, 1.96, 1), path_point_with_lane_id(3.02, 1.87, 1),
+    path_point_with_lane_id(3.56, 1.82, 1), path_point_with_lane_id(4.14, 2.02, 1),
+    path_point_with_lane_id(4.56, 2.36, 1), path_point_with_lane_id(4.89, 2.72, 1),
+    path_point_with_lane_id(5.27, 3.15, 1), path_point_with_lane_id(5.71, 3.69, 1),
+    path_point_with_lane_id(6.09, 4.02, 0), path_point_with_lane_id(6.54, 4.16, 0),
+    path_point_with_lane_id(6.79, 3.92, 0), path_point_with_lane_id(7.11, 3.60, 0),
+    path_point_with_lane_id(7.42, 3.01, 0)};
+
+  auto trajectory = Trajectory::Builder{}.build(points);
+
+  if (!trajectory) {
+    return 1;
+  }
+
+  const auto intervals = autoware::trajectory::find_intervals(
+    *trajectory, [](const tier4_planning_msgs::msg::PathPointWithLaneId & point) {
+      return point.lane_ids[0] == 1;
+    });
+
+  std::vector<double> x_id0;
+  std::vector<double> y_id0;
+  std::vector<double> x_id1;
+  std::vector<double> y_id1;
+  std::vector<double> x_all;
+  std::vector<double> y_all;
+
+  for (const auto & point : points) {
+    if (point.lane_ids[0] == 0) {
+      x_id0.push_back(point.point.pose.position.x);
+      y_id0.push_back(point.point.pose.position.y);
+    } else {
+      x_id1.push_back(point.point.pose.position.x);
+      y_id1.push_back(point.point.pose.position.y);
+    }
+  }
+
+  for (double s = 0.0; s < trajectory->length(); s += 0.01) {
+    auto p = trajectory->compute(s);
+    x_all.push_back(p.point.pose.position.x);
+    y_all.push_back(p.point.pose.position.y);
+  }
+
+  plt.plot(Args(x_all, y_all), Kwargs("color"_a = "blue"));
+  plt.scatter(Args(x_id0, y_id0), Kwargs("color"_a = "blue", "label"_a = "lane_id = 0"));
+  plt.scatter(Args(x_id1, y_id1), Kwargs("color"_a = "red", "label"_a = "lane_id = 1"));
+
+  std::vector<double> x_cropped;
+  std::vector<double> y_cropped;
+
+  trajectory->crop(intervals[0].start, intervals[0].end - intervals[0].start);
+
+  for (double s = 0.0; s < trajectory->length(); s += 0.01) {
+    auto p = trajectory->compute(s);
+    x_cropped.push_back(p.point.pose.position.x);
+    y_cropped.push_back(p.point.pose.position.y);
+  }
+
+  plt.plot(
+    Args(x_cropped, y_cropped),
+    Kwargs("color"_a = "red", "label"_a = "interval between lane_id = 1"));
+
+  plt.legend();
+  plt.show();
+
+  return 0;
+}
diff --git a/common/autoware_trajectory/examples/example_shift.cpp b/common/autoware_trajectory/examples/example_shift.cpp
@@ -0,0 +1,187 @@
+// 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.
+
+#include "autoware/trajectory/point.hpp"
+#include "autoware/trajectory/utils/shift.hpp"
+
+#include <matplotlibcpp17/pyplot.h>
+
+#include <iostream>
+#include <vector>
+
+geometry_msgs::msg::Point point(double x, double y)
+{
+  geometry_msgs::msg::Point p;
+  p.x = x;
+  p.y = y;
+  return p;
+}
+
+int main()
+{
+  pybind11::scoped_interpreter guard{};
+  auto plt = matplotlibcpp17::pyplot::import();
+
+  geometry_msgs::msg::Point p;
+  (void)(p);
+
+  std::vector<geometry_msgs::msg::Point> points = {
+    point(0.49, 0.59), point(0.61, 1.22), point(0.86, 1.93), point(1.20, 2.56), point(1.51, 3.17),
+    point(1.85, 3.76), point(2.14, 4.26), point(2.60, 4.56), point(3.07, 4.55), point(3.61, 4.30),
+    point(3.95, 4.01), point(4.29, 3.68), point(4.90, 3.25), point(5.54, 3.10), point(6.24, 3.18),
+    point(6.88, 3.54), point(7.51, 4.25), point(7.85, 4.93), point(8.03, 5.73), point(8.16, 6.52),
+    point(8.31, 7.28), point(8.45, 7.93), point(8.68, 8.45), point(8.96, 8.96), point(9.32, 9.36)};
+
+  auto trajectory =
+    autoware::trajectory::Trajectory<geometry_msgs::msg::Point>::Builder{}.build(points);
+
+  if (!trajectory) {
+    return 1;
+  }
+
+  std::cout << "length: " << trajectory->length() << std::endl;
+
+  {
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double s = 0.0; s < trajectory->length(); s += 0.01) {
+      auto p = trajectory->compute(s);
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "original"));
+
+    x.clear();
+    y.clear();
+
+    autoware::trajectory::ShiftInterval shift_interval;
+    shift_interval.end = -1.0;
+    shift_interval.lateral_offset = 0.5;
+
+    auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval);
+
+    for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) {
+      auto p = shifted_trajectory.compute(s);
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+
+    plt.axis(Args("equal"));
+    plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
+    plt.legend();
+    plt.show();
+  }
+
+  {
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double s = 0.0; s < trajectory->length(); s += 0.01) {
+      auto p = trajectory->compute(s);
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "original"));
+
+    x.clear();
+    y.clear();
+
+    autoware::trajectory::ShiftInterval shift_interval;
+    shift_interval.start = trajectory->length() / 4.0;
+    shift_interval.end = trajectory->length() * 3.0 / 4.0;
+    shift_interval.lateral_offset = 0.5;
+    auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval);
+
+    for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) {
+      auto p = shifted_trajectory.compute(s);
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+
+    plt.axis(Args("equal"));
+    plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
+    plt.legend();
+    plt.show();
+  }
+
+  {
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double s = 0.0; s < trajectory->length(); s += 0.01) {
+      auto p = trajectory->compute(s);
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "original"));
+
+    x.clear();
+    y.clear();
+
+    autoware::trajectory::ShiftInterval shift_interval;
+    shift_interval.start = trajectory->length() * 3.0 / 4.0;
+    shift_interval.end = trajectory->length() / 4.0;
+    shift_interval.lateral_offset = 0.5;
+    auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval);
+
+    for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) {
+      auto p = shifted_trajectory.compute(s);
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+
+    plt.axis(Args("equal"));
+    plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
+    plt.legend();
+    plt.show();
+  }
+
+  {
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double s = 0.0; s < trajectory->length(); s += 0.01) {
+      auto p = trajectory->compute(s);
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "original"));
+
+    x.clear();
+    y.clear();
+
+    autoware::trajectory::ShiftInterval shift_interval1;
+    shift_interval1.start = trajectory->length() / 4.0;
+    shift_interval1.end = trajectory->length() * 2.0 / 4.0;
+    shift_interval1.lateral_offset = 0.5;
+
+    autoware::trajectory::ShiftInterval shift_interval2;
+    shift_interval2.start = trajectory->length() * 2.0 / 4.0;
+    shift_interval2.end = trajectory->length() * 3.0 / 4.0;
+    shift_interval2.lateral_offset = -0.5;
+
+    auto shifted_trajectory =
+      autoware::trajectory::shift(*trajectory, {shift_interval1, shift_interval2});
+
+    for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) {
+      auto p = shifted_trajectory.compute(s);
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+
+    plt.axis(Args("equal"));
+    plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
+    plt.legend();
+    plt.show();
+  }
+
+  return 0;
+}
diff --git a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp
@@ -13,10 +13,15 @@
 // limitations under the License.
 
 #include "autoware/trajectory/path_point.hpp"
+#include "autoware/trajectory/utils/crossed.hpp"
+#include "lanelet2_core/primitives/LineString.h"
 
 #include <autoware_planning_msgs/msg/path_point.hpp>
 #include <geometry_msgs/msg/point.hpp>
 
+#include <boost/geometry/geometries/linestring.hpp>
+
+#include <Eigen/src/Core/Matrix.h>
 #include <matplotlibcpp17/pyplot.h>
 
 #include <iostream>
@@ -69,25 +74,24 @@ int main()
 
   trajectory->align_orientation_with_trajectory_direction();
 
-  geometry_msgs::msg::Point p1;
-  geometry_msgs::msg::Point p2;
-  p1.x = 7.5;
-  p1.y = 8.6;
-  p2.x = 10.2;
-  p2.y = 7.7;
+  lanelet::LineString2d line_string;
+  line_string.push_back(lanelet::Point3d(lanelet::InvalId, 7.5, 8.6, 0.0));
+  line_string.push_back(lanelet::Point3d(lanelet::InvalId, 10.2, 7.7, 0.0));
 
-  auto s = trajectory->crossed(p1, p2);
-  auto crossed = trajectory->compute(s.value());
+  auto s = autoware::trajectory::crossed(*trajectory, line_string);
+  auto crossed = trajectory->compute(s.at(0));
 
   plt.plot(
-    Args(std::vector<double>{p1.x, p2.x}, std::vector<double>{p1.y, p2.y}),
+    Args(
+      std::vector<double>{line_string[0].x(), line_string[1].x()},
+      std::vector<double>{line_string[0].y(), line_string[1].y()}),
     Kwargs("color"_a = "purple"));
 
   plt.scatter(
     Args(crossed.pose.position.x, crossed.pose.position.y),
     Kwargs("label"_a = "Crossed on trajectory", "color"_a = "purple"));
 
-  trajectory->longitudinal_velocity_mps.range(s.value(), trajectory->length()).set(0.0);
+  trajectory->longitudinal_velocity_mps().range(s.at(0), trajectory->length()).set(0.0);
 
   std::vector<double> x;
   std::vector<double> y;

diff --git a/common/autoware_trajectory/examples/example_trajectory_point.cpp b/common/autoware_trajectory/examples/example_trajectory_point.cpp
@@ -14,6 +14,9 @@
 
 #include "autoware/trajectory/interpolator/cubic_spline.hpp"
 #include "autoware/trajectory/point.hpp"
+#include "autoware/trajectory/utils/closest.hpp"
+#include "autoware/trajectory/utils/crossed.hpp"
+#include "lanelet2_core/primitives/LineString.h"
 
 #include <geometry_msgs/msg/point.hpp>
 
@@ -86,7 +89,8 @@ int main()
     p.x = 5.37;
     p.y = 6.0;
 
-    auto s = trajectory->closest(p);
+    double s = autoware::trajectory::closest(*trajectory, p);
+
     auto closest = trajectory->compute(s);
 
     plt.scatter(Args(p.x, p.y), Kwargs("color"_a = "green"));
@@ -98,18 +102,21 @@ int main()
       Kwargs("color"_a = "green"));
   }
   {
-    geometry_msgs::msg::Point p1;
-    geometry_msgs::msg::Point p2;
-    p1.x = 6.97;
-    p1.y = 6.36;
-    p2.x = 9.23;
-    p2.y = 5.92;
-
-    auto s = trajectory->crossed(p1, p2);
-    auto crossed = trajectory->compute(s.value());
+    lanelet::LineString2d line_string;
+    line_string.push_back(lanelet::Point3d(lanelet::InvalId, 6.97, 6.36, 0.0));
+    line_string.push_back(lanelet::Point3d(lanelet::InvalId, 9.23, 5.92, 0.0));
+
+    auto s = autoware::trajectory::crossed(*trajectory, line_string);
+    if (s.empty()) {
+      std::cerr << "Failed to find a crossing point" << std::endl;
+      return 1;
+    }
+    auto crossed = trajectory->compute(s.at(0));
 
     plt.plot(
-      Args(std::vector<double>{p1.x, p2.x}, std::vector<double>{p1.y, p2.y}),
+      Args(
+        std::vector<double>{line_string[0].x(), line_string[1].x()},
+        std::vector<double>{line_string[0].y(), line_string[1].y()}),
       Kwargs("color"_a = "purple"));
 
     plt.scatter(