feat(autoware_motion_utils): add new trajectory class (#8693)

* feat(autoware_motion_utils): add interpolator Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * use int32_t instead of int Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * use int32_t instead of int Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * use int32_t instead of int Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * add const as much as possible and use `at()` in `vector` Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * fix directory name Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * refactor code and add example Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * update Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * remove unused include Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * refactor code Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * add clone function Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * fix stairstep Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * make constructor to public Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * feat(autoware_motion_utils): add trajectory class Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * Update CMakeLists.txt * fix Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * fix package.xml Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * update crop Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * revert crtp change Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * update package.xml Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * updating... Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * update Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * solve build problem Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * style(pre-commit): autofix --------- Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Nov 6, 2024 · 32313df · 32313df
1 parent f0f5736
commit 32313df
Show file tree

Hide file tree

Showing 18 changed files with 2,093 additions and 3 deletions.
diff --git a/common/autoware_motion_utils/examples/example_interpolator.cpp b/common/autoware_motion_utils/examples/example_interpolator.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 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.
@@ -17,8 +17,7 @@
 #include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp"
 #include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp"
 #include "autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp"
-
-#include <autoware/motion_utils/trajectory_container/interpolator.hpp>
+#include "autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp"
 
 #include <matplotlibcpp17/pyplot.h>
 

diff --git a/common/autoware_motion_utils/examples/example_trajectory_path_point.cpp b/common/autoware_motion_utils/examples/example_trajectory_path_point.cpp
@@ -0,0 +1,112 @@
+// 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/trajectory/trajectory_path_point.hpp"
+
+#include <autoware_planning_msgs/msg/path_point.hpp>
+#include <geometry_msgs/msg/point.hpp>
+
+#include <matplotlibcpp17/pyplot.h>
+
+#include <vector>
+
+autoware_planning_msgs::msg::PathPoint pose(double x, double y)
+{
+  autoware_planning_msgs::msg::PathPoint p;
+  p.pose.position.x = x;
+  p.pose.position.y = y;
+  p.longitudinal_velocity_mps = 1.0;
+  return p;
+}
+
+int main()
+{
+  using autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer;
+
+  pybind11::scoped_interpreter guard{};
+
+  auto plt = matplotlibcpp17::pyplot::import();
+
+  std::vector<autoware_planning_msgs::msg::PathPoint> points = {
+    pose(0.49, 0.59), pose(0.61, 1.22), pose(0.86, 1.93), pose(1.20, 2.56), pose(1.51, 3.17),
+    pose(1.85, 3.76), pose(2.14, 4.26), pose(2.60, 4.56), pose(3.07, 4.55), pose(3.61, 4.30),
+    pose(3.95, 4.01), pose(4.29, 3.68), pose(4.90, 3.25), pose(5.54, 3.10), pose(6.24, 3.18),
+    pose(6.88, 3.54), pose(7.51, 4.25), pose(7.85, 4.93), pose(8.03, 5.73), pose(8.16, 6.52),
+    pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)};
+
+  {
+    std::vector<double> x;
+    std::vector<double> y;
+
+    for (const auto & p : points) {
+      x.push_back(p.pose.position.x);
+      y.push_back(p.pose.position.y);
+    }
+
+    plt.scatter(Args(x, y), Kwargs("label"_a = "Original", "color"_a = "red"));
+  }
+
+  auto trajectory =
+    TrajectoryContainer<autoware_planning_msgs::msg::PathPoint>::Builder{}.build(points);
+
+  if (!trajectory) {
+    std::cerr << "Failed to build trajectory" << std::endl;
+    return 1;
+  }
+
+  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;
+
+  auto s = trajectory->crossed(p1, p2);
+  auto crossed = trajectory->compute(s.value());
+
+  plt.plot(
+    Args(std::vector<double>{p1.x, p2.x}, std::vector<double>{p1.y, p2.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(*s, trajectory->length()) = 0.0;
+
+  std::vector<double> x;
+  std::vector<double> y;
+  std::vector<double> x_stopped;
+  std::vector<double> y_stopped;
+
+  for (double i = 0.0; i <= trajectory->length(); i += 0.005) {
+    auto p = trajectory->compute(i);
+    if (p.longitudinal_velocity_mps > 0.0) {
+      x.push_back(p.pose.position.x);
+      y.push_back(p.pose.position.y);
+    } else {
+      x_stopped.push_back(p.pose.position.x);
+      y_stopped.push_back(p.pose.position.y);
+    }
+  }
+
+  plt.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue"));
+  plt.plot(Args(x_stopped, y_stopped), Kwargs("label"_a = "Stopped", "color"_a = "orange"));
+
+  plt.axis(Args("equal"));
+  plt.legend();
+  plt.show();
+}
diff --git a/common/autoware_motion_utils/examples/example_trajectory_point.cpp b/common/autoware_motion_utils/examples/example_trajectory_point.cpp
@@ -0,0 +1,133 @@
+// 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 "autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp"
+
+#include <geometry_msgs/msg/point.hpp>
+
+#include <matplotlibcpp17/pyplot.h>
+
+#include <iostream>
+#include <vector>
+
+geometry_msgs::msg::Point pose(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();
+
+  std::vector<geometry_msgs::msg::Point> points = {
+    pose(0.49, 0.59), pose(0.61, 1.22), pose(0.86, 1.93), pose(1.20, 2.56), pose(1.51, 3.17),
+    pose(1.85, 3.76), pose(2.14, 4.26), pose(2.60, 4.56), pose(3.07, 4.55), pose(3.61, 4.30),
+    pose(3.95, 4.01), pose(4.29, 3.68), pose(4.90, 3.25), pose(5.54, 3.10), pose(6.24, 3.18),
+    pose(6.88, 3.54), pose(7.51, 4.25), pose(7.85, 4.93), pose(8.03, 5.73), pose(8.16, 6.52),
+    pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)};
+
+  {
+    std::vector<double> x;
+    std::vector<double> y;
+
+    for (const auto & p : points) {
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+
+    plt.scatter(Args(x, y), Kwargs("label"_a = "Original", "color"_a = "red"));
+  }
+
+  using autoware::motion_utils::trajectory_container::interpolator::CubicSpline;
+  using autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer;
+
+  auto trajectory = TrajectoryContainer<geometry_msgs::msg::Point>::Builder()
+                      .set_xy_interpolator<CubicSpline>()
+                      .set_z_interpolator<CubicSpline>()
+                      .build(points);
+  std::cout << "Trajectory length: " << trajectory->length() << std::endl;
+
+  trajectory->crop(2.0, trajectory->length() - 4.0);
+
+  std::cout << "Trajectory length after cropping: " << trajectory->length() << std::endl;
+
+  {
+    std::vector<double> x;
+    std::vector<double> y;
+
+    for (double i = 0.0; i <= trajectory->length(); i += 0.01) {
+      auto p = trajectory->compute(i);
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+
+    plt.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue"));
+  }
+
+  {
+    geometry_msgs::msg::Point p;
+    p.x = 5.37;
+    p.y = 6.0;
+
+    auto s = trajectory->closest(p);
+    auto closest = trajectory->compute(s);
+
+    plt.scatter(Args(p.x, p.y), Kwargs("color"_a = "green"));
+    plt.scatter(
+      Args(closest.x, closest.y), Kwargs("label"_a = "Closest on trajectory", "color"_a = "green"));
+
+    plt.plot(
+      Args(std::vector<double>{p.x, closest.x}, std::vector<double>{p.y, closest.y}),
+      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());
+
+    plt.plot(
+      Args(std::vector<double>{p1.x, p2.x}, std::vector<double>{p1.y, p2.y}),
+      Kwargs("color"_a = "purple"));
+
+    plt.scatter(
+      Args(crossed.x, crossed.y),
+      Kwargs("label"_a = "Crossed on trajectory", "color"_a = "purple"));
+  }
+  {
+    auto restored = trajectory->restore(50);
+    std::vector<double> x;
+    std::vector<double> y;
+    for (const auto & p : restored) {
+      x.push_back(p.x);
+      y.push_back(p.y);
+    }
+    plt.scatter(Args(x, y), Kwargs("label"_a = "Restored", "color"_a = "orange", "marker"_a = "x"));
+  }
+
+  plt.axis(Args("equal"));
+  plt.legend();
+  plt.show();
+}
diff --git a/common/autoware_motion_utils/examples/example_trajectory_pose.cpp b/common/autoware_motion_utils/examples/example_trajectory_pose.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/trajectory/trajectory_pose.hpp"
+
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <matplotlibcpp17/pyplot.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
+
+#include <vector>
+
+geometry_msgs::msg::Pose pose(double x, double y)
+{
+  geometry_msgs::msg::Pose p;
+  p.position.x = x;
+  p.position.y = y;
+  p.position.z = 0.0;
+  return p;
+}
+
+int main()
+{
+  pybind11::scoped_interpreter guard{};
+
+  auto plt = matplotlibcpp17::pyplot::import();
+
+  std::vector<geometry_msgs::msg::Pose> poses = {
+    pose(0.49, 0.59), pose(0.61, 1.22), pose(0.86, 1.93), pose(1.20, 2.56), pose(1.51, 3.17),
+    pose(1.85, 3.76), pose(2.14, 4.26), pose(2.60, 4.56), pose(3.07, 4.55), pose(3.61, 4.30),
+    pose(3.95, 4.01), pose(4.29, 3.68), pose(4.90, 3.25), pose(5.54, 3.10), pose(6.24, 3.18),
+    pose(6.88, 3.54), pose(7.51, 4.25), pose(7.85, 4.93), pose(8.03, 5.73), pose(8.16, 6.52),
+    pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)};
+
+  using autoware::motion_utils::trajectory_container::interpolator::CubicSpline;
+  using autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer;
+
+  auto trajectory = TrajectoryContainer<geometry_msgs::msg::Pose>::Builder{}.build(poses);
+
+  trajectory->align_orientation_with_trajectory_direction();
+
+  {
+    std::vector<double> x;
+    std::vector<double> y;
+
+    for (double i = 0.0; i <= trajectory->length(); i += 0.01) {
+      auto p = trajectory->compute(i);
+      x.push_back(p.position.x);
+      y.push_back(p.position.y);
+    }
+
+    plt.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue"));
+  }
+
+  {
+    std::vector<double> x;
+    std::vector<double> y;
+    std::vector<double> dx;
+    std::vector<double> dy;
+
+    for (double i = 0.0; i <= trajectory->length(); i += 1.0) {
+      auto p = trajectory->compute(i);
+      x.push_back(p.position.x);
+      y.push_back(p.position.y);
+
+      tf2::Vector3 x_axis(1.0, 0.0, 0.0);
+      tf2::Quaternion q(p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
+      tf2::Vector3 direction = tf2::quatRotate(q, x_axis);
+      dx.push_back(direction.x());
+      dy.push_back(direction.y());
+      // double azimuth = trajectory->azimuth(i);
+      // dx.push_back(std::cos(azimuth));
+      // dy.push_back(std::sin(azimuth));
+    }
+    plt.quiver(Args(x, y, dx, dy), Kwargs("label"_a = "Direction", "color"_a = "green"));
+  }
+
+  plt.axis(Args("equal"));
+  plt.legend();
+  plt.show();
+}
diff --git a/...n/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp b/...n/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp
@@ -0,0 +1,21 @@
+// 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__TRAJECTORY_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY_HPP_
+#include <autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp>
+#include <autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp>
+#include <autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp>
+#include <autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp>
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY_HPP_