feat(trajectory): improve shift function and their documents (#337)

* feat(trajectory): add populate function

Signed-off-by: Mamoru Sobue <[email protected]>

* update curvature figure for approximation desc

Signed-off-by: Mamoru Sobue <[email protected]>

* update align_orientation_with_trajectory_direction fig

Signed-off-by: Mamoru Sobue <[email protected]>

* finished trajectory classes

Signed-off-by: Mamoru Sobue <[email protected]>

* refactored shift

Signed-off-by: Mamoru Sobue <[email protected]>

* add comment

Signed-off-by: Mamoru Sobue <[email protected]>

* update error message

Signed-off-by: Mamoru Sobue <[email protected]>

---------

Signed-off-by: Mamoru Sobue <[email protected]>

Author: soblin

common/autoware_trajectory/CMakeLists.txt

@@ -12,13 +12,16 @@ ament_auto_add_library(autoware_trajectory SHARED
 
 if(BUILD_TESTING)
   find_package(ament_cmake_ros REQUIRED)
+  find_package(autoware_motion_utils REQUIRED)
+  include_directories(${autoware_motion_utils_INCLUDE_DIRS})
 
   file(GLOB_RECURSE test_files test/*.cpp)
 
   ament_add_ros_isolated_gtest(test_autoware_trajectory ${test_files})
 
   target_link_libraries(test_autoware_trajectory
     autoware_trajectory
+    ${autoware_motion_utils_LIBRARIES}
   )
 
   # Examples
@@ -27,20 +30,27 @@ if(BUILD_TESTING)
   add_definitions("-Wno-attributes")
   find_package(ament_lint_auto REQUIRED)
   find_package(autoware_pyplot REQUIRED)
+  find_package(autoware_utils_geometry REQUIRED)
   find_package(range-v3 REQUIRED)
+  find_package(autoware_test_utils REQUIRED)
   ament_lint_auto_find_test_dependencies()
   find_package(pybind11 REQUIRED)
   include_directories(${autoware_pyplot_INCLUDE_DIRS})
+  include_directories(${autoware_test_utils_INCLUDE_DIRS})
+  include_directories(${autoware_utils_geometry_INCLUDE_DIRS})
   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_trajectory
+      ${autoware_utils_geometry_LIBRARIES}
+      ${autoware_motion_utils_LIBRARIES}
       ${autoware_pyplot_LIBRARIES}
       range-v3::range-v3
       pybind11::embed
+      ${autoware_test_utils_LIBRARIES}
     )
   endforeach()
 endif()

common/autoware_trajectory/README.md

@@ -13,8 +13,10 @@
 // limitations under the License.
 
 #include "autoware/trajectory/pose.hpp"
+#include "autoware_utils_geometry/geometry.hpp"
 
 #include <autoware/pyplot/pyplot.hpp>
+#include <range/v3/all.hpp>
 #include <tf2/LinearMath/Quaternion.hpp>
 #include <tf2/LinearMath/Vector3.hpp>
 
@@ -23,6 +25,8 @@
 #include <pybind11/embed.h>
 #include <pybind11/stl.h>
 
+#include <algorithm>
+#include <string>
 #include <vector>
 
 geometry_msgs::msg::Pose pose(double x, double y)
@@ -34,11 +38,37 @@ geometry_msgs::msg::Pose pose(double x, double y)
   return p;
 }
 
+using autoware::trajectory::Trajectory;
+using ranges::to;
+using ranges::views::transform;
+
+void plot_trajectory_base_with_orientation(
+  const Trajectory<geometry_msgs::msg::Pose> & trajectory, const std::string & label,
+  autoware::pyplot::Axes & ax)
+{
+  const auto s = trajectory.get_underlying_bases();
+  const auto c = trajectory.compute(s);
+  const auto x =
+    c | transform([](const auto & point) { return point.position.x; }) | to<std::vector>();
+  const auto y =
+    c | transform([](const auto & point) { return point.position.y; }) | to<std::vector>();
+  const auto th =
+    c | transform([](const auto & quat) { return autoware_utils_geometry::get_rpy(quat).z; }) |
+    to<std::vector>();
+  const auto cos_th = th | transform([](const auto v) { return std::cos(v); }) | to<std::vector>();
+  const auto sin_th = th | transform([](const auto v) { return std::sin(v); }) | to<std::vector>();
+  ax.scatter(Args(x, y), Kwargs("color"_a = "red", "marker"_a = "o", "label"_a = "underlying"));
+  ax.quiver(Args(x, y, cos_th, sin_th), Kwargs("color"_a = "green", "label"_a = label));
+}
+
 int main()
 {
   pybind11::scoped_interpreter guard{};
 
   auto plt = autoware::pyplot::import();
+  auto [fig, axes] = plt.subplots(1, 2);
+  auto & ax1 = axes[0];
+  auto & ax2 = axes[1];
 
   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),
@@ -47,11 +77,12 @@ int main()
     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::trajectory::Trajectory;
-
   auto trajectory = Trajectory<geometry_msgs::msg::Pose>::Builder{}.build(poses);
 
+  plot_trajectory_base_with_orientation(*trajectory, "before", ax1);
   trajectory->align_orientation_with_trajectory_direction();
+  plot_trajectory_base_with_orientation(
+    *trajectory, "after align_orientation_with_trajectory_direction()", ax2);
 
   {
     std::vector<double> x;
@@ -63,34 +94,15 @@ int main()
       y.push_back(p.position.y);
     }
 
-    plt.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue"));
+    ax1.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue"));
+    ax2.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"));
+  fig.tight_layout();
+  for (auto & ax : axes) {
+    ax.set_aspect(Args("equal"));
+    ax.grid();
+    ax.legend();
   }
-
-  plt.axis(Args("equal"));
-  plt.grid();
-  plt.legend();
   plt.show();
 }

common/autoware_trajectory/examples/example_pose.cpp

@@ -12,17 +12,21 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "autoware/motion_utils/resample/resample.hpp"
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/trajectory/interpolator/akima_spline.hpp"
 #include "autoware/trajectory/interpolator/cubic_spline.hpp"
 #include "autoware/trajectory/interpolator/linear.hpp"
 #include "autoware/trajectory/interpolator/stairstep.hpp"
 #include "autoware/trajectory/pose.hpp"
+#include "autoware_utils_geometry/geometry.hpp"
 
 #include <autoware/pyplot/pyplot.hpp>
 #include <range/v3/all.hpp>
 #include <tf2/LinearMath/Quaternion.hpp>
 #include <tf2/LinearMath/Vector3.hpp>
 
+#include <autoware_internal_planning_msgs/msg/path_point_with_lane_id.hpp>
 #include <autoware_planning_msgs/msg/path_point.hpp>
 #include <geometry_msgs/msg/point.hpp>
 
@@ -509,7 +513,7 @@ int main_curvature()
 
   ax1.plot(Args(s, cubic.curvature(s)), Kwargs("color"_a = "navy", "label"_a = "curvature"));
   ax.quiver(
-    Args(points_x, points_y, cos_yaw, sin_yaw), Kwargs("color"_a = "green", "label"_a = "yaw"));
+    Args(points_x, points_y, cos_yaw, sin_yaw), Kwargs("color"_a = "green", "label"_a = "azimuth"));
 
   fig.tight_layout();
   for (auto & a : axes) {
@@ -525,7 +529,9 @@ int main_trajectory_overview()
 {
   using autoware::trajectory::Trajectory;
   using ranges::to;
+  using ranges::views::drop;
   using ranges::views::stride;
+  using ranges::views::take;
   using ranges::views::transform;
 
   auto plt = autoware::pyplot::import();
@@ -551,7 +557,7 @@ int main_trajectory_overview()
     return 0;
   }
   auto & trajectory = result.value();
-  const auto s = trajectory.base_arange(0.05);  // like numpy.arange
+  const auto s = trajectory.base_arange(0.05);  // cppcheck-suppress shadowVariable
   const auto C = trajectory.compute(s);
   const auto Cx = C | transform([&](const auto & p) { return p.x; }) | to<std::vector>();
   const auto Cy = C | transform([&](const auto & p) { return p.y; }) | to<std::vector>();
@@ -569,6 +575,28 @@ int main_trajectory_overview()
 
   ax1.plot(Args(s, trajectory.curvature(s)), Kwargs("color"_a = "purple", "label"_a = "curvature"));
 
+  // compare curvature with discrete version
+  autoware_internal_planning_msgs::msg::PathWithLaneId discrete_path;
+  for (unsigned i = 0; i < underlying_points.size(); ++i) {
+    const auto & point = underlying_points.at(i);
+    const auto s = trajectory.get_underlying_bases().at(i);  // cppcheck-suppress shadowVariable
+    autoware_internal_planning_msgs::msg::PathPointWithLaneId point_with_lane_id;
+    point_with_lane_id.point.pose.position = point;
+    point_with_lane_id.point.pose.orientation =
+      autoware_utils_geometry::create_quaternion_from_yaw(trajectory.azimuth(s));
+    discrete_path.points.push_back(point_with_lane_id);
+  }
+  const auto discrete_interpolated_path =
+    autoware::motion_utils::resamplePath(discrete_path, s /* arc lengths for this interpolation */);
+  const auto discrete_curvature =
+    autoware::motion_utils::calcCurvature(discrete_interpolated_path.points);
+  const auto discrete_curvature_bases = s | drop(1) | take(s.size() - 2) | to<std::vector>();
+  ax1.plot(
+    Args(s, discrete_curvature),
+    Kwargs(
+      "color"_a = "blue",
+      "label"_a = "discrete approximation using menger curvature"));  // # cspell: ignore menger
+
   const auto points_x = underlying_points |
                         ranges::views::transform([&](const auto & p) { return p.x; }) |
                         ranges::to<std::vector>();