From 278b5dd1aee808351b9b50a85360ec9ad5ad2fe1 Mon Sep 17 00:00:00 2001 From: xs1nus <54557665+funham@users.noreply.github.com> Date: Tue, 18 Feb 2025 13:56:54 +0300 Subject: [PATCH] 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 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 --- packages/geom/CMakeLists.txt | 1 + packages/geom/include/geom/bezier.h | 16 +- packages/geom/include/geom/common.h | 6 +- packages/geom/include/geom/distance.h | 5 + packages/geom/include/geom/motion_state.h | 35 +++ packages/geom/include/geom/polyline_index.h | 101 +++++++ packages/geom/include/geom/pose.h | 1 + packages/geom/src/bezier.cpp | 84 +++--- packages/geom/src/distance.cpp | 8 + packages/geom/src/motion_state.cpp | 14 + packages/geom/src/pose.cpp | 1 + packages/geom/src/segment.cpp | 2 +- packages/geom/test/CMakeLists.txt | 1 + packages/geom/test/polyline_index_tests.cpp | 53 ++++ packages/motion/src/primitive.cpp | 4 +- packages/route_follower/CMakeLists.txt | 41 +++ .../route_follower/route_follower_node.h | 90 ++++++ .../route_follower/launch/route_follower.yaml | 22 ++ packages/route_follower/package.xml | 24 ++ packages/route_follower/src/main.cpp | 9 + .../src/route_follower_node.cpp | 261 ++++++++++++++++++ packages/routing/CMakeLists.txt | 2 + .../routing/include/routing/routing_node.h | 11 + packages/routing/package.xml | 1 + packages/routing/src/routing_node.cpp | 39 ++- .../truck/launch/route_follower_demo.yaml | 20 ++ packages/truck/launch/routing_demo.yaml | 11 +- packages/truck/launch/slam.yaml | 9 +- .../visualization/src/visualization_node.cpp | 5 +- 29 files changed, 815 insertions(+), 62 deletions(-) create mode 100644 packages/geom/include/geom/motion_state.h create mode 100644 packages/geom/include/geom/polyline_index.h create mode 100644 packages/geom/src/motion_state.cpp create mode 100644 packages/geom/test/polyline_index_tests.cpp create mode 100644 packages/route_follower/CMakeLists.txt create mode 100644 packages/route_follower/include/route_follower/route_follower_node.h create mode 100644 packages/route_follower/launch/route_follower.yaml create mode 100644 packages/route_follower/package.xml create mode 100644 packages/route_follower/src/main.cpp create mode 100644 packages/route_follower/src/route_follower_node.cpp create mode 100644 packages/truck/launch/route_follower_demo.yaml diff --git a/packages/geom/CMakeLists.txt b/packages/geom/CMakeLists.txt index d8910fc19..74fcd779d 100644 --- a/packages/geom/CMakeLists.txt +++ b/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 diff --git a/packages/geom/include/geom/bezier.h b/packages/geom/include/geom/bezier.h index 1006fbd32..2b8f68c2c 100644 --- a/packages/geom/include/geom/bezier.h +++ b/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 diff --git a/packages/geom/include/geom/common.h b/packages/geom/include/geom/common.h index d8558c0f9..0e49ab8fc 100644 --- a/packages/geom/include/geom/common.h +++ b/packages/geom/include/geom/common.h @@ -1,9 +1,13 @@ #pragma once #include +#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 diff --git a/packages/geom/include/geom/distance.h b/packages/geom/include/geom/distance.h index 4043bc039..bfcf1bad2 100644 --- a/packages/geom/include/geom/distance.h +++ b/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 diff --git a/packages/geom/include/geom/motion_state.h b/packages/geom/include/geom/motion_state.h new file mode 100644 index 000000000..e0b9ff700 --- /dev/null +++ b/packages/geom/include/geom/motion_state.h @@ -0,0 +1,35 @@ +#pragma once + +#include "geom/pose.h" +#include "geom/common.h" +#include + +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; + +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 diff --git a/packages/geom/include/geom/polyline_index.h b/packages/geom/include/geom/polyline_index.h new file mode 100644 index 000000000..18eeaf562 --- /dev/null +++ b/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 +#include + +namespace truck::geom { + +struct PolylineIdx { + std::size_t seg_n = 0; + double t = 0; // [0, 1] +}; + +template +struct AdvanceResult { + P point; + double dist = 0.0; + PolylineIdx poly_idx; + bool reached_end = false; +}; + +template +class PolylineIndex { + public: + PolylineIndex(std::vector

&& 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 At(const PolylineIdx idx) const { + return std::make_pair(StateAt(idx), DistAt(idx)); + } + + // default parameter to use in the loop initialization + AdvanceResult

AdvanceFromBegin(double dist = 0) const { + if (dist >= Length()) { + return AdvanceResult

{ + .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

{ + .point = StateAt(polyidx), .dist = dist, .poly_idx = polyidx, .reached_end = false}; + } + + AdvanceResult

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

points_; + std::vector distances_; +}; + +using PolylineMotionIndex = PolylineIndex; + +} // namespace truck::geom diff --git a/packages/geom/include/geom/pose.h b/packages/geom/include/geom/pose.h index 7a1ae4ba1..0eca6efe3 100644 --- a/packages/geom/include/geom/pose.h +++ b/packages/geom/include/geom/pose.h @@ -2,6 +2,7 @@ #include "geom/angle_vector.h" #include "geom/vector.h" +#include "geom/common.h" #include #include diff --git a/packages/geom/src/bezier.cpp b/packages/geom/src/bezier.cpp index 6bc650b26..4cd5f7db4 100644 --- a/packages/geom/src/bezier.cpp +++ b/packages/geom/src/bezier.cpp @@ -5,28 +5,24 @@ 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(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(); @@ -34,30 +30,36 @@ Poses bezier1(const Vec2& p0, const Vec2& p1, double 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(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(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(); @@ -65,33 +67,41 @@ Poses bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, double 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 states; + states.reserve(n); - for (size_t i = 1; i < n - 1; ++i) { - const double t = static_cast(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(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(); diff --git a/packages/geom/src/distance.cpp b/packages/geom/src/distance.cpp index 4af418c3b..58b85a7e0 100644 --- a/packages/geom/src/distance.cpp +++ b/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 diff --git a/packages/geom/src/motion_state.cpp b/packages/geom/src/motion_state.cpp new file mode 100644 index 000000000..09f55ad69 --- /dev/null +++ b/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 diff --git a/packages/geom/src/pose.cpp b/packages/geom/src/pose.cpp index 8fbaa2f21..9151a0ee6 100644 --- a/packages/geom/src/pose.cpp +++ b/packages/geom/src/pose.cpp @@ -1,3 +1,4 @@ +#include "geom/common.h" #include "geom/pose.h" #include "common/exception.h" diff --git a/packages/geom/src/segment.cpp b/packages/geom/src/segment.cpp index 7d063f3c2..3ae454e29 100644 --- a/packages/geom/src/segment.cpp +++ b/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); diff --git a/packages/geom/test/CMakeLists.txt b/packages/geom/test/CMakeLists.txt index 1ca2cee5a..2d409fc17 100644 --- a/packages/geom/test/CMakeLists.txt +++ b/packages/geom/test/CMakeLists.txt @@ -14,6 +14,7 @@ ament_add_gtest( uniform_stepper_tests.cpp vector_tests.cpp vector3_tests.cpp + polyline_index_tests.cpp ) target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) diff --git a/packages/geom/test/polyline_index_tests.cpp b/packages/geom/test/polyline_index_tests.cpp new file mode 100644 index 000000000..689864398 --- /dev/null +++ b/packages/geom/test/polyline_index_tests.cpp @@ -0,0 +1,53 @@ +#include + +#include "geom/polyline_index.h" +#include "geom/bezier.h" +#include "geom/test/equal_assert.h" + +using namespace truck::geom; + +TEST(PolylineIndex, it_works) { + const PolylineIndex polyline = + bezier3({0, 0}, {1, 1}, {2, 1}, {3, 0}, size_t(60)); + + double dist = 0; + const double dist_inc = .1; + + AdvanceResult res = polyline.AdvanceFromBegin(); + ASSERT_GEOM_EQUAL(Vec2{0, 0}, res.point.pos); + + while (!res.reached_end) { + res = polyline.AdvanceFrom(res.poly_idx, dist_inc); + dist += dist_inc; + } + + ASSERT_GEOM_EQUAL(Vec2{3, 0}, res.point.pos); + ASSERT_GEOM_EQUAL(dist, polyline.Length(), dist_inc); +} + +TEST(PolylineIndex, even_steps) { + const PolylineIndex polyline = + bezier3({0, -5}, {10, -5}, {0, 5}, {-10, 5}, size_t(1023)); + const double dist_inc = .2; + + Vec2 prev = polyline.AdvanceFromBegin().point.pos; + + for (auto it = polyline.AdvanceFromBegin(dist_inc); !it.reached_end; + it = polyline.AdvanceFrom(it.poly_idx, dist_inc)) { + ASSERT_GEOM_EQUAL(distance(prev, it.point.pos), dist_inc, 1e-3); + prev = it.point.pos; + } +} + +TEST(PolylineIndex, edge_case) { + const PolylineIndex polyline = + bezier3({0, 0}, {1, 1}, {2, 1}, {3, 0}, .1); + + const auto end = polyline.AdvanceFromBegin(polyline.Length() + 1e3); + ASSERT_TRUE(end.reached_end); + + const auto also_end = polyline.AdvanceFrom(end.poly_idx, 1e3); + ASSERT_TRUE(also_end.reached_end); + + ASSERT_GEOM_EQUAL(end.point.pos, also_end.point.pos); +} diff --git a/packages/motion/src/primitive.cpp b/packages/motion/src/primitive.cpp index bd3562a1a..33f292b9e 100644 --- a/packages/motion/src/primitive.cpp +++ b/packages/motion/src/primitive.cpp @@ -5,13 +5,13 @@ namespace truck::geom { Poses findMotion(const Pose& from, const Vec2& to, double gamma, double step) { const Vec2 middle = from.pos + from.dir * gamma; - return bezier2(from.pos, middle, to, step); + return toPoses(bezier2(from.pos, middle, to, step)); } Poses findMotion(const Pose& from, const Pose& to, double gamma, double step) { const Vec2 from_ref = from.pos + from.dir * gamma; const Vec2 to_ref = to.pos - to.dir * gamma; - return bezier3(from.pos, from_ref, to_ref, to.pos, step); + return toPoses(bezier3(from.pos, from_ref, to_ref, to.pos, step)); } } // namespace truck::geom diff --git a/packages/route_follower/CMakeLists.txt b/packages/route_follower/CMakeLists.txt new file mode 100644 index 000000000..ab4fd2f5a --- /dev/null +++ b/packages/route_follower/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(route_follower) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(truck_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(speed REQUIRED) +find_package(collision REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +add_executable(${PROJECT_NAME}_node src/main.cpp src/route_follower_node.cpp) + +ament_target_dependencies( + ${PROJECT_NAME}_node + rclcpp + truck_msgs + nav_msgs + geometry_msgs + speed + collision + tf2 + tf2_ros + std_srvs + visualization_msgs +) + +install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) + +target_include_directories( + ${PROJECT_NAME}_node + PUBLIC "$" + "$" + "$" +) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/packages/route_follower/include/route_follower/route_follower_node.h b/packages/route_follower/include/route_follower/route_follower_node.h new file mode 100644 index 000000000..640cfe293 --- /dev/null +++ b/packages/route_follower/include/route_follower/route_follower_node.h @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "truck_msgs/msg/navigation_route.hpp" +#include "motion/trajectory.h" +#include "speed/greedy_planner.h" +#include "collision/collision_checker.h" +#include "geom/localization.h" +#include "geom/transform.h" + +#include +#include + +#include +#include +#include + +namespace truck::route_follower { + +using namespace std::chrono_literals; +using namespace std::placeholders; + +class RouteFollowerNode : public rclcpp::Node { + public: + RouteFollowerNode(); + + private: + void onRoute(const truck_msgs::msg::NavigationRoute::SharedPtr msg); + void onOdometry(nav_msgs::msg::Odometry::SharedPtr odometry); + void onGrid(nav_msgs::msg::OccupancyGrid::SharedPtr msg); + void onTf(tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static); + + void onReset( + const std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr); + + void publishGridCostMap(); + void publishTrajectory(); + void publishFullState(); + + std::optional getLatestTranform( + const std::string& source, const std::string& target); + + struct Parameters { + std::chrono::duration period = 0.1s; + double safety_margin = 0.3; + } params_; + + speed::GreedyPlanner::Params speed_params_{}; + + struct Timers { + rclcpp::TimerBase::SharedPtr main = nullptr; + } timer_; + + struct Services { + rclcpp::Service::SharedPtr reset = nullptr; + } service_; + + struct Slots { + rclcpp::Subscription::SharedPtr route = nullptr; + rclcpp::Subscription::SharedPtr odometry = nullptr; + rclcpp::Subscription::SharedPtr grid = nullptr; + rclcpp::Subscription::SharedPtr tf = nullptr; + rclcpp::Subscription::SharedPtr tf_static = nullptr; + } slot_; + + struct Signals { + rclcpp::Publisher::SharedPtr distance_transform = nullptr; + rclcpp::Publisher::SharedPtr trajectory = nullptr; + } signal_; + + struct State { + nav_msgs::msg::Odometry::SharedPtr odometry = nullptr; + std::optional localization = std::nullopt; + nav_msgs::msg::OccupancyGrid::SharedPtr grid = nullptr; + std::shared_ptr distance_transform = nullptr; + motion::Trajectory trajectory; + double scheduled_velocity = 0; + } state_; + + std::unique_ptr model_ = nullptr; + std::unique_ptr checker_ = nullptr; + std::unique_ptr tf_buffer_ = nullptr; +}; + +} // namespace truck::route_follower diff --git a/packages/route_follower/launch/route_follower.yaml b/packages/route_follower/launch/route_follower.yaml new file mode 100644 index 000000000..2d1f80043 --- /dev/null +++ b/packages/route_follower/launch/route_follower.yaml @@ -0,0 +1,22 @@ +launch: +- arg: + name: "model_config" + default: "$(find-pkg-share model)/config/model.yaml" +- arg: + name: "simulation" + default: "false" +- node: + pkg: "route_follower" + exec: "route_follower_node" + name: "route_follower_node" + output: "log" + param: + - { name: "use_sim_time", value: "$(var simulation)" } + - { name: "model_config", value: "$(var model_config)" } + - { name: "period", value: 0.1 } + - { name: "resolution", value: 0.02 } + - { name: "check_in_distance", value: 0.30 } + - { name: "safety_margin", value: 0.15 } + - { name: "distance_to_obstacle", value: 1.5 } + - { name: "acceleration/min", value: -0.5 } + - { name: "acceleration/max", value: 0.3 } diff --git a/packages/route_follower/package.xml b/packages/route_follower/package.xml new file mode 100644 index 000000000..6c0e00412 --- /dev/null +++ b/packages/route_follower/package.xml @@ -0,0 +1,24 @@ + + + + route_follower + 0.0.0 + route_follower + xs1nus + MIT + + ament_cmake + + rclcpp + truck_msgs + nav_msgs + geometry_msgs + speed + collision + tf2 + tf2_ros + + + ament_cmake + + diff --git a/packages/route_follower/src/main.cpp b/packages/route_follower/src/main.cpp new file mode 100644 index 000000000..e1d38fb0b --- /dev/null +++ b/packages/route_follower/src/main.cpp @@ -0,0 +1,9 @@ +#include "route_follower/route_follower_node.h" +#include + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/packages/route_follower/src/route_follower_node.cpp b/packages/route_follower/src/route_follower_node.cpp new file mode 100644 index 000000000..cd298175a --- /dev/null +++ b/packages/route_follower/src/route_follower_node.cpp @@ -0,0 +1,261 @@ +#include "route_follower/route_follower_node.h" + +#include "geom/msg.h" +#include "model/model.h" +#include "motion/trajectory.h" +#include "speed/greedy_planner.h" + +#include +#include + +#include +#include + +namespace truck::route_follower { + +namespace { + +Limits velocityLimits(const model::Model& model, double velocity, double time) { + const auto& vel = model.baseVelocityLimits(); + + return { + std::max(.0, vel.clamp(velocity) - model.baseMaxDeceleration() * time), + std::min(vel.max, vel.clamp(velocity) + model.baseMaxAcceleration() * time)}; +} + +template +motion::Trajectory makeTrajectory(It begin, It end, const geom::Transform& tf) { + VERIFY(std::distance(begin, end) > 1); + + motion::Trajectory trajectory; + + for (auto curr = begin + 1; curr != end; ++curr) { + const auto prev = curr - 1; + const geom::Pose world_pose{ + .pos = geom::toVec2(*prev), + .dir = geom::AngleVec2::fromVector(geom::toVec2(*curr) - geom::toVec2(*prev))}; + + trajectory.states.push_back(motion::State{.pose = tf.apply(world_pose)}); + } + + const geom::Pose world_pose{ + .pos = geom::toVec2(*(end - 1)), .dir = trajectory.states.back().pose.dir}; + + trajectory.states.push_back(motion::State{.pose = tf.apply(world_pose)}); + trajectory.fillDistance(); + + return trajectory; +} + +} // namespace + +RouteFollowerNode::RouteFollowerNode() : Node("route_follower") { + const auto qos = static_cast( + this->declare_parameter("qos", RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT)); + slot_.route = this->create_subscription( + "/navigation/route", + rclcpp::QoS(1).reliability(qos), + std::bind(&RouteFollowerNode::onRoute, this, _1)); + + signal_.trajectory = + this->create_publisher("/motion/trajectory", 10); + + params_ = { + .period = std::chrono::duration(this->declare_parameter("period", 0.1)), + .safety_margin = this->declare_parameter("safety_margin", 0.3), + }; + + RCLCPP_INFO(this->get_logger(), "period: %.2fs", params_.period.count()); + RCLCPP_INFO(this->get_logger(), "safety_margin: %.2fm", params_.safety_margin); + + speed_params_ = { + Limits{ + this->declare_parameter("acceleration/min", -0.5), + this->declare_parameter("acceleration/max", 0.3), + }, + this->declare_parameter("distance_to_obstacle", 0.7), + }; + + RCLCPP_INFO( + this->get_logger(), "distance_to_obstacle: %.2fm", speed_params_.distance_to_obstacle); + + RCLCPP_INFO( + this->get_logger(), + "acceleration: [%.2f, %.2f]", + speed_params_.acceleration.min, + speed_params_.acceleration.max); + + service_.reset = this->create_service( + "/reset_path", std::bind(&RouteFollowerNode::onReset, this, _1, _2)); + + slot_.odometry = this->create_subscription( + "/ekf/odometry/filtered", 1, std::bind(&RouteFollowerNode::onOdometry, this, _1)); + + slot_.grid = this->create_subscription( + "/grid", 1, std::bind(&RouteFollowerNode::onGrid, this, _1)); + + using TfCallback = std::function; + + const TfCallback tf_call = std::bind(&RouteFollowerNode::onTf, this, _1, false); + const TfCallback static_tf_callback = std::bind(&RouteFollowerNode::onTf, this, _1, true); + + slot_.tf = this->create_subscription( + "/tf", tf2_ros::DynamicListenerQoS(100), tf_call); + + slot_.tf_static = this->create_subscription( + "/tf_static", tf2_ros::StaticListenerQoS(100), static_tf_callback); + + timer_.main = this->create_wall_timer( + params_.period, std::bind(&RouteFollowerNode::publishFullState, this)); + + signal_.distance_transform = + this->create_publisher("/grid/distance_transform", 1); + + model_ = std::make_unique( + model::load(this->get_logger(), this->declare_parameter("model_config", ""))); + + checker_ = std::make_unique(model_->shape()); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); +} + +void RouteFollowerNode::publishFullState() { + publishTrajectory(); + publishGridCostMap(); +} + +void RouteFollowerNode::publishTrajectory() { + checker_->reset(*state_.distance_transform); + + bool collision = false; + for (auto& state : state_.trajectory.states) { + const double margin = checker_->distance(state.pose); + collision |= margin < params_.safety_margin; + + state.collision = collision; + state.margin = margin; + } + + speed::GreedyPlanner(speed_params_, *model_) + .setScheduledVelocity(state_.scheduled_velocity) + .fill(state_.trajectory); + + // dirty way to drop invalid trajectories and get error message + try { + state_.trajectory.throwIfInvalid(motion::TrajectoryValidations::enableAll(), *model_); + } catch (const std::exception& e) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "%s", e.what()); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Drop invalid trajectory!"); + + state_.trajectory = motion::Trajectory{}; + } + + const double latency = params_.period.count(); + const auto limits = velocityLimits(*model_, state_.localization->velocity, latency); + + if (!state_.trajectory.empty()) { + const auto scheduled_state = state_.trajectory.byTime(latency); + state_.scheduled_velocity = limits.clamp(scheduled_state.velocity); + } else { + state_.scheduled_velocity = 0.0; + } + + signal_.trajectory->publish( + motion::msg::toTrajectory(state_.odometry->header, state_.trajectory)); +} + +void RouteFollowerNode::publishGridCostMap() { + if (!state_.distance_transform) { + return; + } + + if (!signal_.distance_transform->get_subscription_count()) { + return; + } + + constexpr double k_max_distance = 10.0; + const auto msg = state_.distance_transform->makeCostMap(state_.grid->header, k_max_distance); + signal_.distance_transform->publish(msg); +} + +void RouteFollowerNode::onRoute(const truck_msgs::msg::NavigationRoute::SharedPtr msg) { + if (!state_.odometry || !state_.distance_transform) { + return; + } + + const auto source = std::string{"world"}; + const auto target = state_.odometry->header.frame_id; + + const auto tf_opt = getLatestTranform(source, target); + if (!tf_opt) { + RCLCPP_WARN( + this->get_logger(), + "Ignore grid, there is no transform from '%s' -> '%s'!", + source.c_str(), + target.c_str()); + return; + } + + state_.trajectory = makeTrajectory(msg->data.begin(), msg->data.end(), *tf_opt); +} + +void RouteFollowerNode::onReset( + const std::shared_ptr /*unused*/, + std::shared_ptr /*unused*/) { + RCLCPP_INFO(this->get_logger(), "Reset path!"); + + state_.trajectory = motion::Trajectory{}; + publishFullState(); +} + +void RouteFollowerNode::onOdometry(nav_msgs::msg::Odometry::SharedPtr odometry) { + state_.localization = geom::toLocalization(*odometry); + state_.odometry = odometry; +} + +void RouteFollowerNode::onGrid(nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + if (!state_.odometry) { + return; + } + + const auto source = msg->header.frame_id; + const auto target = state_.odometry->header.frame_id; + + const auto tf_opt = getLatestTranform(source, target); + if (!tf_opt) { + RCLCPP_WARN( + this->get_logger(), + "Ignore grid, there is no transform from '%s' -> '%s'!", + source.c_str(), + target.c_str()); + return; + } + + // simple hack to apply transform to grid + msg->header.frame_id = target; + msg->info.origin = geom::msg::toPose(tf_opt->apply(geom::toPose(msg->info.origin))); + + // distance transfor - cpu intensive operation + state_.distance_transform = std::make_shared( + collision::distanceTransform(collision::Map::fromOccupancyGrid(*msg))); + + state_.grid = msg; +} + +void RouteFollowerNode::onTf(tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static) { + static const std::string kAuthority; + for (const auto& transform : msg->transforms) { + tf_buffer_->setTransform(transform, kAuthority, is_static); + } +} + +std::optional RouteFollowerNode::getLatestTranform( + const std::string& source, const std::string& target) { + try { + return geom::toTransform(tf_buffer_->lookupTransform(target, source, rclcpp::Time(0))); + } catch (const tf2::TransformException& ex) { + return std::nullopt; + } +} +} // namespace truck::route_follower diff --git a/packages/routing/CMakeLists.txt b/packages/routing/CMakeLists.txt index 0c610a9da..42301dc1b 100644 --- a/packages/routing/CMakeLists.txt +++ b/packages/routing/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(truck_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Boost REQUIRED) +find_package(tf2_ros REQUIRED) add_executable(${PROJECT_NAME}_node src/main.cpp src/routing_node.cpp) @@ -23,6 +24,7 @@ ament_target_dependencies( nav_msgs geometry_msgs Boost + tf2_ros ) install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) diff --git a/packages/routing/include/routing/routing_node.h b/packages/routing/include/routing/routing_node.h index 516f91c60..087ed6f98 100644 --- a/packages/routing/include/routing/routing_node.h +++ b/packages/routing/include/routing/routing_node.h @@ -7,6 +7,8 @@ #include "truck_msgs/msg/navigation_mesh.hpp" #include "truck_msgs/msg/navigation_route.hpp" +#include +#include #include #include @@ -14,6 +16,7 @@ #include #include "geom/boost/point.h" +#include "geom/transform.h" namespace truck::routing { @@ -63,6 +66,9 @@ class RoutingNode : public rclcpp::Node { void publishMesh() const; void publishRoute() const; + std::optional getLatestTranform( + const std::string& source, const std::string& target) const; + struct Slots { rclcpp::Subscription::SharedPtr odom = nullptr; rclcpp::Subscription::SharedPtr finish = nullptr; @@ -102,6 +108,11 @@ class RoutingNode : public rclcpp::Node { std::unique_ptr map_ = nullptr; std::unique_ptr mesh_builder_ = nullptr; std::unique_ptr graph_builder_ = nullptr; + + std::unique_ptr tf_buffer_ = nullptr; + std::shared_ptr tf_listener_{nullptr}; + + const std::string target_frame_ = "world"; }; } // namespace truck::routing diff --git a/packages/routing/package.xml b/packages/routing/package.xml index 30b4f0c67..dc832f36b 100644 --- a/packages/routing/package.xml +++ b/packages/routing/package.xml @@ -16,6 +16,7 @@ nav_msgs geometry_msgs Boost + tf2_ros ament_cmake diff --git a/packages/routing/src/routing_node.cpp b/packages/routing/src/routing_node.cpp index 07fa6d91b..a0a2c08e8 100644 --- a/packages/routing/src/routing_node.cpp +++ b/packages/routing/src/routing_node.cpp @@ -123,6 +123,10 @@ void RoutingNode::initializeParams() { } void RoutingNode::initializeTopicHandlers() { + tf_buffer_ = std::make_unique(this->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); + tf_listener_ = std::make_shared(*tf_buffer_); + const auto qos = static_cast( this->declare_parameter("qos", RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT)); @@ -147,11 +151,33 @@ void RoutingNode::initializeTopicHandlers() { } void RoutingNode::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) { - state_.ego = geom::toVec2(*msg); + const auto tf_opt = getLatestTranform(msg->header.frame_id, target_frame_); + if (!tf_opt) { + RCLCPP_WARN( + this->get_logger(), + "Can't lookup transform from '%s' to '%s', ignore odometry!", + msg->header.frame_id.c_str(), + target_frame_.c_str()); + + return; + } + + state_.ego = tf_opt->apply(geom::toVec2(*msg)); } void RoutingNode::onFinish(const geometry_msgs::msg::PointStamped::SharedPtr msg) { - state_.finish = geom::toVec2(*msg); + const auto tf_opt = getLatestTranform(msg->header.frame_id, target_frame_); + if (!tf_opt) { + RCLCPP_WARN( + this->get_logger(), + "Can't lookup transform from '%s' to '%s', ignore route finish point!", + msg->header.frame_id.c_str(), + target_frame_.c_str()); + + return; + } + + state_.finish = tf_opt->apply(geom::toVec2(*msg)); if (state_.ego.has_value()) { updateRoute(); @@ -216,4 +242,13 @@ void RoutingNode::publishRoute() const { signals_.route->publish(msg); } +std::optional RoutingNode::getLatestTranform( + const std::string& source, const std::string& target) const { + try { + return geom::toTransform(tf_buffer_->lookupTransform(target, source, rclcpp::Time(0))); + } catch (const tf2::TransformException& ex) { + return std::nullopt; + } +} + } // namespace truck::routing diff --git a/packages/truck/launch/route_follower_demo.yaml b/packages/truck/launch/route_follower_demo.yaml new file mode 100644 index 000000000..1b78f9d88 --- /dev/null +++ b/packages/truck/launch/route_follower_demo.yaml @@ -0,0 +1,20 @@ +launch: +- arg: { name: "simulation", default: "$(env TRUCK_SIMULATION)" } +- arg: { name: "map_config", default: "$(find-pkg-share map)/data/map_6.geojson" } + + +- let: { name: "env", value: "simulator", if: "$(var simulation)" } +- let: { name: "env", value: "truck", unless: "$(var simulation)" } + +- include: + file: $(find-pkg-share truck)/launch/$(var env).yaml + +- include: + file: $(find-pkg-share route_follower)/launch/route_follower.yaml + arg: + - { name: "simulation", value: $(var simulation) } + +- include: + file: $(find-pkg-share routing)/launch/routing.yaml + arg: + - { name: "map_config", value: "$(var map_config)" } diff --git a/packages/truck/launch/routing_demo.yaml b/packages/truck/launch/routing_demo.yaml index a9df8b179..9bf3c104a 100644 --- a/packages/truck/launch/routing_demo.yaml +++ b/packages/truck/launch/routing_demo.yaml @@ -1,12 +1,11 @@ launch: -- arg: { name: "map_config", default: "$(find-pkg-share map)/data/map_6.geojson" } +- arg: { name: "simulation", default: "$(env TRUCK_SIMULATION)" } + +- let: { name: "env", value: "simulator", if: "$(var simulation)" } +- let: { name: "env", value: "truck", unless: "$(var simulation)" } - include: - file: $(find-pkg-share simulator_2d)/launch/simulator_pipeline.yaml - arg: - - { name: "map_config", value: "$(var map_config)" } + file: $(find-pkg-share truck)/launch/$(var env).yaml - include: file: $(find-pkg-share routing)/launch/routing.yaml - arg: - - { name: "map_config", value: "$(var map_config)" } diff --git a/packages/truck/launch/slam.yaml b/packages/truck/launch/slam.yaml index b925b95e4..1ab815b9a 100644 --- a/packages/truck/launch/slam.yaml +++ b/packages/truck/launch/slam.yaml @@ -2,11 +2,12 @@ launch: - arg: { name: "simulation", default: "false" } - arg: { name: "qos", default: "0" } +### HACK: comment the next few lines out if serious lags occur - include: - file: $(find-pkg-share icp_odometry)/launch/icp_odometry.yaml - arg: - - { name: "use_sim_time", value: "$(var simulation)" } - - { name: "qos", value: "$(var qos)" } + file: $(find-pkg-share icp_odometry)/launch/icp_odometry.yaml + arg: + - { name: "use_sim_time", value: "$(var simulation)" } + - { name: "qos", value: "$(var qos)" } - node: pkg: "robot_localization" diff --git a/packages/visualization/src/visualization_node.cpp b/packages/visualization/src/visualization_node.cpp index e98ab7c46..be61f35ae 100644 --- a/packages/visualization/src/visualization_node.cpp +++ b/packages/visualization/src/visualization_node.cpp @@ -487,6 +487,7 @@ void VisualizationNode::publishMap() const { visualization_msgs::msg::Marker msg; msg.header.stamp = now(); msg.header.frame_id = "world"; + // msg.header.frame_id = "odom_ekf"; msg.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; msg.action = visualization_msgs::msg::Marker::ADD; msg.color = color::gray(0.6); @@ -519,7 +520,7 @@ void VisualizationNode::publishNavigationMesh() const { visualization_msgs::msg::Marker msg; msg.header.stamp = now(); - msg.header.frame_id = "odom_ekf"; + msg.header.frame_id = "world"; msg.type = visualization_msgs::msg::Marker::SPHERE_LIST; msg.action = visualization_msgs::msg::Marker::ADD; msg.color = color::white(0.8); @@ -551,7 +552,7 @@ void VisualizationNode::publishNavigationRoute() const { visualization_msgs::msg::Marker msg; msg.header.stamp = now(); - msg.header.frame_id = "odom_ekf"; + msg.header.frame_id = "world"; msg.type = visualization_msgs::msg::Marker::LINE_STRIP; msg.action = visualization_msgs::msg::Marker::ADD; msg.color = color::blue(0.5);