From 982bf1318a010b5e1b8fed092a2c4c3e8246a51a Mon Sep 17 00:00:00 2001 From: Xiyu Date: Fri, 20 Dec 2024 12:22:32 +0800 Subject: [PATCH] Insert in-place rotation waypoint if missing (#111) * Insert in-place rotation if missing Signed-off-by: Xiyu Oh * Fix typo Signed-off-by: Xiyu Oh * Use references Signed-off-by: Xiyu Oh * Remove same stack requirement Signed-off-by: Xiyu Oh * Fix orientation test and checkpoint identification for manual turn-in-place insertions Signed-off-by: Michael X. Grey * Tweak test debugging functions Signed-off-by: Michael X. Grey --------- Signed-off-by: Xiyu Oh Signed-off-by: Michael X. Grey Co-authored-by: Grey --- rmf_traffic/include/rmf_traffic/agv/Graph.hpp | 2 +- .../agv/planning/DifferentialDrivePlanner.cpp | 94 +++++++++++++++++++ rmf_traffic/test/unit/agv/test_Planner.cpp | 52 ++++++++-- 3 files changed, 140 insertions(+), 8 deletions(-) diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp index 2b1d2b82..978d66f0 100644 --- a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp +++ b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp @@ -215,7 +215,7 @@ class Graph std::optional merge_radius() const; /// Set the merge radius specific to this waypoint. - Waypoint& set_merge_radius(std::optional valeu); + Waypoint& set_merge_radius(std::optional value); class Implementation; private: diff --git a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp index 4f535181..ef5d95a6 100644 --- a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp @@ -280,6 +280,12 @@ void stream_trajectory( ss << "(finished)\n"; } +//============================================================================== +inline bool same_orientation(double yaw0, double yaw1) +{ + return std::abs(rmf_utils::wrap_to_pi(yaw0 - yaw1))*180.0 / M_PI < 1e-2; +} + //============================================================================== std::vector find_dependencies( std::vector& itinerary, @@ -473,6 +479,72 @@ std::vector find_dependencies( } } + std::size_t c_last = 0; + std::size_t c = 1; + std::size_t c_next = 2; + for (; c_next < candidates.size(); ++c_last, ++c, ++c_next) + { + auto& candidate = candidates[c]; + if (candidate.waypoint.arrival.empty()) + { + // This was a manually inserted turn-in-place, so let's try to match it + // with a trajectory checkpoint + const auto p_candidate = candidate.waypoint.position; + for (std::size_t r = 0; r < itinerary.size(); ++r) + { + std::optional floor_opt; + for (const auto& checkpoint : candidates[c_last].waypoint.arrival) + { + if (checkpoint.route_id != r) + continue; + + const auto id = checkpoint.checkpoint_id; + if (!floor_opt.has_value()) + floor_opt = id; + else if (*floor_opt < id) + floor_opt = id; + } + + if (!floor_opt.has_value()) + continue; + const auto floor_id = floor_opt.value(); + + std::optional ceil_opt; + for (const auto& checkpoint : candidates[c_next].waypoint.arrival) + { + if (checkpoint.route_id != r) + continue; + + const auto id = checkpoint.checkpoint_id; + if (!ceil_opt.has_value()) + ceil_opt = id; + else if (id < *ceil_opt) + ceil_opt = id; + } + + if (!ceil_opt.has_value()) + continue; + const auto ceil_id = ceil_opt.value(); + + // Between the floor and the ceiling, try to find a waypoint in this + // trajectory that matches the position of the inserted waypoint. + for (std::size_t id = floor_id; id < ceil_id; ++id) + { + const auto& trajectory_checkpoint = itinerary[r].trajectory()[id]; + const auto p_trajectory = trajectory_checkpoint.position(); + const bool same_pos = (p_candidate.block<2,1>(0,0) - p_trajectory.block<2,1>(0,0)).norm() < 1e-2; + const bool same_ori = same_orientation(p_candidate[2], p_trajectory[2]); + if (same_pos && same_ori) + { + // We have a matching checkpoint + candidate.waypoint.arrival.push_back(Plan::Checkpoint { r, id }); + candidate.waypoint.time = trajectory_checkpoint.time(); + } + } + } + } + } + std::vector waypoints; waypoints.reserve(candidates.size()); for (const auto& c : candidates) @@ -551,6 +623,28 @@ reconstruct_waypoints( { const auto& node = node_sequence.at(i); const auto yaw = node->yaw; + + // Insert in-place rotation waypoint if the current and previous waypoints + // are disconnected + const auto& prev_candidate = candidates.back(); + const auto& prev_wp = prev_candidate.waypoint; + const Eigen::Vector2d prev_pos = + Eigen::Vector2d{prev_wp.position[0], prev_wp.position[1]}; + const bool same_pos = (prev_pos - node->position).norm() < 1e-3; + const bool same_ori = same_orientation(prev_wp.position[2], node->yaw); + if (!same_pos && !same_ori) + { + candidates.push_back(WaypointCandidate{ + true, + Plan::Waypoint::Implementation{ + Eigen::Vector3d{prev_pos[0], prev_pos[1], node->yaw}, + prev_wp.time, prev_wp.graph_index, + {}, {}, {}, prev_wp.event, {} + }, + prev_candidate.velocity + }); + } + if (node->approach_lanes.empty()) { // This means we are doing an in-place rotation or a wait diff --git a/rmf_traffic/test/unit/agv/test_Planner.cpp b/rmf_traffic/test/unit/agv/test_Planner.cpp index a32b495d..a8459d42 100644 --- a/rmf_traffic/test/unit/agv/test_Planner.cpp +++ b/rmf_traffic/test/unit/agv/test_Planner.cpp @@ -82,21 +82,52 @@ void print_trajectory_info( rmf_traffic::Time time) { display_path(plan); + std::cout << "Plan waypoint count: " << plan->get_waypoints().size() << std::endl; + int wp_count = 0; + for (const auto& wp : plan->get_waypoints()) + { + std::cout << " wp" << wp_count++ << ": t=" << rmf_traffic::time::to_seconds(wp.time() - time) + << "s {" << wp.position().transpose() << "}"; + if (wp.graph_index()) + { + std::cout << " [graph index " << *wp.graph_index() << "]"; + } + else + { + std::cout << " [no graph index]"; + } + + std::cout << " checkpoints:"; + if (wp.arrival_checkpoints().empty()) + { + std::cout << " none"; + } + else + { + for (const auto c : wp.arrival_checkpoints()) + { + std::cout << " [" << c.route_id << ":" << c.checkpoint_id << "]"; + } + } + + std::cout << std::endl; + } + + std::cout << "Itinerary count: " << plan->get_itinerary().size() << std::endl; - int trajectory_count = 1; + int trajectory_count = 0; for (const auto& r : plan->get_itinerary()) { - int waypoint_count = 1; + int waypoint_count = 0; const auto& t = r.trajectory(); std::cout << "Trajectory [" << trajectory_count << "] in " << r.map() << " with " << t.size() << " waypoints\n"; for (auto it = t.begin(); it != t.end(); it++) { auto position = it->position(); - std::cout << " Waypoint "<< waypoint_count << ": {" << position[0] - << ","<time() - time) << "s" + std::cout << " x"<< waypoint_count << ": t=" << rmf_traffic::time::to_seconds(it->time() - time) + << "s" << " {" << position[0] << ","< 0); const auto& start = original_result->get_start(); @@ -239,6 +271,12 @@ void test_ignore_obstacle( const auto new_plan = original_result.replan(start, std::move(options)); + if (print_info) + { + print_trajectory_info(original_result, original_result->get_start().time()); + print_trajectory_info(new_plan, original_result->get_start().time()); + } + // The new plan which ignores the conflicts should be the same as the original REQUIRE(new_plan->get_itinerary().size() == 1); const auto new_duration = @@ -1505,7 +1543,7 @@ SCENARIO("Test planning") } } -SCENARIO("DP1 Graph") +SCENARIO("DP1 Graph", "[dp1_graph]") { using namespace std::chrono_literals;