From a0d80904dadb3b2e2769fa7876b2689ed3a1a47d Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Wed, 24 Mar 2021 15:31:30 +0530 Subject: [PATCH 1/7] Add regression test for DetectConflict Signed-off-by: Shivesh Khaitan --- .../regress_DetectConflict_fail.cpp | 135 ++++++++++++++++++ 1 file changed, 135 insertions(+) create mode 100644 rmf_traffic/test/regression/regress_DetectConflict_fail.cpp diff --git a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp new file mode 100644 index 00000000..83778626 --- /dev/null +++ b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp @@ -0,0 +1,135 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * 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 +#include + +#include +#include +#include +#include + +namespace { + +//============================================================================== +SCENARIO("Failed Detect Conflict") +{ + rmf_traffic::agv::VehicleTraits traits{ + {0.5, 2.0}, {0.75, 1.5}, + rmf_traffic::Profile{ + rmf_traffic::geometry::make_final_convex( + rmf_traffic::geometry::Circle(0.2)) + } + }; + + auto graph = rmf_traffic::agv::Graph(); + + graph.add_waypoint("test", Eigen::Vector2d(66.4517, -8.31925)); + graph.add_waypoint("test", Eigen::Vector2d(66.4694, -11.86215)); + graph.add_waypoint("test", Eigen::Vector2d(63.575, -11.875)); + graph.add_waypoint("test", Eigen::Vector2d(16.097900000000003, -12.8518)); + graph.add_waypoint("test", Eigen::Vector2d(13.2777, -12.980850000000002)); + graph.add_waypoint("test", Eigen::Vector2d(13.325, -8.2798)); + graph.add_waypoint("test", Eigen::Vector2d(117.30075000000001, -12.863300000000002)); + graph.add_waypoint("test", Eigen::Vector2d(120.03815, -12.8944)); + graph.add_waypoint("test", Eigen::Vector2d(120.05175, -8.3695)); + graph.add_waypoint("test", Eigen::Vector2d(13.675250000000002, -28.884750000000004)); + graph.add_waypoint("test", Eigen::Vector2d(66.48685, -28.959050000000005)); + graph.add_waypoint("test", Eigen::Vector2d(119.97360000000002, -28.97215)); + graph.add_waypoint("test", Eigen::Vector2d(18.28865, -8.2941)); + graph.add_waypoint("test", Eigen::Vector2d(24.8581, -8.2971)); + graph.add_waypoint("test", Eigen::Vector2d(30.8229, -8.302249999999999)); + graph.add_waypoint("test", Eigen::Vector2d(38.977000000000004, -8.3191)); + graph.add_waypoint("test", Eigen::Vector2d(44.60830000000001, -8.324)); + graph.add_waypoint("test", Eigen::Vector2d(52.278800000000004, -8.3107)); + graph.add_waypoint("test", Eigen::Vector2d(63.325, -8.3315)); + + graph.add_lane( 0, 1 ); + graph.add_lane( 1, 0 ); + graph.add_lane( 1, 2 ); + graph.add_lane( 2, 1 ); + graph.add_lane( 3, 4 ); + graph.add_lane( 4, 3 ); + graph.add_lane( 4, 5 ); + graph.add_lane( 5, 4 ); + graph.add_lane( 6, 7 ); + graph.add_lane( 7, 6 ); + graph.add_lane( 7, 8 ); + graph.add_lane( 8, 7 ); + graph.add_lane( 4, 9 ); + graph.add_lane( 9, 4 ); + graph.add_lane( 1, 10 ); + graph.add_lane( 10, 1 ); + graph.add_lane( 0, 8 ); + graph.add_lane( 8, 0 ); + graph.add_lane( 7, 11 ); + graph.add_lane( 11, 7 ); + graph.add_lane( 5, 12 ); + graph.add_lane( 12, 13 ); + graph.add_lane( 13, 14 ); + graph.add_lane( 14, 15 ); + graph.add_lane( 15, 16 ); + graph.add_lane( 16, 17 ); + graph.add_lane( 17, 18 ); + graph.add_lane( 18, 0 ); + graph.add_lane( 0, 18 ); + graph.add_lane( 18, 17 ); + graph.add_lane( 17, 16 ); + graph.add_lane( 16, 15 ); + graph.add_lane( 15, 14 ); + graph.add_lane( 14, 13 ); + graph.add_lane( 13, 12 ); + graph.add_lane( 12, 5 ); + + const auto start_time = std::chrono::steady_clock::now(); + + const auto database = std::make_shared(); + + rmf_traffic::agv::Planner planner_obstacle = rmf_traffic::agv::Planner{ + {graph, traits}, + {nullptr} + }; + + const auto N = database->participant_ids().size(); + + auto new_obstacle = rmf_traffic::schedule::make_participant( + rmf_traffic::schedule::ParticipantDescription{ + "obstacle_" + std::to_string(N), + "obstacles", + rmf_traffic::schedule::ParticipantDescription::Rx::Unresponsive, + planner_obstacle.get_configuration().vehicle_traits().profile() + }, database); + + auto plan_obstacle = planner_obstacle.plan({rmf_traffic::time::apply_offset(start_time, 106.362), 1, 90.0}, {5}); + + new_obstacle.set(plan_obstacle->get_itinerary()); + + const auto obstacle_validator = + rmf_traffic::agv::ScheduleRouteValidator::make( + database, std::numeric_limits::max(), traits.profile()); + + rmf_traffic::agv::Planner::Options options(obstacle_validator); + options.saturation_limit(100000); + rmf_traffic::agv::Planner planner = rmf_traffic::agv::Planner{ + {graph, traits}, + options + }; + auto plan = planner.plan({start_time, 5, 180.0}, {2}); + + CHECK_FALSE(plan.success()); +} +} \ No newline at end of file From ae44516df13def3259cd6f707053360ece1d6dbd Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Wed, 24 Mar 2021 18:05:55 +0530 Subject: [PATCH 2/7] Fix style Signed-off-by: Shivesh Khaitan --- .../regress_DetectConflict_fail.cpp | 116 +++++++++--------- 1 file changed, 60 insertions(+), 56 deletions(-) diff --git a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp index 83778626..12acfad0 100644 --- a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp +++ b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp @@ -29,11 +29,11 @@ namespace { SCENARIO("Failed Detect Conflict") { rmf_traffic::agv::VehicleTraits traits{ - {0.5, 2.0}, {0.75, 1.5}, - rmf_traffic::Profile{ - rmf_traffic::geometry::make_final_convex( - rmf_traffic::geometry::Circle(0.2)) - } + {0.5, 2.0}, {0.75, 1.5}, + rmf_traffic::Profile{ + rmf_traffic::geometry::make_final_convex( + rmf_traffic::geometry::Circle(0.2)) + } }; auto graph = rmf_traffic::agv::Graph(); @@ -44,10 +44,12 @@ SCENARIO("Failed Detect Conflict") graph.add_waypoint("test", Eigen::Vector2d(16.097900000000003, -12.8518)); graph.add_waypoint("test", Eigen::Vector2d(13.2777, -12.980850000000002)); graph.add_waypoint("test", Eigen::Vector2d(13.325, -8.2798)); - graph.add_waypoint("test", Eigen::Vector2d(117.30075000000001, -12.863300000000002)); + graph.add_waypoint("test", + Eigen::Vector2d(117.30075000000001, -12.863300000000002)); graph.add_waypoint("test", Eigen::Vector2d(120.03815, -12.8944)); graph.add_waypoint("test", Eigen::Vector2d(120.05175, -8.3695)); - graph.add_waypoint("test", Eigen::Vector2d(13.675250000000002, -28.884750000000004)); + graph.add_waypoint("test", + Eigen::Vector2d(13.675250000000002, -28.884750000000004)); graph.add_waypoint("test", Eigen::Vector2d(66.48685, -28.959050000000005)); graph.add_waypoint("test", Eigen::Vector2d(119.97360000000002, -28.97215)); graph.add_waypoint("test", Eigen::Vector2d(18.28865, -8.2941)); @@ -58,75 +60,77 @@ SCENARIO("Failed Detect Conflict") graph.add_waypoint("test", Eigen::Vector2d(52.278800000000004, -8.3107)); graph.add_waypoint("test", Eigen::Vector2d(63.325, -8.3315)); - graph.add_lane( 0, 1 ); - graph.add_lane( 1, 0 ); - graph.add_lane( 1, 2 ); - graph.add_lane( 2, 1 ); - graph.add_lane( 3, 4 ); - graph.add_lane( 4, 3 ); - graph.add_lane( 4, 5 ); - graph.add_lane( 5, 4 ); - graph.add_lane( 6, 7 ); - graph.add_lane( 7, 6 ); - graph.add_lane( 7, 8 ); - graph.add_lane( 8, 7 ); - graph.add_lane( 4, 9 ); - graph.add_lane( 9, 4 ); - graph.add_lane( 1, 10 ); - graph.add_lane( 10, 1 ); - graph.add_lane( 0, 8 ); - graph.add_lane( 8, 0 ); - graph.add_lane( 7, 11 ); - graph.add_lane( 11, 7 ); - graph.add_lane( 5, 12 ); - graph.add_lane( 12, 13 ); - graph.add_lane( 13, 14 ); - graph.add_lane( 14, 15 ); - graph.add_lane( 15, 16 ); - graph.add_lane( 16, 17 ); - graph.add_lane( 17, 18 ); - graph.add_lane( 18, 0 ); - graph.add_lane( 0, 18 ); - graph.add_lane( 18, 17 ); - graph.add_lane( 17, 16 ); - graph.add_lane( 16, 15 ); - graph.add_lane( 15, 14 ); - graph.add_lane( 14, 13 ); - graph.add_lane( 13, 12 ); - graph.add_lane( 12, 5 ); + graph.add_lane(0, 1); + graph.add_lane(1, 0); + graph.add_lane(1, 2); + graph.add_lane(2, 1); + graph.add_lane(3, 4); + graph.add_lane(4, 3); + graph.add_lane(4, 5); + graph.add_lane(5, 4); + graph.add_lane(6, 7); + graph.add_lane(7, 6); + graph.add_lane(7, 8); + graph.add_lane(8, 7); + graph.add_lane(4, 9); + graph.add_lane(9, 4); + graph.add_lane(1, 10); + graph.add_lane(10, 1); + graph.add_lane(0, 8); + graph.add_lane(8, 0); + graph.add_lane(7, 11); + graph.add_lane(11, 7); + graph.add_lane(5, 12); + graph.add_lane(12, 13); + graph.add_lane(13, 14); + graph.add_lane(14, 15); + graph.add_lane(15, 16); + graph.add_lane(16, 17); + graph.add_lane(17, 18); + graph.add_lane(18, 0); + graph.add_lane(0, 18); + graph.add_lane(18, 17); + graph.add_lane(17, 16); + graph.add_lane(16, 15); + graph.add_lane(15, 14); + graph.add_lane(14, 13); + graph.add_lane(13, 12); + graph.add_lane(12, 5); const auto start_time = std::chrono::steady_clock::now(); const auto database = std::make_shared(); rmf_traffic::agv::Planner planner_obstacle = rmf_traffic::agv::Planner{ - {graph, traits}, - {nullptr} + {graph, traits}, + {nullptr} }; const auto N = database->participant_ids().size(); auto new_obstacle = rmf_traffic::schedule::make_participant( - rmf_traffic::schedule::ParticipantDescription{ - "obstacle_" + std::to_string(N), - "obstacles", - rmf_traffic::schedule::ParticipantDescription::Rx::Unresponsive, - planner_obstacle.get_configuration().vehicle_traits().profile() - }, database); + rmf_traffic::schedule::ParticipantDescription{ + "obstacle_" + std::to_string(N), + "obstacles", + rmf_traffic::schedule::ParticipantDescription::Rx::Unresponsive, + planner_obstacle.get_configuration().vehicle_traits().profile() + }, database); - auto plan_obstacle = planner_obstacle.plan({rmf_traffic::time::apply_offset(start_time, 106.362), 1, 90.0}, {5}); + auto plan_obstacle = + planner_obstacle.plan({rmf_traffic::time::apply_offset(start_time, + 106.362), 1, 90.0}, {5}); new_obstacle.set(plan_obstacle->get_itinerary()); const auto obstacle_validator = - rmf_traffic::agv::ScheduleRouteValidator::make( - database, std::numeric_limits::max(), traits.profile()); + rmf_traffic::agv::ScheduleRouteValidator::make( + database, std::numeric_limits::max(), traits.profile()); rmf_traffic::agv::Planner::Options options(obstacle_validator); options.saturation_limit(100000); rmf_traffic::agv::Planner planner = rmf_traffic::agv::Planner{ - {graph, traits}, - options + {graph, traits}, + options }; auto plan = planner.plan({start_time, 5, 180.0}, {2}); From d4ab8c241f75f885966e81b05cf7346a3fe92f02 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Wed, 24 Mar 2021 18:30:39 +0530 Subject: [PATCH 3/7] Update license Signed-off-by: Shivesh Khaitan --- rmf_traffic/test/regression/regress_DetectConflict_fail.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp index 12acfad0..e586765e 100644 --- a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp +++ b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. From e832c78c5cebd906731d27e225729181436c14e3 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Tue, 30 Mar 2021 14:05:38 +0530 Subject: [PATCH 4/7] Add rotation value test Signed-off-by: Shivesh Khaitan --- .../include/rmf_traffic/DetectConflict.hpp | 3 +- rmf_traffic/package.xml | 2 +- .../src/rmf_traffic/DetectConflict.cpp | 30 +++++++++-- .../rmf_traffic/DetectConflictInternal.hpp | 3 +- .../regress_DetectConflict_fail.cpp | 51 +++++++++++++++++-- 5 files changed, 77 insertions(+), 12 deletions(-) diff --git a/rmf_traffic/include/rmf_traffic/DetectConflict.hpp b/rmf_traffic/include/rmf_traffic/DetectConflict.hpp index 2590e4d7..16ad7a4d 100644 --- a/rmf_traffic/include/rmf_traffic/DetectConflict.hpp +++ b/rmf_traffic/include/rmf_traffic/DetectConflict.hpp @@ -56,7 +56,8 @@ class DetectConflict const Trajectory& trajectory_a, const Profile& profile_b, const Trajectory& trajectory_b, - Interpolate interpolation = Interpolate::CubicSpline); + Interpolate interpolation = Interpolate::CubicSpline, + bool zero = true); class Implementation; }; diff --git a/rmf_traffic/package.xml b/rmf_traffic/package.xml index 76ebb7bc..3d9ae315 100644 --- a/rmf_traffic/package.xml +++ b/rmf_traffic/package.xml @@ -14,7 +14,7 @@ libccd-dev libfcl-dev - + fcl ament_cmake_catch2 rmf_cmake_uncrustify diff --git a/rmf_traffic/src/rmf_traffic/DetectConflict.cpp b/rmf_traffic/src/rmf_traffic/DetectConflict.cpp index ee9f89b1..37669438 100644 --- a/rmf_traffic/src/rmf_traffic/DetectConflict.cpp +++ b/rmf_traffic/src/rmf_traffic/DetectConflict.cpp @@ -425,12 +425,15 @@ rmf_utils::optional DetectConflict::between( const Trajectory& trajectory_a, const Profile& profile_b, const Trajectory& trajectory_b, - Interpolate interpolation) + Interpolate interpolation, + bool zero) { return Implementation::between( profile_a, trajectory_a, profile_b, trajectory_b, - interpolation); + interpolation, + nullptr, + zero); } namespace { @@ -764,12 +767,29 @@ rmf_utils::optional detect_approach( //============================================================================== rmf_utils::optional DetectConflict::Implementation::between( const Profile& input_profile_a, - const Trajectory& trajectory_a, + const Trajectory& trajectory_a_actual, const Profile& input_profile_b, - const Trajectory& trajectory_b, + const Trajectory& trajectory_b_actual, Interpolate /*interpolation*/, - std::vector* output_conflicts) + std::vector* output_conflicts, + bool zero) { + rmf_traffic::Trajectory trajectory_a; + rmf_traffic::Trajectory trajectory_b; + for (auto it = trajectory_a_actual.begin(); it != trajectory_a_actual.end(); ++it) { + auto position = it->position(); + auto velocity = it->velocity(); + if (zero) + position << position(0), position(1), 0.0; + trajectory_a.insert(it->time(), position, velocity); + } + for (auto it = trajectory_b_actual.begin(); it != trajectory_b_actual.end(); ++it) { + auto position = it->position(); + auto velocity = it->velocity(); + if (zero) + position << position(0), position(1), 0.0; + trajectory_b.insert(it->time(), position, velocity); + } if (trajectory_a.size() < 2) { throw invalid_trajectory_error::Implementation diff --git a/rmf_traffic/src/rmf_traffic/DetectConflictInternal.hpp b/rmf_traffic/src/rmf_traffic/DetectConflictInternal.hpp index 44f82650..c416df65 100644 --- a/rmf_traffic/src/rmf_traffic/DetectConflictInternal.hpp +++ b/rmf_traffic/src/rmf_traffic/DetectConflictInternal.hpp @@ -48,7 +48,8 @@ class DetectConflict::Implementation const Profile& profile_b, const Trajectory& trajectory_b, Interpolate interpolation, - std::vector* output_conflicts = nullptr); + std::vector* output_conflicts = nullptr, + bool zero = true); }; diff --git a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp index e586765e..0376cd72 100644 --- a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp +++ b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp @@ -19,9 +19,10 @@ #include #include +#include #include +#include #include -#include namespace { @@ -118,7 +119,7 @@ SCENARIO("Failed Detect Conflict") auto plan_obstacle = planner_obstacle.plan({rmf_traffic::time::apply_offset(start_time, - 106.362), 1, 90.0}, {5}); + 106.362), 1, 90.0 * M_PI / 180.0}, {5}); new_obstacle.set(plan_obstacle->get_itinerary()); @@ -127,13 +128,55 @@ SCENARIO("Failed Detect Conflict") database, std::numeric_limits::max(), traits.profile()); rmf_traffic::agv::Planner::Options options(obstacle_validator); - options.saturation_limit(100000); + options.saturation_limit(1000000); rmf_traffic::agv::Planner planner = rmf_traffic::agv::Planner{ {graph, traits}, options }; - auto plan = planner.plan({start_time, 5, 180.0}, {2}); + auto plan = planner.plan({start_time, 5, 180.0 * M_PI / 180.0}, {2}); CHECK_FALSE(plan.success()); + + for (const auto& obstacle_it : plan_obstacle->get_itinerary()) + { + for (const auto& it : plan->get_itinerary()) + { + rmf_traffic::Trajectory obstacle_traj; + rmf_traffic::Trajectory traj; + for (auto waypoint_traj = obstacle_it.trajectory().begin(); + waypoint_traj != obstacle_it.trajectory().end(); ++waypoint_traj) + { + auto position = waypoint_traj->position(); + auto velocity = waypoint_traj->velocity(); + obstacle_traj.insert(waypoint_traj->time(), position, velocity); + } + for (auto waypoint_traj = it.trajectory().begin(); + waypoint_traj != it.trajectory().end(); + ++waypoint_traj) + { + auto position = waypoint_traj->position(); + auto velocity = waypoint_traj->velocity(); + traj.insert(waypoint_traj->time(), position, velocity); + } + + bool detect_conflict_flag_true = + rmf_traffic::DetectConflict::between(traits.profile(), + obstacle_traj, + traits.profile(), + traj, + rmf_traffic::DetectConflict::Interpolate::CubicSpline, + true).has_value(); + + bool detect_conflict_flag_false = + rmf_traffic::DetectConflict::between(traits.profile(), + obstacle_traj, + traits.profile(), + traj, + rmf_traffic::DetectConflict::Interpolate::CubicSpline, + false).has_value(); + + CHECK(detect_conflict_flag_true == detect_conflict_flag_false); + } + } } } \ No newline at end of file From d7b184db4260df3ce9a4a529d917408a940794ab Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Fri, 16 Apr 2021 11:39:40 +0530 Subject: [PATCH 5/7] Isolate rotation value test Signed-off-by: Shivesh Khaitan --- rmf_traffic/CMakeLists.txt | 11 ++ .../src/rmf_traffic/DetectConflict.cpp | 8 +- .../regress_DetectConflict_fail.cpp | 110 +++++++++++------- .../regress_DetectConflict_fail/Waypoint1.txt | Bin 0 -> 56 bytes .../regress_DetectConflict_fail/Waypoint2.txt | Bin 0 -> 56 bytes .../regress_DetectConflict_fail/Waypoint3.txt | Bin 0 -> 56 bytes .../regress_DetectConflict_fail/Waypoint4.txt | Bin 0 -> 56 bytes 7 files changed, 87 insertions(+), 42 deletions(-) create mode 100644 rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint1.txt create mode 100644 rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint2.txt create mode 100644 rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint3.txt create mode 100644 rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint4.txt diff --git a/rmf_traffic/CMakeLists.txt b/rmf_traffic/CMakeLists.txt index d5516187..43e5a092 100644 --- a/rmf_traffic/CMakeLists.txt +++ b/rmf_traffic/CMakeLists.txt @@ -76,6 +76,17 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) CONFIG_FILE ${uncrustify_config_file} MAX_LINE_LENGTH 80 ) + + file(GLOB_RECURSE regress_DetectConflict_fail_resources "test/resources/regress_DetectConflict_fail/*") + foreach(resource ${regress_DetectConflict_fail_resources}) + get_filename_component(resource_name ${resource} NAME) + configure_file(${resource} "${CMAKE_CURRENT_BINARY_DIR}/resources/${resource_name}" COPYONLY) + endforeach() + + target_compile_definitions(test_rmf_traffic + PRIVATE + "RESOURCES_DIR=\"${CMAKE_CURRENT_BINARY_DIR}/resources/\"") + endif() if(using_new_fcl) diff --git a/rmf_traffic/src/rmf_traffic/DetectConflict.cpp b/rmf_traffic/src/rmf_traffic/DetectConflict.cpp index 37669438..f51a6b29 100644 --- a/rmf_traffic/src/rmf_traffic/DetectConflict.cpp +++ b/rmf_traffic/src/rmf_traffic/DetectConflict.cpp @@ -779,15 +779,19 @@ rmf_utils::optional DetectConflict::Implementation::between( for (auto it = trajectory_a_actual.begin(); it != trajectory_a_actual.end(); ++it) { auto position = it->position(); auto velocity = it->velocity(); - if (zero) + if (zero) { position << position(0), position(1), 0.0; + velocity << velocity(0), velocity(1), 0.0; + } trajectory_a.insert(it->time(), position, velocity); } for (auto it = trajectory_b_actual.begin(); it != trajectory_b_actual.end(); ++it) { auto position = it->position(); auto velocity = it->velocity(); - if (zero) + if (zero) { position << position(0), position(1), 0.0; + velocity << velocity(0), velocity(1), 0.0; + } trajectory_b.insert(it->time(), position, velocity); } if (trajectory_a.size() < 2) diff --git a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp index 0376cd72..f79a4cbf 100644 --- a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp +++ b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp @@ -24,6 +24,8 @@ #include #include +#include + namespace { //============================================================================== @@ -136,47 +138,75 @@ SCENARIO("Failed Detect Conflict") auto plan = planner.plan({start_time, 5, 180.0 * M_PI / 180.0}, {2}); CHECK_FALSE(plan.success()); +} + +class SerializedWaypoint +{ +public: + Eigen::Vector3d position, velocity; + + rmf_traffic::Time time; +}; + +SerializedWaypoint read_waypoint(std::string file_name) +{ + std::ifstream file; + file.open(file_name, std::ios::in); + SerializedWaypoint serialized_waypoint; + file.seekg(0); + file.read((char*)&serialized_waypoint, sizeof(SerializedWaypoint)); + file.close(); + return serialized_waypoint; +} + +//============================================================================== +SCENARIO("Rotation failure") +{ + SerializedWaypoint serialized_waypoint[4]; + serialized_waypoint[0] = + read_waypoint(RESOURCES_DIR + std::string("/Waypoint1.txt")); + serialized_waypoint[1] = + read_waypoint(RESOURCES_DIR + std::string("/Waypoint2.txt")); + serialized_waypoint[2] = + read_waypoint(RESOURCES_DIR + std::string("/Waypoint3.txt")); + serialized_waypoint[3] = + read_waypoint(RESOURCES_DIR + std::string("/Waypoint4.txt")); - for (const auto& obstacle_it : plan_obstacle->get_itinerary()) - { - for (const auto& it : plan->get_itinerary()) - { - rmf_traffic::Trajectory obstacle_traj; - rmf_traffic::Trajectory traj; - for (auto waypoint_traj = obstacle_it.trajectory().begin(); - waypoint_traj != obstacle_it.trajectory().end(); ++waypoint_traj) - { - auto position = waypoint_traj->position(); - auto velocity = waypoint_traj->velocity(); - obstacle_traj.insert(waypoint_traj->time(), position, velocity); - } - for (auto waypoint_traj = it.trajectory().begin(); - waypoint_traj != it.trajectory().end(); - ++waypoint_traj) - { - auto position = waypoint_traj->position(); - auto velocity = waypoint_traj->velocity(); - traj.insert(waypoint_traj->time(), position, velocity); - } - - bool detect_conflict_flag_true = - rmf_traffic::DetectConflict::between(traits.profile(), - obstacle_traj, - traits.profile(), - traj, - rmf_traffic::DetectConflict::Interpolate::CubicSpline, - true).has_value(); - - bool detect_conflict_flag_false = - rmf_traffic::DetectConflict::between(traits.profile(), - obstacle_traj, - traits.profile(), - traj, - rmf_traffic::DetectConflict::Interpolate::CubicSpline, - false).has_value(); - - CHECK(detect_conflict_flag_true == detect_conflict_flag_false); + rmf_traffic::agv::VehicleTraits traits{ + {0.5, 2.0}, {0.75, 1.5}, + rmf_traffic::Profile{ + rmf_traffic::geometry::make_final_convex( + rmf_traffic::geometry::Circle(0.2)) } - } + }; + + rmf_traffic::Trajectory traj1, traj2; + traj1.insert(serialized_waypoint[0].time, serialized_waypoint[0].position, + serialized_waypoint[0].velocity); + traj1.insert(serialized_waypoint[1].time, serialized_waypoint[1].position, + serialized_waypoint[1].velocity); + + traj2.insert(serialized_waypoint[2].time, serialized_waypoint[2].position, + serialized_waypoint[2].velocity); + traj2.insert(serialized_waypoint[3].time, serialized_waypoint[3].position, + serialized_waypoint[3].velocity); + + bool detect_conflict_flag_true = + rmf_traffic::DetectConflict::between(traits.profile(), + traj1, + traits.profile(), + traj2, + rmf_traffic::DetectConflict::Interpolate::CubicSpline, + true).has_value(); + + bool detect_conflict_flag_false = + rmf_traffic::DetectConflict::between(traits.profile(), + traj1, + traits.profile(), + traj2, + rmf_traffic::DetectConflict::Interpolate::CubicSpline, + false).has_value(); + + CHECK(detect_conflict_flag_true == detect_conflict_flag_false); } } \ No newline at end of file diff --git a/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint1.txt b/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint1.txt new file mode 100644 index 0000000000000000000000000000000000000000..2d2e54be54242ef077623c5148745b387d056e0a GIT binary patch literal 56 zcmaz}x|EuBOv@qde>l5GzrukW4vrd4o&x*&ufMbZ|G#hl=EmvyBB2)h8KB_H?KdsU HnHU%Vy>%B& literal 0 HcmV?d00001 diff --git a/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint2.txt b/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint2.txt new file mode 100644 index 0000000000000000000000000000000000000000..20253979421c0882fe23a3fded05f00c92a90372 GIT binary patch literal 56 zcmb11H(35+Mt}o*xa#t}#R>;;H-ZVcJk>PZx|N1-o|Nr~@-`qGoUnJDRo&gF>rY+Oi I&&0q00MuC)x&QzG literal 0 HcmV?d00001 diff --git a/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint4.txt b/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint4.txt new file mode 100644 index 0000000000000000000000000000000000000000..fcbf3f02cb7c702232ca75db96e2515a6b5ea6cb GIT binary patch literal 56 zcmaz^x|EuBOv@qde>l5GzrulzH_eYlWH=q_zy8kt|Ns8}H#bht7YVhnXMlo7y`OGf IXJTLg0Q-Fy@c;k- literal 0 HcmV?d00001 From b5baabb65f610f62b059a171bccc9c3f46e7676a Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Tue, 20 Apr 2021 10:54:04 +0530 Subject: [PATCH 6/7] Revert changes to DetectConflict Signed-off-by: Shivesh Khaitan --- .../include/rmf_traffic/DetectConflict.hpp | 3 +- .../src/rmf_traffic/DetectConflict.cpp | 34 ++------ .../rmf_traffic/DetectConflictInternal.hpp | 3 +- .../regress_DetectConflict_fail.cpp | 78 ++++++++++++++----- 4 files changed, 66 insertions(+), 52 deletions(-) diff --git a/rmf_traffic/include/rmf_traffic/DetectConflict.hpp b/rmf_traffic/include/rmf_traffic/DetectConflict.hpp index 16ad7a4d..2590e4d7 100644 --- a/rmf_traffic/include/rmf_traffic/DetectConflict.hpp +++ b/rmf_traffic/include/rmf_traffic/DetectConflict.hpp @@ -56,8 +56,7 @@ class DetectConflict const Trajectory& trajectory_a, const Profile& profile_b, const Trajectory& trajectory_b, - Interpolate interpolation = Interpolate::CubicSpline, - bool zero = true); + Interpolate interpolation = Interpolate::CubicSpline); class Implementation; }; diff --git a/rmf_traffic/src/rmf_traffic/DetectConflict.cpp b/rmf_traffic/src/rmf_traffic/DetectConflict.cpp index f51a6b29..ee9f89b1 100644 --- a/rmf_traffic/src/rmf_traffic/DetectConflict.cpp +++ b/rmf_traffic/src/rmf_traffic/DetectConflict.cpp @@ -425,15 +425,12 @@ rmf_utils::optional DetectConflict::between( const Trajectory& trajectory_a, const Profile& profile_b, const Trajectory& trajectory_b, - Interpolate interpolation, - bool zero) + Interpolate interpolation) { return Implementation::between( profile_a, trajectory_a, profile_b, trajectory_b, - interpolation, - nullptr, - zero); + interpolation); } namespace { @@ -767,33 +764,12 @@ rmf_utils::optional detect_approach( //============================================================================== rmf_utils::optional DetectConflict::Implementation::between( const Profile& input_profile_a, - const Trajectory& trajectory_a_actual, + const Trajectory& trajectory_a, const Profile& input_profile_b, - const Trajectory& trajectory_b_actual, + const Trajectory& trajectory_b, Interpolate /*interpolation*/, - std::vector* output_conflicts, - bool zero) + std::vector* output_conflicts) { - rmf_traffic::Trajectory trajectory_a; - rmf_traffic::Trajectory trajectory_b; - for (auto it = trajectory_a_actual.begin(); it != trajectory_a_actual.end(); ++it) { - auto position = it->position(); - auto velocity = it->velocity(); - if (zero) { - position << position(0), position(1), 0.0; - velocity << velocity(0), velocity(1), 0.0; - } - trajectory_a.insert(it->time(), position, velocity); - } - for (auto it = trajectory_b_actual.begin(); it != trajectory_b_actual.end(); ++it) { - auto position = it->position(); - auto velocity = it->velocity(); - if (zero) { - position << position(0), position(1), 0.0; - velocity << velocity(0), velocity(1), 0.0; - } - trajectory_b.insert(it->time(), position, velocity); - } if (trajectory_a.size() < 2) { throw invalid_trajectory_error::Implementation diff --git a/rmf_traffic/src/rmf_traffic/DetectConflictInternal.hpp b/rmf_traffic/src/rmf_traffic/DetectConflictInternal.hpp index c416df65..44f82650 100644 --- a/rmf_traffic/src/rmf_traffic/DetectConflictInternal.hpp +++ b/rmf_traffic/src/rmf_traffic/DetectConflictInternal.hpp @@ -48,8 +48,7 @@ class DetectConflict::Implementation const Profile& profile_b, const Trajectory& trajectory_b, Interpolate interpolation, - std::vector* output_conflicts = nullptr, - bool zero = true); + std::vector* output_conflicts = nullptr); }; diff --git a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp index f79a4cbf..a6b948ab 100644 --- a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp +++ b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp @@ -19,10 +19,10 @@ #include #include -#include #include -#include #include +#include +#include #include @@ -130,7 +130,7 @@ SCENARIO("Failed Detect Conflict") database, std::numeric_limits::max(), traits.profile()); rmf_traffic::agv::Planner::Options options(obstacle_validator); - options.saturation_limit(1000000); + options.saturation_limit(100000); rmf_traffic::agv::Planner planner = rmf_traffic::agv::Planner{ {graph, traits}, options @@ -148,7 +148,7 @@ class SerializedWaypoint rmf_traffic::Time time; }; -SerializedWaypoint read_waypoint(std::string file_name) +SerializedWaypoint read_waypoint(const std::string& file_name) { std::ifstream file; file.open(file_name, std::ios::in); @@ -181,32 +181,72 @@ SCENARIO("Rotation failure") }; rmf_traffic::Trajectory traj1, traj2; - traj1.insert(serialized_waypoint[0].time, serialized_waypoint[0].position, + traj1.insert( + serialized_waypoint[0].time, + serialized_waypoint[0].position, serialized_waypoint[0].velocity); - traj1.insert(serialized_waypoint[1].time, serialized_waypoint[1].position, + traj1.insert( + serialized_waypoint[1].time, + serialized_waypoint[1].position, serialized_waypoint[1].velocity); - traj2.insert(serialized_waypoint[2].time, serialized_waypoint[2].position, + traj2.insert( + serialized_waypoint[2].time, + serialized_waypoint[2].position, serialized_waypoint[2].velocity); - traj2.insert(serialized_waypoint[3].time, serialized_waypoint[3].position, + traj2.insert( + serialized_waypoint[3].time, + serialized_waypoint[3].position, serialized_waypoint[3].velocity); - bool detect_conflict_flag_true = + bool detect_conflict_with_rotation = rmf_traffic::DetectConflict::between(traits.profile(), traj1, traits.profile(), - traj2, - rmf_traffic::DetectConflict::Interpolate::CubicSpline, - true).has_value(); - - bool detect_conflict_flag_false = + traj2).has_value(); + + rmf_traffic::Trajectory traj3, traj4; + traj3.insert( + serialized_waypoint[0].time, + {serialized_waypoint[0].position.x(), + serialized_waypoint[0].position.y(), + 0.0}, + {serialized_waypoint[0].velocity.x(), + serialized_waypoint[0].velocity.y(), + 0.0}); + traj3.insert( + serialized_waypoint[1].time, + {serialized_waypoint[1].position.x(), + serialized_waypoint[1].position.y(), + 0.0}, + {serialized_waypoint[1].velocity.x(), + serialized_waypoint[1].velocity.y(), + 0.0}); + + traj4.insert( + serialized_waypoint[2].time, + {serialized_waypoint[2].position.x(), + serialized_waypoint[2].position.y(), + 0.0}, + {serialized_waypoint[2].velocity.x(), + serialized_waypoint[2].velocity.y(), + 0.0}); + traj4.insert( + serialized_waypoint[3].time, + {serialized_waypoint[3].position.x(), + serialized_waypoint[3].position.y(), + 0.0}, + {serialized_waypoint[3].velocity.x(), + serialized_waypoint[3].velocity.y(), + 0.0}); + + + bool detect_conflict_without_rotation = rmf_traffic::DetectConflict::between(traits.profile(), - traj1, + traj3, traits.profile(), - traj2, - rmf_traffic::DetectConflict::Interpolate::CubicSpline, - false).has_value(); + traj4).has_value(); - CHECK(detect_conflict_flag_true == detect_conflict_flag_false); + CHECK(detect_conflict_with_rotation == detect_conflict_without_rotation); } } \ No newline at end of file From 816fd1dbeb53af83d93beea6442f8d54efc77bf2 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Wed, 21 Apr 2021 20:14:58 +0530 Subject: [PATCH 7/7] Use binary waypoints for test Signed-off-by: Shivesh Khaitan --- .../regress_DetectConflict_fail.cpp | 174 ++++++------------ .../regress_DetectConflict_fail/Waypoint5.txt | Bin 0 -> 56 bytes .../regress_DetectConflict_fail/Waypoint6.txt | Bin 0 -> 56 bytes .../regress_DetectConflict_fail/Waypoint7.txt | Bin 0 -> 56 bytes .../regress_DetectConflict_fail/Waypoint8.txt | Bin 0 -> 56 bytes 5 files changed, 58 insertions(+), 116 deletions(-) create mode 100644 rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint5.txt create mode 100644 rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint6.txt create mode 100644 rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint7.txt create mode 100644 rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint8.txt diff --git a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp index a6b948ab..b7e5b41d 100644 --- a/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp +++ b/rmf_traffic/test/regression/regress_DetectConflict_fail.cpp @@ -15,131 +15,15 @@ * */ -#include -#include - #include #include #include -#include #include #include namespace { -//============================================================================== -SCENARIO("Failed Detect Conflict") -{ - rmf_traffic::agv::VehicleTraits traits{ - {0.5, 2.0}, {0.75, 1.5}, - rmf_traffic::Profile{ - rmf_traffic::geometry::make_final_convex( - rmf_traffic::geometry::Circle(0.2)) - } - }; - - auto graph = rmf_traffic::agv::Graph(); - - graph.add_waypoint("test", Eigen::Vector2d(66.4517, -8.31925)); - graph.add_waypoint("test", Eigen::Vector2d(66.4694, -11.86215)); - graph.add_waypoint("test", Eigen::Vector2d(63.575, -11.875)); - graph.add_waypoint("test", Eigen::Vector2d(16.097900000000003, -12.8518)); - graph.add_waypoint("test", Eigen::Vector2d(13.2777, -12.980850000000002)); - graph.add_waypoint("test", Eigen::Vector2d(13.325, -8.2798)); - graph.add_waypoint("test", - Eigen::Vector2d(117.30075000000001, -12.863300000000002)); - graph.add_waypoint("test", Eigen::Vector2d(120.03815, -12.8944)); - graph.add_waypoint("test", Eigen::Vector2d(120.05175, -8.3695)); - graph.add_waypoint("test", - Eigen::Vector2d(13.675250000000002, -28.884750000000004)); - graph.add_waypoint("test", Eigen::Vector2d(66.48685, -28.959050000000005)); - graph.add_waypoint("test", Eigen::Vector2d(119.97360000000002, -28.97215)); - graph.add_waypoint("test", Eigen::Vector2d(18.28865, -8.2941)); - graph.add_waypoint("test", Eigen::Vector2d(24.8581, -8.2971)); - graph.add_waypoint("test", Eigen::Vector2d(30.8229, -8.302249999999999)); - graph.add_waypoint("test", Eigen::Vector2d(38.977000000000004, -8.3191)); - graph.add_waypoint("test", Eigen::Vector2d(44.60830000000001, -8.324)); - graph.add_waypoint("test", Eigen::Vector2d(52.278800000000004, -8.3107)); - graph.add_waypoint("test", Eigen::Vector2d(63.325, -8.3315)); - - graph.add_lane(0, 1); - graph.add_lane(1, 0); - graph.add_lane(1, 2); - graph.add_lane(2, 1); - graph.add_lane(3, 4); - graph.add_lane(4, 3); - graph.add_lane(4, 5); - graph.add_lane(5, 4); - graph.add_lane(6, 7); - graph.add_lane(7, 6); - graph.add_lane(7, 8); - graph.add_lane(8, 7); - graph.add_lane(4, 9); - graph.add_lane(9, 4); - graph.add_lane(1, 10); - graph.add_lane(10, 1); - graph.add_lane(0, 8); - graph.add_lane(8, 0); - graph.add_lane(7, 11); - graph.add_lane(11, 7); - graph.add_lane(5, 12); - graph.add_lane(12, 13); - graph.add_lane(13, 14); - graph.add_lane(14, 15); - graph.add_lane(15, 16); - graph.add_lane(16, 17); - graph.add_lane(17, 18); - graph.add_lane(18, 0); - graph.add_lane(0, 18); - graph.add_lane(18, 17); - graph.add_lane(17, 16); - graph.add_lane(16, 15); - graph.add_lane(15, 14); - graph.add_lane(14, 13); - graph.add_lane(13, 12); - graph.add_lane(12, 5); - - const auto start_time = std::chrono::steady_clock::now(); - - const auto database = std::make_shared(); - - rmf_traffic::agv::Planner planner_obstacle = rmf_traffic::agv::Planner{ - {graph, traits}, - {nullptr} - }; - - const auto N = database->participant_ids().size(); - - auto new_obstacle = rmf_traffic::schedule::make_participant( - rmf_traffic::schedule::ParticipantDescription{ - "obstacle_" + std::to_string(N), - "obstacles", - rmf_traffic::schedule::ParticipantDescription::Rx::Unresponsive, - planner_obstacle.get_configuration().vehicle_traits().profile() - }, database); - - auto plan_obstacle = - planner_obstacle.plan({rmf_traffic::time::apply_offset(start_time, - 106.362), 1, 90.0 * M_PI / 180.0}, {5}); - - new_obstacle.set(plan_obstacle->get_itinerary()); - - const auto obstacle_validator = - rmf_traffic::agv::ScheduleRouteValidator::make( - database, std::numeric_limits::max(), traits.profile()); - - rmf_traffic::agv::Planner::Options options(obstacle_validator); - options.saturation_limit(100000); - rmf_traffic::agv::Planner planner = rmf_traffic::agv::Planner{ - {graph, traits}, - options - }; - auto plan = planner.plan({start_time, 5, 180.0 * M_PI / 180.0}, {2}); - - CHECK_FALSE(plan.success()); -} - class SerializedWaypoint { public: @@ -159,6 +43,64 @@ SerializedWaypoint read_waypoint(const std::string& file_name) return serialized_waypoint; } +void write_waypoint(const std::string& file_name, + SerializedWaypoint& serialized_waypoint) +{ + std::ofstream file; + file.open(file_name, std::ios::out); + file.write((char*) &serialized_waypoint, sizeof(SerializedWaypoint)); + file.close(); +} + +//============================================================================== +SCENARIO("Failed Detect Conflict") +{ + SerializedWaypoint serialized_waypoint[4]; + serialized_waypoint[0] = + read_waypoint(RESOURCES_DIR + std::string("/Waypoint5.txt")); + serialized_waypoint[1] = + read_waypoint(RESOURCES_DIR + std::string("/Waypoint6.txt")); + serialized_waypoint[2] = + read_waypoint(RESOURCES_DIR + std::string("/Waypoint7.txt")); + serialized_waypoint[3] = + read_waypoint(RESOURCES_DIR + std::string("/Waypoint8.txt")); + + rmf_traffic::agv::VehicleTraits traits{ + {0.5, 2.0}, {0.75, 1.5}, + rmf_traffic::Profile{ + rmf_traffic::geometry::make_final_convex( + rmf_traffic::geometry::Circle(0.2)) + } + }; + + rmf_traffic::Trajectory traj1, traj2; + traj1.insert( + serialized_waypoint[0].time, + serialized_waypoint[0].position, + serialized_waypoint[0].velocity); + traj1.insert( + serialized_waypoint[1].time, + serialized_waypoint[1].position, + serialized_waypoint[1].velocity); + + traj2.insert( + serialized_waypoint[2].time, + serialized_waypoint[2].position, + serialized_waypoint[2].velocity); + traj2.insert( + serialized_waypoint[3].time, + serialized_waypoint[3].position, + serialized_waypoint[3].velocity); + + bool detect_conflict = + rmf_traffic::DetectConflict::between(traits.profile(), + traj1, + traits.profile(), + traj2).has_value(); + + CHECK(detect_conflict); +} + //============================================================================== SCENARIO("Rotation failure") { diff --git a/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint5.txt b/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint5.txt new file mode 100644 index 0000000000000000000000000000000000000000..83b97b8a0a8320b16c72b4e6d6d20e3b5b9486ce GIT binary patch literal 56 zcmbQaWmO03ya0!#t~)Mg&s0BP`|j}lLbIRtf2EeaZY@jMU(;-H<>~+X_6$%kbCY~- I9}@!u05Jp@`2YX_ literal 0 HcmV?d00001 diff --git a/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint6.txt b/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint6.txt new file mode 100644 index 0000000000000000000000000000000000000000..f1090fec723c7b8f050ba2b4bd5b22dd3c9171af GIT binary patch literal 56 zcmZoo+<)ZloB)Tq?RJYw4l5k6eRueNq1jLSzf#Lyx0a>suW7cp^7Q|Gdj=>tB0QO8 I0uuuR08XA6E&u=k literal 0 HcmV?d00001 diff --git a/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint7.txt b/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint7.txt new file mode 100644 index 0000000000000000000000000000000000000000..991ff44d2f9036be43707932b35e6ae9861a3ecf GIT binary patch literal 56 zcmZoo+<)ZloB)T~?RJYw4l5k^oPFd(3-8bUf2EeaZY@i(uW7cp^7Q}x{R~hL_}@*S IpNWA10A7i_@% literal 0 HcmV?d00001 diff --git a/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint8.txt b/rmf_traffic/test/resources/regress_DetectConflict_fail/Waypoint8.txt new file mode 100644 index 0000000000000000000000000000000000000000..f8e373e2a7aa22b631c609cf84d9414473594075 GIT binary patch literal 56 zcmbQaWmO03ya0zKt~)Mg&s0C~Is3?o7T%xx|4J=;-CCAnU(;-H<>~+X`x&61CSlj| I2}}$O06|$9=Kufz literal 0 HcmV?d00001