xs1nus motion planner package (#196)

* feat: create route_follower package

* feat: add proper topic handlers

* routing node: fix transforms

* feat: create route_follower package

* feat: add proper topic handlers

* feat: mvp route_follower

* refactor: extracted trajectory creation logic

* * fix: publish grid cost map
* feat: publish on timer, not on subscription

* style: format files

* - feat: add PolylineIndex class.
- feat: introduce concepts in the common package
- feat: add generalized template for linear interpolation.

* style: fix linter errors

* - refactor: split PolylineIndex-related structs into different headers

* test: add basic test

* fix: resolve several PR conversations

 - refactor MotionState to be more consistent with the Pose
 - add dist field to AdvanceResult
 - refined PolyLineIndex interface
 - add more tests for PolyLineIndex
 - refactor makeTrajectory in route_follower_node.cpp

* refactor: reverted lerp<V> to interpolate overloads

* refactor: refined polyline index

  - separate interfaces for getting distance and state
  - changed logic of advance and advanceFromBegin

* fix: small edits of routefollower

* refactor: completely dissociated lerp and it's derivatives

* fix: remove concepts from the common package

* refactor: general improvements and fixes

* fix: uncomment lines that include icp_odometry in truck/launch/slam.yaml

* style: refine comment text

---------

Co-authored-by: apmilko <[email protected]>

packages/geom/CMakeLists.txt

@@ -24,6 +24,7 @@ add_library(
   src/distance.cpp
   src/intersection.cpp
   src/line.cpp
+  src/motion_state.cpp
   src/msg.cpp
   src/polygon.cpp
   src/polyline.cpp

packages/geom/include/geom/bezier.h

@@ -1,15 +1,17 @@
-#include "geom/pose.h"
+#pragma once
+
+#include "geom/motion_state.h"
 #include "geom/vector.h"
 
 namespace truck::geom {
 
-Poses bezier1(const Vec2& p0, const Vec2& p1, size_t n);
-Poses bezier1(const Vec2& p0, const Vec2& p1, double step);
+MotionStates bezier1(const Vec2& p0, const Vec2& p1, size_t n);
+MotionStates bezier1(const Vec2& p0, const Vec2& p1, double step);
 
-Poses bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, size_t n);
-Poses bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, double step);
+MotionStates bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, size_t n);
+MotionStates bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, double step);
 
-Poses bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, size_t n);
-Poses bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, double step);
+MotionStates bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, size_t n);
+MotionStates bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, double step);
 
 }  // namespace truck::geom

packages/geom/include/geom/common.h

@@ -1,9 +1,13 @@
 #pragma once
 
 #include <cmath>
+#include "common/math.h"
 
 namespace truck::geom {
 
 inline bool equal(double a, double b, double eps = 0) noexcept { return std::abs(a - b) <= eps; }
-
+inline double interpolate(double a, double b, double t) noexcept {
+    VERIFY(0 <= t && t <= 1);
+    return (1 - t) * a + t * b;
+}
 }  // namespace truck::geom

packages/geom/include/geom/distance.h

@@ -3,6 +3,7 @@
 #include "geom/line.h"
 #include "geom/segment.h"
 #include "geom/vector.h"
+#include "geom/motion_state.h"
 
 namespace truck::geom {
 
@@ -22,4 +23,8 @@ double distance(const Line& l, const Vec2& p) noexcept;
 
 double distance(const Vec2& p, const Line& l) noexcept;
 
+double distanceSq(const MotionState& a, const MotionState& b) noexcept;
+
+double distance(const MotionState& a, const MotionState& b) noexcept;
+
 }  // namespace truck::geom

packages/geom/include/geom/motion_state.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "geom/pose.h"
+#include "geom/common.h"
+#include <vector>
+
+namespace truck::geom {
+
+struct MotionState {
+    Vec2 pos;
+    AngleVec2 dir;
+    double curvature = 0;
+
+    Pose pose() const { return Pose{.pos = pos, .dir = dir}; }
+};
+
+using MotionStates = std::vector<MotionState>;
+
+Poses toPoses(const MotionStates& states);
+
+struct MotionStateLinearInterpolator {
+    MotionState operator()(const MotionState& from, const MotionState& to, double t) const {
+        return {
+            .pos = interpolate(from.pos, to.pos, t),
+            .dir = interpolate(from.dir, to.dir, t),
+            .curvature = interpolate(from.curvature, to.curvature, t),
+        };
+    }
+
+    double operator()(double from, const double to, double t) const {
+        return interpolate(from, to, t);
+    }
+};
+
+}  // namespace truck::geom

packages/geom/include/geom/polyline_index.h

@@ -0,0 +1,101 @@
+#pragma once
+
+#include "geom/pose.h"
+#include "geom/distance.h"
+#include "geom/motion_state.h"
+#include "common/exception.h"
+#include <vector>
+#include <optional>
+
+namespace truck::geom {
+
+struct PolylineIdx {
+    std::size_t seg_n = 0;
+    double t = 0;  // [0, 1]
+};
+
+template<class P>
+struct AdvanceResult {
+    P point;
+    double dist = 0.0;
+    PolylineIdx poly_idx;
+    bool reached_end = false;
+};
+
+template<typename P, typename Interpolator>
+class PolylineIndex {
+  public:
+    PolylineIndex(std::vector<P>&& range) : points_(std::move(range)) {
+        VERIFY(points_.size() >= 2);
+
+        distances_.reserve(points_.size());
+        distances_.push_back(0);
+
+        auto prev = points_.begin();
+        for (auto curr = points_.begin() + 1; curr != points_.end(); prev = curr, ++curr) {
+            distances_.push_back(geom::distance(*prev, *curr) + distances_.back());
+        }
+    }
+
+    P StateAt(const PolylineIdx idx) const {
+        const auto& a = points_[idx.seg_n];
+        const auto& b = points_[idx.seg_n + 1];
+        return Interpolator{}(a, b, idx.t);
+    }
+
+    double DistAt(const PolylineIdx idx) const {
+        const auto& a = distances_[idx.seg_n];
+        const auto& b = distances_[idx.seg_n + 1];
+        return a * (1 - idx.t) + b * idx.t;
+    }
+
+    std::pair<P, double> At(const PolylineIdx idx) const {
+        return std::make_pair(StateAt(idx), DistAt(idx));
+    }
+
+    // default parameter to use in the loop initialization
+    AdvanceResult<P> AdvanceFromBegin(double dist = 0) const {
+        if (dist >= Length()) {
+            return AdvanceResult<P>{
+                .point = points_.back(),
+                .dist = Length(),
+                .poly_idx = {.seg_n = points_.size() - 1, .t = 1},
+                .reached_end = true};
+        }
+
+        // index of the first point at which distance traveled exceeds `target`
+        size_t index = std::distance(
+            distances_.begin(), std::upper_bound(distances_.begin(), distances_.end(), dist));
+
+        VERIFY(index < distances_.size());
+
+        --index;
+
+        const double rem_dist = dist - distances_[index];
+        const double seg_len = geom::distance(points_[index], points_[index + 1]);
+        const double t = seg_len ? clamp(rem_dist / seg_len, 0., 1.) : 0;
+
+        PolylineIdx polyidx = {.seg_n = index, .t = t};
+
+        return AdvanceResult<P>{
+            .point = StateAt(polyidx), .dist = dist, .poly_idx = polyidx, .reached_end = false};
+    }
+
+    AdvanceResult<P> AdvanceFrom(const PolylineIdx from_id, double dist) const {
+        double start = DistAt(from_id);
+        return AdvanceFromBegin(start + dist);
+    }
+
+    double Length() const { return distances_.back(); }
+
+    const auto& Points() const { return points_; }
+    const auto& Distances() const { return distances_; }
+
+  private:
+    std::vector<P> points_;
+    std::vector<double> distances_;
+};
+
+using PolylineMotionIndex = PolylineIndex<MotionState, MotionStateLinearInterpolator>;
+
+}  // namespace truck::geom

packages/geom/include/geom/pose.h

@@ -2,6 +2,7 @@
 
 #include "geom/angle_vector.h"
 #include "geom/vector.h"
+#include "geom/common.h"
 
 #include <ostream>
 #include <vector>

packages/geom/src/bezier.cpp

@@ -5,93 +5,103 @@
 
 namespace truck::geom {
 
-Poses bezier1(const Vec2& p0, const Vec2& p1, size_t n) {
+MotionStates bezier1(const Vec2& p0, const Vec2& p1, size_t n) {
     VERIFY(n > 2);
 
     const auto dir = AngleVec2::fromVector(p1 - p0);
 
-    Poses poses;
-    poses.reserve(n);
+    MotionStates states;
+    states.reserve(n);
 
-    poses.emplace_back(p0, dir);
-
-    for (size_t i = 1; i < n - 1; ++i) {
+    for (size_t i = 0; i < n; ++i) {
         const double t = static_cast<double>(i) / (n - 1);
         const double t_1 = 1 - t;
-        poses.emplace_back(p0 * t_1 + p1 * t, dir);
+        states.push_back(MotionState{.pos = interpolate(p0, p1, t), .dir = dir, .curvature = 0});
     }
 
-    poses.emplace_back(p1, dir);
-
-    return poses;
+    return states;
 }
 
-Poses bezier1(const Vec2& p0, const Vec2& p1, double step) {
+MotionStates bezier1(const Vec2& p0, const Vec2& p1, double step) {
     VERIFY(step > 0);
 
     const double dist = (p1 - p0).len();
     const size_t n = 1 + ceil<size_t>(dist / step);
     return bezier1(p0, p1, n);
 }
 
-Poses bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, size_t n) {
+MotionStates bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, size_t n) {
     VERIFY(n > 2);
 
-    Poses poses;
-    poses.reserve(n);
-
-    poses.emplace_back(p0, AngleVec2::fromVector(p1 - p0));
+    MotionStates states;
+    states.reserve(n);
 
-    for (size_t i = 1; i < n - 1; ++i) {
-        const double t = static_cast<double>(i) / (n - 1);
+    auto eval = [&](const Vec2& p0, const Vec2& p1, const Vec2& p2, double t) -> MotionState {
         const double t_1 = 1 - t;
 
-        const auto pos = p0 * t_1 * t_1 + p1 * 2 * t * t_1 + p2 * t * t;
+        const auto position = p0 * t_1 * t_1 + p1 * 2 * t * t_1 + p2 * t * t;
         const auto derivative = 2 * t_1 * (p1 - p0) + 2 * t * (p2 - p1);
+        const auto second_derivative = 2 * (p2 - 2 * p1 + p0);
+        const double curvature = cross(derivative, second_derivative) / pow(derivative.len(), 3);
+
+        return MotionState{
+            .pos = position,
+            .dir = AngleVec2::fromVector(derivative),
+            .curvature = curvature,
+        };
+    };
 
-        poses.emplace_back(pos, AngleVec2::fromVector(derivative));
+    for (size_t i = 0; i < n; ++i) {
+        const double t = static_cast<double>(i) / (n - 1);
+        states.push_back(eval(p0, p1, p2, t));
     }
 
-    poses.emplace_back(p2, AngleVec2::fromVector(p2 - p1));
-
-    return poses;
+    return states;
 }
 
-Poses bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, double step) {
+MotionStates bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, double step) {
     VERIFY(step > 0);
 
     const double dist = (p1 - p0).len() + (p2 - p1).len();
     const size_t n = 1 + ceil<size_t>(dist / step);
     return bezier2(p0, p1, p2, n);
 }
 
-Poses bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, size_t n) {
+MotionStates bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, size_t n) {
     VERIFY(n > 2);
 
-    Poses poses;
-    poses.reserve(n);
-
-    poses.emplace_back(p0, AngleVec2::fromVector(p1 - p0));
+    std::vector<MotionState> states;
+    states.reserve(n);
 
-    for (size_t i = 1; i < n - 1; ++i) {
-        const double t = static_cast<double>(i) / (n - 1);
+    auto eval = [&](const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, double t)
+        -> MotionState {
         const double t2 = t * t;
 
         const double t_1 = 1 - t;
         const double t_2 = t_1 * t_1;
 
-        const auto pos = p0 * t_2 * t_1 + p1 * 3 * t * t_2 + p2 * 3 * t2 * t_1 + p3 * t2 * t;
+        const auto position = p0 * t_2 * t_1 + p1 * 3 * t * t_2 + p2 * 3 * t2 * t_1 + p3 * t2 * t;
         const auto derivative = 3 * t_2 * (p1 - p0) + 6 * t_1 * t * (p2 - p1) + 3 * t2 * (p3 - p2);
-
-        poses.emplace_back(pos, AngleVec2::fromVector(derivative));
+        const auto second_derivative =
+            6 * (1 - t) * (p2 - 2 * p1 + p0) + 6 * t * (p3 - 2 * p2 + p1);
+        const double curvature = cross(derivative, second_derivative) / pow(derivative.len(), 3);
+
+        return MotionState{
+            .pos = position,
+            .dir = AngleVec2::fromVector(derivative),
+            .curvature = curvature,
+        };
     };
 
-    poses.emplace_back(p3, AngleVec2::fromVector(p3 - p2));
+    for (size_t i = 0; i < n; ++i) {
+        const double t = static_cast<double>(i) / (n - 1);
+        states.push_back(eval(p0, p1, p2, p3, t));
+    }
 
-    return poses;
+    return states;
 }
 
-Poses bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, double step) {
+MotionStates bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, double step) {
     VERIFY(step > 0);
 
     const double dist = (p1 - p0).len() + (p2 - p1).len() + (p3 - p2).len();

packages/geom/src/distance.cpp

@@ -42,4 +42,12 @@ double distance(const Line& l, const Vec2& p) noexcept {
 
 double distance(const Vec2& p, const Line& l) noexcept { return distance(l, p); }
 
+double distanceSq(const MotionState& a, const MotionState& b) noexcept {
+    return distanceSq(a.pos, b.pos);
+}
+
+double distance(const MotionState& a, const MotionState& b) noexcept {
+    return distance(a.pos, b.pos);
+}
+
 }  // namespace truck::geom

packages/geom/src/motion_state.cpp

@@ -0,0 +1,14 @@
+#include "geom/motion_state.h"
+
+namespace truck::geom {
+Poses toPoses(const MotionStates& states) {
+    Poses poses(states.size());
+
+    std::transform(states.begin(), states.end(), poses.begin(), [](const MotionState& state) {
+        return state.pose();
+    });
+
+    return poses;
+}
+
+}  // namespace truck::geom

packages/geom/src/pose.cpp

@@ -1,3 +1,4 @@
+#include "geom/common.h"
 #include "geom/pose.h"
 
 #include "common/exception.h"

packages/geom/src/segment.cpp

@@ -5,7 +5,7 @@
 
 namespace truck::geom {
 
-Poses Segment::trace(double step) const noexcept { return bezier1(begin, end, step); }
+Poses Segment::trace(double step) const noexcept { return toPoses(bezier1(begin, end, step)); }
 
 Vec2 Segment::pos(double t) const noexcept {
     t = clamp(t, 0.0, 1.0);