From 142e30baeba1147bc0c7858a8ebd37d44b2cec3b Mon Sep 17 00:00:00 2001 From: Decwest Date: Mon, 6 Oct 2025 20:50:34 +0900 Subject: [PATCH 01/29] :zap: add dwpp algorithm --- .../parameter_handler.hpp | 6 +- .../regulated_pure_pursuit_controller.hpp | 18 +++ .../src/parameter_handler.cpp | 13 ++ .../src/regulated_pure_pursuit_controller.cpp | 144 +++++++++++++++++- 4 files changed, 179 insertions(+), 2 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 2b03443f5fb..a9d45d4223f 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America +// Copyright (c) 2025 Fumiya Ohnishi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -33,6 +34,7 @@ namespace nav2_regulated_pure_pursuit_controller struct Parameters { double desired_linear_vel, base_desired_linear_vel; + double desired_angular_vel; double lookahead_dist; double rotate_to_heading_angular_vel; double max_lookahead_dist; @@ -53,7 +55,7 @@ struct Parameters bool use_fixed_curvature_lookahead; double curvature_lookahead_dist; bool use_rotate_to_heading; - double max_angular_accel; + double max_linear_accel, max_angular_accel; bool use_cancel_deceleration; double cancel_deceleration; double rotate_to_heading_min_angle; @@ -63,6 +65,8 @@ struct Parameters bool use_collision_detection; double transform_tolerance; bool stateful; + bool use_dynamic_window; + std::string velocity_feedback; }; /** diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index fd79208d291..7c4d83c439e 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -1,5 +1,6 @@ // Copyright (c) 2020 Shrijit Singh // Copyright (c) 2020 Samsung Research America +// Copyright (c) 2025 Fumiya Ohnishi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -80,6 +81,22 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ void deactivate() override; + /** + * @brief Compute the optimal command given the current pose, velocity and acceleration constraints using dynamic window + * @param curvature Curvature of the path to follow + * @param current_speed Current robot velocity + * @param regulated_linear_vel Regulated linear velocity + * @param optimal_linear_vel Optimal linear velocity to follow the path under velocity and acceleration constraints + * @param optimal_angular_vel Optimal angular velocity to follow the path under velocity and acceleration constraints + */ + void computeOptimalVelocityUsingDynamicWindow( + const double curvature, + const geometry_msgs::msg::Twist current_speed, + const double regulated_linear_vel, + double & optimal_linear_vel, + double & optimal_angular_vel + ); + /** * @brief Compute the best command given the current pose and velocity, with possible debug information * @@ -195,6 +212,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool finished_cancelling_ = false; bool is_rotating_to_heading_ = false; bool has_reached_xy_tolerance_ = false; + geometry_msgs::msg::Twist last_command_velocity_; nav2::Publisher::SharedPtr global_path_pub_; nav2::Publisher::SharedPtr carrot_pub_; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 5bd1f2a7eeb..a3c015bca0f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America +// Copyright (c) 2025 Fumiya Ohnishi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -38,6 +39,8 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_angular_vel", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( @@ -87,6 +90,8 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(0.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( @@ -106,9 +111,14 @@ ParameterHandler::ParameterHandler( rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_dynamic_window", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".velocity_feedback", rclcpp::ParameterValue(std::string("OPEN_LOOP"))); node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); params_.base_desired_linear_vel = params_.desired_linear_vel; + node->get_parameter(plugin_name_ + ".desired_angular_vel", params_.desired_angular_vel); node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist); node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist); node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist); @@ -163,6 +173,7 @@ ParameterHandler::ParameterHandler( node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading); node->get_parameter( plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); + node->get_parameter(plugin_name_ + ".max_linear_accel", params_.max_linear_accel); node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); @@ -190,6 +201,8 @@ ParameterHandler::ParameterHandler( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); node->get_parameter(plugin_name_ + ".stateful", params_.stateful); + node->get_parameter(plugin_name_ + ".use_dynamic_window", params_.use_dynamic_window); + node->get_parameter(plugin_name_ + ".velocity_feedback", params_.velocity_feedback); if (params_.inflation_cost_scaling_factor <= 0.0) { RCLCPP_WARN( diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index fced1dff734..e4239c938ee 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2020 Shrijit Singh // Copyright (c) 2020 Samsung Research America +// Copyright (c) 2025 Fumiya Ohnishi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -162,6 +163,130 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point) } } +void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( + const double curvature, + const geometry_msgs::msg::Twist current_speed, + const double regulated_linear_vel, + double & optimal_linear_vel, + double & optimal_angular_vel + ) +{ + // ---- Parameters (assumed available in this scope) ---- + const double A_MAX = params_->max_linear_accel; // A_MAX + const double V_MAX = params_->desired_linear_vel; // V_MAX + const double AW_MAX = params_->max_angular_accel; // AW_MAX + const double W_MAX = params_->desired_angular_vel; // W_MAX + const double DT = control_duration_; // DT + + const double V_MIN = -V_MAX; + const double W_MIN = -W_MAX; + + // ---- 1) Dynamic Window ---- + double dw_vmax = std::min(current_speed.linear.x + A_MAX * DT, V_MAX); + const double dw_vmin = std::max(current_speed.linear.x - A_MAX * DT, V_MIN); + double dw_wmax = std::min(current_speed.angular.z + AW_MAX * DT, W_MAX); + const double dw_wmin = std::max(current_speed.angular.z - AW_MAX * DT, W_MIN); + + // Reflect regulated v (tighten upper limit) + if (dw_vmax > regulated_linear_vel) { + dw_vmax = std::max(dw_vmin, regulated_linear_vel); + } + + const double k = curvature; + + // ---- Curvature is 0 (w = 0) ---- + if (k == 0.0) { + // If w=0 is within DW, then the maximum linear speed is adopted as it is. + if (dw_wmin <= 0.0 && 0.0 <= dw_wmax) { + optimal_linear_vel = dw_vmax; // Always maximum v + optimal_angular_vel = 0.0; + return; + } + // If w=0 is outside, choose the side closer to w=0 and with smaller |w|. + const double w_choice = (std::abs(dw_wmin) <= std::abs(dw_wmax)) ? dw_wmin : dw_wmax; + optimal_linear_vel = dw_vmax; // Always maximum v + optimal_angular_vel = w_choice; + return; + } + + // ---- 2) Select 'max v' from the candidates in the DW among the intersections. ---- + double best_v = -1e300; // Initial value for maximization + double best_w = 0.0; + + // Intersection with vertical edges + { + const double v1 = dw_vmin; + const double w1 = k * v1; + if (w1 >= dw_wmin && w1 <= dw_wmax) { + if (v1 > best_v) { best_v = v1; best_w = w1; } + } + } + { + const double v2 = dw_vmax; + const double w2 = k * v2; + if (w2 >= dw_wmin && w2 <= dw_wmax) { + if (v2 > best_v) { best_v = v2; best_w = w2; } + } + } + + // Intersection with horizontal edge (k ! = 0) + { + const double v3 = dw_wmin / k; + if (v3 >= dw_vmin && v3 <= dw_vmax) { + const double w3 = dw_wmin; + if (v3 > best_v) { best_v = v3; best_w = w3; } + } + } + { + const double v4 = dw_wmax / k; + if (v4 >= dw_vmin && v4 <= dw_vmax) { + const double w4 = dw_wmax; + if (v4 > best_v) { best_v = v4; best_w = w4; } + } + } + + if (best_v > -1e290) { + // Intersection found → Adopt max. v + optimal_linear_vel = best_v; + optimal_angular_vel = best_w; + return; + } + + // ---- 3) If no intersection exists: Select the one with the smallest Euclidean distance to the line w = k v among the 4 corners + struct Corner { double v; double w; }; + const Corner corners[4] = { + {dw_vmin, dw_wmin}, + {dw_vmin, dw_wmax}, + {dw_vmax, dw_wmin}, + {dw_vmax, dw_wmax} + }; + + const double denom = std::sqrt(k * k + 1.0); // Just sqrt once + + auto euclid_dist = [&](const Corner &c) -> double { + // Distance from point (v, w) to line w - k v = 0 + return std::abs(k * c.v - c.w) / denom; + }; + + double best_dist = 1e300; + best_v = corners[0].v; + best_w = corners[0].w; + + for (int i = 0; i < 4; ++i) { + const double d = euclid_dist(corners[i]); + // 1) Smaller distance → Adopted + // 2) If distances are equal (~1e-12) → Choose the one with larger v (acceleration policy) + if (d < best_dist || (std::abs(d - best_dist) <= 1e-12 && corners[i].v > best_v)) { + best_dist = d; + best_v = corners[i].v; + best_w = corners[i].w; + } + } + + optimal_linear_vel = best_v; + optimal_angular_vel = best_w; +} + geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & speed, @@ -269,7 +394,21 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Apply curvature to angular velocity after constraining linear velocity - angular_vel = linear_vel * regulation_curvature; + if (params_->use_dynamic_window == false){ + angular_vel = linear_vel * regulation_curvature; + } + else{ + // compute optimal path tracking velocity commands considering velocity and acceleration constraints + const double regulated_linear_vel = linear_vel; + if (params_->velocity_feedback == "CLOSED_LOOP"){ + // using odom velocity as a current velocity (not recommended) + computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, regulated_linear_vel, linear_vel, angular_vel); + } + else{ + // using last command velocity as a current velocity (recommended) + computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, regulated_linear_vel, linear_vel, angular_vel); + } + } } // Collision checking on this velocity heading @@ -290,6 +429,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity cmd_vel.header = pose.header; cmd_vel.twist.linear.x = linear_vel; cmd_vel.twist.angular.z = angular_vel; + + last_command_velocity_ = cmd_vel.twist; + return cmd_vel; } From 547c54ef2d32fb2fecc79a6641274daacf299d7e Mon Sep 17 00:00:00 2001 From: Decwest Date: Sat, 11 Oct 2025 21:45:28 +0900 Subject: [PATCH 02/29] :recycle: modify to pass lint --- .../src/regulated_pure_pursuit_controller.cpp | 72 +- nav2_route/graphs/aws_graph.geojson | 1792 +++++++++++++++-- nav2_route/graphs/sample_graph.geojson | 581 +++++- 3 files changed, 2181 insertions(+), 264 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index e4239c938ee..ed0d7d676ee 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -164,26 +164,26 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point) } void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( - const double curvature, - const geometry_msgs::msg::Twist current_speed, - const double regulated_linear_vel, - double & optimal_linear_vel, - double & optimal_angular_vel - ) + const double curvature, + const geometry_msgs::msg::Twist current_speed, + const double regulated_linear_vel, + double & optimal_linear_vel, + double & optimal_angular_vel +) { // ---- Parameters (assumed available in this scope) ---- - const double A_MAX = params_->max_linear_accel; // A_MAX - const double V_MAX = params_->desired_linear_vel; // V_MAX + const double A_MAX = params_->max_linear_accel; // A_MAX + const double V_MAX = params_->desired_linear_vel; // V_MAX const double AW_MAX = params_->max_angular_accel; // AW_MAX - const double W_MAX = params_->desired_angular_vel; // W_MAX - const double DT = control_duration_; // DT + const double W_MAX = params_->desired_angular_vel; // W_MAX + const double DT = control_duration_; // DT const double V_MIN = -V_MAX; const double W_MIN = -W_MAX; // ---- 1) Dynamic Window ---- - double dw_vmax = std::min(current_speed.linear.x + A_MAX * DT, V_MAX); - const double dw_vmin = std::max(current_speed.linear.x - A_MAX * DT, V_MIN); + double dw_vmax = std::min(current_speed.linear.x + A_MAX * DT, V_MAX); + const double dw_vmin = std::max(current_speed.linear.x - A_MAX * DT, V_MIN); double dw_wmax = std::min(current_speed.angular.z + AW_MAX * DT, W_MAX); const double dw_wmin = std::max(current_speed.angular.z - AW_MAX * DT, W_MIN); @@ -198,13 +198,13 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( if (k == 0.0) { // If w=0 is within DW, then the maximum linear speed is adopted as it is. if (dw_wmin <= 0.0 && 0.0 <= dw_wmax) { - optimal_linear_vel = dw_vmax; // Always maximum v + optimal_linear_vel = dw_vmax; // Always maximum v optimal_angular_vel = 0.0; return; } // If w=0 is outside, choose the side closer to w=0 and with smaller |w|. const double w_choice = (std::abs(dw_wmin) <= std::abs(dw_wmax)) ? dw_wmin : dw_wmax; - optimal_linear_vel = dw_vmax; // Always maximum v + optimal_linear_vel = dw_vmax; // Always maximum v optimal_angular_vel = w_choice; return; } @@ -218,14 +218,14 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double v1 = dw_vmin; const double w1 = k * v1; if (w1 >= dw_wmin && w1 <= dw_wmax) { - if (v1 > best_v) { best_v = v1; best_w = w1; } + if (v1 > best_v) {best_v = v1; best_w = w1;} } } { const double v2 = dw_vmax; const double w2 = k * v2; if (w2 >= dw_wmin && w2 <= dw_wmax) { - if (v2 > best_v) { best_v = v2; best_w = w2; } + if (v2 > best_v) {best_v = v2; best_w = w2;} } } @@ -234,25 +234,26 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double v3 = dw_wmin / k; if (v3 >= dw_vmin && v3 <= dw_vmax) { const double w3 = dw_wmin; - if (v3 > best_v) { best_v = v3; best_w = w3; } + if (v3 > best_v) {best_v = v3; best_w = w3;} } } { const double v4 = dw_wmax / k; if (v4 >= dw_vmin && v4 <= dw_vmax) { const double w4 = dw_wmax; - if (v4 > best_v) { best_v = v4; best_w = w4; } + if (v4 > best_v) {best_v = v4; best_w = w4;} } } if (best_v > -1e290) { // Intersection found → Adopt max. v - optimal_linear_vel = best_v; + optimal_linear_vel = best_v; optimal_angular_vel = best_w; return; } - // ---- 3) If no intersection exists: Select the one with the smallest Euclidean distance to the line w = k v among the 4 corners + // ---- 3) If no intersection exists: Select the one with the smallest + // Euclidean distance to the line w = k v among the 4 corners struct Corner { double v; double w; }; const Corner corners[4] = { {dw_vmin, dw_wmin}, @@ -261,12 +262,12 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( {dw_vmax, dw_wmax} }; - const double denom = std::sqrt(k * k + 1.0); // Just sqrt once + const double denom = std::sqrt(k * k + 1.0); // Just sqrt once - auto euclid_dist = [&](const Corner &c) -> double { + auto euclid_dist = [&](const Corner & c) -> double { // Distance from point (v, w) to line w - k v = 0 - return std::abs(k * c.v - c.w) / denom; - }; + return std::abs(k * c.v - c.w) / denom; + }; double best_dist = 1e300; best_v = corners[0].v; @@ -283,7 +284,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( } } - optimal_linear_vel = best_v; + optimal_linear_vel = best_v; optimal_angular_vel = best_w; } @@ -394,19 +395,20 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Apply curvature to angular velocity after constraining linear velocity - if (params_->use_dynamic_window == false){ + if (!params_->use_dynamic_window) { angular_vel = linear_vel * regulation_curvature; - } - else{ - // compute optimal path tracking velocity commands considering velocity and acceleration constraints + } else { + // compute optimal path tracking velocity commands + // considering velocity and acceleration constraints const double regulated_linear_vel = linear_vel; - if (params_->velocity_feedback == "CLOSED_LOOP"){ + if (params_->velocity_feedback == "CLOSED_LOOP") { // using odom velocity as a current velocity (not recommended) - computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, regulated_linear_vel, linear_vel, angular_vel); - } - else{ + computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, + regulated_linear_vel, linear_vel, angular_vel); + } else { // using last command velocity as a current velocity (recommended) - computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, regulated_linear_vel, linear_vel, angular_vel); + computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, + regulated_linear_vel, linear_vel, angular_vel); } } } @@ -429,7 +431,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity cmd_vel.header = pose.header; cmd_vel.twist.linear.x = linear_vel; cmd_vel.twist.angular.z = angular_vel; - + last_command_velocity_ = cmd_vel.twist; return cmd_vel; diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson index 2e6a43af148..4c4fb3208ff 100644 --- a/nav2_route/graphs/aws_graph.geojson +++ b/nav2_route/graphs/aws_graph.geojson @@ -1,132 +1,1664 @@ { -"type": "FeatureCollection", -"name": "graph", -"crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, -"date_generated": "Wed Feb 22 05:41:45 PM EST 2025", -"features": [ -{ "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, -{ "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, -{ "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, -{ "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, -{ "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, -{ "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, -{ "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, -{ "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, -{ "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, -{ "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, -{ "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, -{ "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, -{ "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, -{ "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, -{ "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, -{ "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, -{ "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, -{ "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, -{ "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, -{ "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, -{ "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, -{ "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, -{ "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, -{ "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, -{ "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, -{ "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, -{ "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, -{ "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, -{ "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, -{ "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, -{ "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } -] + "crs": { + "properties": { + "name": "urn:ogc:def:crs:EPSG::3857" + }, + "type": "name" + }, + "features": [ + { + "geometry": { + "coordinates": [ + -4.0, + -9.350000381469727 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -3.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 0.6499999761581421 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 5.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 3.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 8.649999618530273 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -7.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -7.763441562652588 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -7.874690055847168 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 16 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -7.874690055847168 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 17 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + -8.690509796142578 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -9.395081520080566 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 3.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 22 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 4.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 23 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 2.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 24 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 25 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 26 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 27 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -6.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 28 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -9.350000381469727 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 29 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 30 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -5.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 35 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 39 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 75, + "overridable": true, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 19, + "id": 92, + "overridable": true, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 78, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 89, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 26, + "id": 102, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 119, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 120, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 79, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 88, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 24, + "id": 99, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 30, + "id": 117, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 118, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 81, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 86, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 80, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 87, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 122, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 82, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 21, + "id": 83, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 85, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 22, + "id": 96, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 84, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 76, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 91, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 28, + "id": 105, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 42, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 52, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 38, + "id": 54, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 10, + "id": 43, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 51, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 30, + "id": 115, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 11, + "id": 44, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 50, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 34, + "id": 59, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 111, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 45, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 10, + "id": 49, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 108, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 46, + "overridable": true, + "startid": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 11, + "id": 48, + "overridable": true, + "startid": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 63, + "overridable": true, + "startid": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 16, + "id": 71, + "overridable": true, + "startid": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 13, + "id": 62, + "overridable": true, + "startid": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 17, + "id": 64, + "overridable": true, + "startid": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 47, + "overridable": true, + "startid": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 18, + "id": 72, + "overridable": true, + "startid": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 66, + "overridable": true, + "startid": 16 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 16, + "id": 65, + "overridable": true, + "startid": 17 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 19, + "id": 73, + "overridable": true, + "startid": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 94, + "overridable": true, + "startid": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 74, + "overridable": true, + "startid": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 18, + "id": 93, + "overridable": true, + "startid": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 41, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 21, + "id": 53, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 123, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 40, + "overridable": true, + "startid": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 95, + "overridable": true, + "startid": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 23, + "id": 97, + "overridable": true, + "startid": 22 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 98, + "overridable": true, + "startid": 23 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 25, + "id": 100, + "overridable": true, + "startid": 24 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 101, + "overridable": true, + "startid": 25 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 27, + "id": 103, + "overridable": true, + "startid": 26 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 104, + "overridable": true, + "startid": 27 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 29, + "id": 106, + "overridable": true, + "startid": 28 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 107, + "overridable": true, + "startid": 29 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 116, + "overridable": true, + "startid": 30 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 112, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 113, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 114, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 109, + "overridable": true, + "startid": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 110, + "overridable": true, + "startid": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 77, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 90, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 121, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 35, + "id": 60, + "overridable": true, + "startid": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 13, + "id": 70, + "overridable": true, + "startid": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 14, + "id": 61, + "overridable": true, + "startid": 35 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 37, + "id": 57, + "overridable": true, + "startid": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 35, + "id": 67, + "overridable": true, + "startid": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 58, + "overridable": true, + "startid": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 34, + "id": 69, + "overridable": true, + "startid": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 39, + "id": 55, + "overridable": true, + "startid": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 37, + "id": 68, + "overridable": true, + "startid": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 36, + "id": 56, + "overridable": true, + "startid": 39 + }, + "type": "Feature" + } + ], + "name": "graph", + "type": "FeatureCollection" } diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson index 162a1a88884..87a19b468ee 100644 --- a/nav2_route/graphs/sample_graph.geojson +++ b/nav2_route/graphs/sample_graph.geojson @@ -1,121 +1,504 @@ { - "type": "FeatureCollection", - "name": "graph", - "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, - "date_generated": "Tue Feb 28 07:48:03 PM EST 2025", + "crs": { + "properties": { + "name": "urn:ogc:def:crs:EPSG::3857" + }, + "type": "name" + }, "features": [ { - "type": "Feature", - "properties": - { - "id": 0, + "geometry": { + "coordinates": [ + 0.0, + 0.0 + ], + "type": "Point" + }, + "properties": { "frame": "map", - "metadata": - { - "region": - { - "x_values": [1.0, -1.0, -1.0, 1.0], - "y_values": [1.0, 1.0, -1.0, -1.0], + "id": 0, + "metadata": { + "region": { "properties": { "class_type": "living_room", "number_of_lights": 10 - } + }, + "x_values": [ + 1.0, + -1.0, + -1.0, + 1.0 + ], + "y_values": [ + 1.0, + 1.0, + -1.0, + -1.0 + ] } }, - "operations": - { - "stop": - { - "type": "stop", - "trigger": "NODE", - "metadata": - { + "operations": { + "stop": { + "metadata": { "wait_for": 5.0 - } + }, + "trigger": "NODE", + "type": "stop" } } }, - "geometry": - { - "type": "Point", - "coordinates": [ 0.0, 0.0 ] - } - }, - { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, - { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, - { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 1.0 ] } }, - { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.0 ] } }, - { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 1.0 ] } }, - { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, - { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 2.0 ] } }, - { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 2.0 ] } }, - { - "type": "Feature", - "properties": - { - "id": 9, - "startid": 0, - "endid": 1, - "overridable": false, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 0.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 0.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { "cost": 10.0, - "metadata": - { - "speed_limit": 0.85, - "aisle_number": 14 + "endid": 1, + "id": 9, + "metadata": { + "aisle_number": 14, + "speed_limit": 0.8500000238418579 }, - "operations": - { - "open_door": - { - "type": "open_door", - "trigger": "ON_ENTER", - "metadata": - { + "operations": { + "open_door": { + "metadata": { "door_id": 54, "service_name": "open_door" - } + }, + "trigger": "ON_ENTER", + "type": "open_door" }, - "take_picture": - { - "type": "take_picture", + "take_picture": { + "metadata": { + "resolution": [ + 1080, + 720 + ], + "type": "jpg" + }, "trigger": "ON_EXIT", - "metadata": - { - "type": "jpg", - "resolution": [1080, 720] - } + "type": "take_picture" } - } + }, + "overridable": false, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 13, + "overridable": true, + "startid": 0 }, - "geometry": - { - "type": "MultiLineString", - "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] - } - }, - { "type": "Feature", "properties": { "id": 10, "startid": 1, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 0.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 11, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 2.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 12, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 13, "startid": 0, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 0.0 ], [ 0.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 14, "startid": 3, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 16, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 17, "startid": 2, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 2.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 18, "startid": 5, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 19, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 20, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 0.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 2.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 22, "startid": 5, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 23, "startid": 3, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 24, "startid": 6, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 25, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 26, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 27, "startid": 5, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 28, "startid": 8, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 2.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 29, "startid": 6, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 1.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 30, "startid": 7, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 0.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 31, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 2.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 32, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 1.0, 2.0 ] ] ] } } - - ] + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 10, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 11, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 15, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 12, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 17, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 14, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 19, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 23, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 16, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 20, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 21, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 25, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 18, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 22, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 27, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 24, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 29, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 26, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 30, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 31, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 28, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 32, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + } + ], + "name": "graph", + "type": "FeatureCollection" } From 53c5cd4f6c79ea0f37d7151af39ef1a67b181889 Mon Sep 17 00:00:00 2001 From: Decwest Date: Sat, 11 Oct 2025 21:52:22 +0900 Subject: [PATCH 03/29] :recycle: delete unrelated files & test signed off Signed-off-by: Decwest --- nav2_route/graphs/aws_graph.geojson | 1664 ------------------------ nav2_route/graphs/sample_graph.geojson | 504 ------- 2 files changed, 2168 deletions(-) delete mode 100644 nav2_route/graphs/aws_graph.geojson delete mode 100644 nav2_route/graphs/sample_graph.geojson diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson deleted file mode 100644 index 4c4fb3208ff..00000000000 --- a/nav2_route/graphs/aws_graph.geojson +++ /dev/null @@ -1,1664 +0,0 @@ -{ - "crs": { - "properties": { - "name": "urn:ogc:def:crs:EPSG::3857" - }, - "type": "name" - }, - "features": [ - { - "geometry": { - "coordinates": [ - -4.0, - -9.350000381469727 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -3.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 0.6499999761581421 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 5.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 3.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 8.649999618530273 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -7.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -7.763441562652588 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -7.874690055847168 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 16 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -7.874690055847168 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 17 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - -8.690509796142578 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -9.395081520080566 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 3.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 22 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 4.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 23 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 2.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 24 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 25 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 26 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 27 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -6.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 28 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -9.350000381469727 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 29 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 30 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -5.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 35 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 39 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 75, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 19, - "id": 92, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 78, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 89, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 26, - "id": 102, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 119, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 120, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 79, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 88, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 24, - "id": 99, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 30, - "id": 117, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 118, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 81, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 86, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 80, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 87, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 122, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 82, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 21, - "id": 83, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 85, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 22, - "id": 96, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 84, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 76, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 91, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 28, - "id": 105, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 42, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 52, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 38, - "id": 54, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 10, - "id": 43, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 51, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 30, - "id": 115, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 11, - "id": 44, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 50, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 34, - "id": 59, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 111, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 45, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 10, - "id": 49, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 108, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 46, - "overridable": true, - "startid": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 11, - "id": 48, - "overridable": true, - "startid": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 63, - "overridable": true, - "startid": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 16, - "id": 71, - "overridable": true, - "startid": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 13, - "id": 62, - "overridable": true, - "startid": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 17, - "id": 64, - "overridable": true, - "startid": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 47, - "overridable": true, - "startid": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 18, - "id": 72, - "overridable": true, - "startid": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 66, - "overridable": true, - "startid": 16 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 16, - "id": 65, - "overridable": true, - "startid": 17 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 19, - "id": 73, - "overridable": true, - "startid": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 94, - "overridable": true, - "startid": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 74, - "overridable": true, - "startid": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 18, - "id": 93, - "overridable": true, - "startid": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 41, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 21, - "id": 53, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 123, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 40, - "overridable": true, - "startid": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 95, - "overridable": true, - "startid": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 23, - "id": 97, - "overridable": true, - "startid": 22 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 98, - "overridable": true, - "startid": 23 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 25, - "id": 100, - "overridable": true, - "startid": 24 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 101, - "overridable": true, - "startid": 25 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 27, - "id": 103, - "overridable": true, - "startid": 26 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 104, - "overridable": true, - "startid": 27 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 29, - "id": 106, - "overridable": true, - "startid": 28 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 107, - "overridable": true, - "startid": 29 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 116, - "overridable": true, - "startid": 30 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 112, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 113, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 114, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 109, - "overridable": true, - "startid": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 110, - "overridable": true, - "startid": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 77, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 90, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 121, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 35, - "id": 60, - "overridable": true, - "startid": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 13, - "id": 70, - "overridable": true, - "startid": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 14, - "id": 61, - "overridable": true, - "startid": 35 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 37, - "id": 57, - "overridable": true, - "startid": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 35, - "id": 67, - "overridable": true, - "startid": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 58, - "overridable": true, - "startid": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 34, - "id": 69, - "overridable": true, - "startid": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 39, - "id": 55, - "overridable": true, - "startid": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 37, - "id": 68, - "overridable": true, - "startid": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 36, - "id": 56, - "overridable": true, - "startid": 39 - }, - "type": "Feature" - } - ], - "name": "graph", - "type": "FeatureCollection" -} diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson deleted file mode 100644 index 87a19b468ee..00000000000 --- a/nav2_route/graphs/sample_graph.geojson +++ /dev/null @@ -1,504 +0,0 @@ -{ - "crs": { - "properties": { - "name": "urn:ogc:def:crs:EPSG::3857" - }, - "type": "name" - }, - "features": [ - { - "geometry": { - "coordinates": [ - 0.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 0, - "metadata": { - "region": { - "properties": { - "class_type": "living_room", - "number_of_lights": 10 - }, - "x_values": [ - 1.0, - -1.0, - -1.0, - 1.0 - ], - "y_values": [ - 1.0, - 1.0, - -1.0, - -1.0 - ] - } - }, - "operations": { - "stop": { - "metadata": { - "wait_for": 5.0 - }, - "trigger": "NODE", - "type": "stop" - } - } - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 10.0, - "endid": 1, - "id": 9, - "metadata": { - "aisle_number": 14, - "speed_limit": 0.8500000238418579 - }, - "operations": { - "open_door": { - "metadata": { - "door_id": 54, - "service_name": "open_door" - }, - "trigger": "ON_ENTER", - "type": "open_door" - }, - "take_picture": { - "metadata": { - "resolution": [ - 1080, - 720 - ], - "type": "jpg" - }, - "trigger": "ON_EXIT", - "type": "take_picture" - } - }, - "overridable": false, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 13, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 10, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 11, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 15, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 12, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 17, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 14, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 19, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 23, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 16, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 20, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 21, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 25, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 18, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 22, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 27, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 24, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 29, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 26, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 30, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 31, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 28, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 32, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - } - ], - "name": "graph", - "type": "FeatureCollection" -} From e2176e14e8a0d1cbf470e7c3a63c580bc3922a3e Mon Sep 17 00:00:00 2001 From: Decwest Date: Sat, 11 Oct 2025 21:59:23 +0900 Subject: [PATCH 04/29] Revert ":recycle: delete unrelated files & test signed off" This reverts commit 53c5cd4f6c79ea0f37d7151af39ef1a67b181889. --- nav2_route/graphs/aws_graph.geojson | 1664 ++++++++++++++++++++++++ nav2_route/graphs/sample_graph.geojson | 504 +++++++ 2 files changed, 2168 insertions(+) create mode 100644 nav2_route/graphs/aws_graph.geojson create mode 100644 nav2_route/graphs/sample_graph.geojson diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson new file mode 100644 index 00000000000..4c4fb3208ff --- /dev/null +++ b/nav2_route/graphs/aws_graph.geojson @@ -0,0 +1,1664 @@ +{ + "crs": { + "properties": { + "name": "urn:ogc:def:crs:EPSG::3857" + }, + "type": "name" + }, + "features": [ + { + "geometry": { + "coordinates": [ + -4.0, + -9.350000381469727 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -3.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 0.6499999761581421 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 5.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 3.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 8.649999618530273 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -7.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -7.763441562652588 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -7.874690055847168 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 16 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -7.874690055847168 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 17 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + -8.690509796142578 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -9.395081520080566 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 3.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 22 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 4.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 23 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 2.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 24 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 25 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 26 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 27 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -6.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 28 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -9.350000381469727 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 29 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 30 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -5.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 35 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 39 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 75, + "overridable": true, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 19, + "id": 92, + "overridable": true, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 78, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 89, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 26, + "id": 102, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 119, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 120, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 79, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 88, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 24, + "id": 99, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 30, + "id": 117, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 118, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 81, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 86, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 80, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 87, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 122, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 82, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 21, + "id": 83, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 85, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 22, + "id": 96, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 84, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 76, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 91, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 28, + "id": 105, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 42, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 52, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 38, + "id": 54, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 10, + "id": 43, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 51, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 30, + "id": 115, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 11, + "id": 44, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 50, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 34, + "id": 59, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 111, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 45, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 10, + "id": 49, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 108, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 46, + "overridable": true, + "startid": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 11, + "id": 48, + "overridable": true, + "startid": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 63, + "overridable": true, + "startid": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 16, + "id": 71, + "overridable": true, + "startid": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 13, + "id": 62, + "overridable": true, + "startid": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 17, + "id": 64, + "overridable": true, + "startid": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 47, + "overridable": true, + "startid": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 18, + "id": 72, + "overridable": true, + "startid": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 66, + "overridable": true, + "startid": 16 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 16, + "id": 65, + "overridable": true, + "startid": 17 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 19, + "id": 73, + "overridable": true, + "startid": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 94, + "overridable": true, + "startid": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 74, + "overridable": true, + "startid": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 18, + "id": 93, + "overridable": true, + "startid": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 41, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 21, + "id": 53, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 123, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 40, + "overridable": true, + "startid": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 95, + "overridable": true, + "startid": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 23, + "id": 97, + "overridable": true, + "startid": 22 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 98, + "overridable": true, + "startid": 23 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 25, + "id": 100, + "overridable": true, + "startid": 24 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 101, + "overridable": true, + "startid": 25 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 27, + "id": 103, + "overridable": true, + "startid": 26 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 104, + "overridable": true, + "startid": 27 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 29, + "id": 106, + "overridable": true, + "startid": 28 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 107, + "overridable": true, + "startid": 29 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 116, + "overridable": true, + "startid": 30 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 112, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 113, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 114, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 109, + "overridable": true, + "startid": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 110, + "overridable": true, + "startid": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 77, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 90, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 121, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 35, + "id": 60, + "overridable": true, + "startid": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 13, + "id": 70, + "overridable": true, + "startid": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 14, + "id": 61, + "overridable": true, + "startid": 35 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 37, + "id": 57, + "overridable": true, + "startid": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 35, + "id": 67, + "overridable": true, + "startid": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 58, + "overridable": true, + "startid": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 34, + "id": 69, + "overridable": true, + "startid": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 39, + "id": 55, + "overridable": true, + "startid": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 37, + "id": 68, + "overridable": true, + "startid": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 36, + "id": 56, + "overridable": true, + "startid": 39 + }, + "type": "Feature" + } + ], + "name": "graph", + "type": "FeatureCollection" +} diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson new file mode 100644 index 00000000000..87a19b468ee --- /dev/null +++ b/nav2_route/graphs/sample_graph.geojson @@ -0,0 +1,504 @@ +{ + "crs": { + "properties": { + "name": "urn:ogc:def:crs:EPSG::3857" + }, + "type": "name" + }, + "features": [ + { + "geometry": { + "coordinates": [ + 0.0, + 0.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 0, + "metadata": { + "region": { + "properties": { + "class_type": "living_room", + "number_of_lights": 10 + }, + "x_values": [ + 1.0, + -1.0, + -1.0, + 1.0 + ], + "y_values": [ + 1.0, + 1.0, + -1.0, + -1.0 + ] + } + }, + "operations": { + "stop": { + "metadata": { + "wait_for": 5.0 + }, + "trigger": "NODE", + "type": "stop" + } + } + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 0.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 0.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 10.0, + "endid": 1, + "id": 9, + "metadata": { + "aisle_number": 14, + "speed_limit": 0.8500000238418579 + }, + "operations": { + "open_door": { + "metadata": { + "door_id": 54, + "service_name": "open_door" + }, + "trigger": "ON_ENTER", + "type": "open_door" + }, + "take_picture": { + "metadata": { + "resolution": [ + 1080, + 720 + ], + "type": "jpg" + }, + "trigger": "ON_EXIT", + "type": "take_picture" + } + }, + "overridable": false, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 13, + "overridable": true, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 10, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 11, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 15, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 12, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 17, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 14, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 19, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 23, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 16, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 20, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 21, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 25, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 18, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 22, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 27, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 24, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 29, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 26, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 30, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 31, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 28, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 32, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + } + ], + "name": "graph", + "type": "FeatureCollection" +} From df7b2af8174a51fee9a7794f15403bb858bb8e4a Mon Sep 17 00:00:00 2001 From: Decwest Date: Sat, 11 Oct 2025 22:00:28 +0900 Subject: [PATCH 05/29] Revert ":recycle: modify to pass lint" This reverts commit 547c54ef2d32fb2fecc79a6641274daacf299d7e. --- .../src/regulated_pure_pursuit_controller.cpp | 72 +- nav2_route/graphs/aws_graph.geojson | 1792 ++--------------- nav2_route/graphs/sample_graph.geojson | 581 +----- 3 files changed, 264 insertions(+), 2181 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index ed0d7d676ee..e4239c938ee 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -164,26 +164,26 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point) } void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( - const double curvature, - const geometry_msgs::msg::Twist current_speed, - const double regulated_linear_vel, - double & optimal_linear_vel, - double & optimal_angular_vel -) + const double curvature, + const geometry_msgs::msg::Twist current_speed, + const double regulated_linear_vel, + double & optimal_linear_vel, + double & optimal_angular_vel + ) { // ---- Parameters (assumed available in this scope) ---- - const double A_MAX = params_->max_linear_accel; // A_MAX - const double V_MAX = params_->desired_linear_vel; // V_MAX + const double A_MAX = params_->max_linear_accel; // A_MAX + const double V_MAX = params_->desired_linear_vel; // V_MAX const double AW_MAX = params_->max_angular_accel; // AW_MAX - const double W_MAX = params_->desired_angular_vel; // W_MAX - const double DT = control_duration_; // DT + const double W_MAX = params_->desired_angular_vel; // W_MAX + const double DT = control_duration_; // DT const double V_MIN = -V_MAX; const double W_MIN = -W_MAX; // ---- 1) Dynamic Window ---- - double dw_vmax = std::min(current_speed.linear.x + A_MAX * DT, V_MAX); - const double dw_vmin = std::max(current_speed.linear.x - A_MAX * DT, V_MIN); + double dw_vmax = std::min(current_speed.linear.x + A_MAX * DT, V_MAX); + const double dw_vmin = std::max(current_speed.linear.x - A_MAX * DT, V_MIN); double dw_wmax = std::min(current_speed.angular.z + AW_MAX * DT, W_MAX); const double dw_wmin = std::max(current_speed.angular.z - AW_MAX * DT, W_MIN); @@ -198,13 +198,13 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( if (k == 0.0) { // If w=0 is within DW, then the maximum linear speed is adopted as it is. if (dw_wmin <= 0.0 && 0.0 <= dw_wmax) { - optimal_linear_vel = dw_vmax; // Always maximum v + optimal_linear_vel = dw_vmax; // Always maximum v optimal_angular_vel = 0.0; return; } // If w=0 is outside, choose the side closer to w=0 and with smaller |w|. const double w_choice = (std::abs(dw_wmin) <= std::abs(dw_wmax)) ? dw_wmin : dw_wmax; - optimal_linear_vel = dw_vmax; // Always maximum v + optimal_linear_vel = dw_vmax; // Always maximum v optimal_angular_vel = w_choice; return; } @@ -218,14 +218,14 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double v1 = dw_vmin; const double w1 = k * v1; if (w1 >= dw_wmin && w1 <= dw_wmax) { - if (v1 > best_v) {best_v = v1; best_w = w1;} + if (v1 > best_v) { best_v = v1; best_w = w1; } } } { const double v2 = dw_vmax; const double w2 = k * v2; if (w2 >= dw_wmin && w2 <= dw_wmax) { - if (v2 > best_v) {best_v = v2; best_w = w2;} + if (v2 > best_v) { best_v = v2; best_w = w2; } } } @@ -234,26 +234,25 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double v3 = dw_wmin / k; if (v3 >= dw_vmin && v3 <= dw_vmax) { const double w3 = dw_wmin; - if (v3 > best_v) {best_v = v3; best_w = w3;} + if (v3 > best_v) { best_v = v3; best_w = w3; } } } { const double v4 = dw_wmax / k; if (v4 >= dw_vmin && v4 <= dw_vmax) { const double w4 = dw_wmax; - if (v4 > best_v) {best_v = v4; best_w = w4;} + if (v4 > best_v) { best_v = v4; best_w = w4; } } } if (best_v > -1e290) { // Intersection found → Adopt max. v - optimal_linear_vel = best_v; + optimal_linear_vel = best_v; optimal_angular_vel = best_w; return; } - // ---- 3) If no intersection exists: Select the one with the smallest - // Euclidean distance to the line w = k v among the 4 corners + // ---- 3) If no intersection exists: Select the one with the smallest Euclidean distance to the line w = k v among the 4 corners struct Corner { double v; double w; }; const Corner corners[4] = { {dw_vmin, dw_wmin}, @@ -262,12 +261,12 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( {dw_vmax, dw_wmax} }; - const double denom = std::sqrt(k * k + 1.0); // Just sqrt once + const double denom = std::sqrt(k * k + 1.0); // Just sqrt once - auto euclid_dist = [&](const Corner & c) -> double { + auto euclid_dist = [&](const Corner &c) -> double { // Distance from point (v, w) to line w - k v = 0 - return std::abs(k * c.v - c.w) / denom; - }; + return std::abs(k * c.v - c.w) / denom; + }; double best_dist = 1e300; best_v = corners[0].v; @@ -284,7 +283,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( } } - optimal_linear_vel = best_v; + optimal_linear_vel = best_v; optimal_angular_vel = best_w; } @@ -395,20 +394,19 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Apply curvature to angular velocity after constraining linear velocity - if (!params_->use_dynamic_window) { + if (params_->use_dynamic_window == false){ angular_vel = linear_vel * regulation_curvature; - } else { - // compute optimal path tracking velocity commands - // considering velocity and acceleration constraints + } + else{ + // compute optimal path tracking velocity commands considering velocity and acceleration constraints const double regulated_linear_vel = linear_vel; - if (params_->velocity_feedback == "CLOSED_LOOP") { + if (params_->velocity_feedback == "CLOSED_LOOP"){ // using odom velocity as a current velocity (not recommended) - computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, - regulated_linear_vel, linear_vel, angular_vel); - } else { + computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, regulated_linear_vel, linear_vel, angular_vel); + } + else{ // using last command velocity as a current velocity (recommended) - computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, - regulated_linear_vel, linear_vel, angular_vel); + computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, regulated_linear_vel, linear_vel, angular_vel); } } } @@ -431,7 +429,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity cmd_vel.header = pose.header; cmd_vel.twist.linear.x = linear_vel; cmd_vel.twist.angular.z = angular_vel; - + last_command_velocity_ = cmd_vel.twist; return cmd_vel; diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson index 4c4fb3208ff..2e6a43af148 100644 --- a/nav2_route/graphs/aws_graph.geojson +++ b/nav2_route/graphs/aws_graph.geojson @@ -1,1664 +1,132 @@ { - "crs": { - "properties": { - "name": "urn:ogc:def:crs:EPSG::3857" - }, - "type": "name" - }, - "features": [ - { - "geometry": { - "coordinates": [ - -4.0, - -9.350000381469727 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -3.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 0.6499999761581421 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 5.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 3.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 8.649999618530273 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -7.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -7.763441562652588 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -7.874690055847168 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 16 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -7.874690055847168 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 17 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - -8.690509796142578 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -9.395081520080566 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 3.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 22 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 4.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 23 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 2.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 24 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 25 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 26 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 27 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -6.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 28 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -9.350000381469727 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 29 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 30 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -5.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 35 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 39 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 75, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 19, - "id": 92, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 78, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 89, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 26, - "id": 102, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 119, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 120, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 79, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 88, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 24, - "id": 99, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 30, - "id": 117, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 118, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 81, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 86, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 80, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 87, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 122, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 82, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 21, - "id": 83, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 85, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 22, - "id": 96, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 84, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 76, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 91, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 28, - "id": 105, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 42, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 52, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 38, - "id": 54, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 10, - "id": 43, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 51, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 30, - "id": 115, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 11, - "id": 44, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 50, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 34, - "id": 59, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 111, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 45, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 10, - "id": 49, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 108, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 46, - "overridable": true, - "startid": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 11, - "id": 48, - "overridable": true, - "startid": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 63, - "overridable": true, - "startid": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 16, - "id": 71, - "overridable": true, - "startid": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 13, - "id": 62, - "overridable": true, - "startid": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 17, - "id": 64, - "overridable": true, - "startid": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 47, - "overridable": true, - "startid": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 18, - "id": 72, - "overridable": true, - "startid": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 66, - "overridable": true, - "startid": 16 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 16, - "id": 65, - "overridable": true, - "startid": 17 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 19, - "id": 73, - "overridable": true, - "startid": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 94, - "overridable": true, - "startid": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 74, - "overridable": true, - "startid": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 18, - "id": 93, - "overridable": true, - "startid": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 41, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 21, - "id": 53, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 123, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 40, - "overridable": true, - "startid": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 95, - "overridable": true, - "startid": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 23, - "id": 97, - "overridable": true, - "startid": 22 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 98, - "overridable": true, - "startid": 23 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 25, - "id": 100, - "overridable": true, - "startid": 24 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 101, - "overridable": true, - "startid": 25 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 27, - "id": 103, - "overridable": true, - "startid": 26 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 104, - "overridable": true, - "startid": 27 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 29, - "id": 106, - "overridable": true, - "startid": 28 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 107, - "overridable": true, - "startid": 29 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 116, - "overridable": true, - "startid": 30 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 112, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 113, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 114, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 109, - "overridable": true, - "startid": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 110, - "overridable": true, - "startid": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 77, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 90, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 121, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 35, - "id": 60, - "overridable": true, - "startid": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 13, - "id": 70, - "overridable": true, - "startid": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 14, - "id": 61, - "overridable": true, - "startid": 35 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 37, - "id": 57, - "overridable": true, - "startid": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 35, - "id": 67, - "overridable": true, - "startid": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 58, - "overridable": true, - "startid": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 34, - "id": 69, - "overridable": true, - "startid": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 39, - "id": 55, - "overridable": true, - "startid": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 37, - "id": 68, - "overridable": true, - "startid": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 36, - "id": 56, - "overridable": true, - "startid": 39 - }, - "type": "Feature" - } - ], - "name": "graph", - "type": "FeatureCollection" +"type": "FeatureCollection", +"name": "graph", +"crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, +"date_generated": "Wed Feb 22 05:41:45 PM EST 2025", +"features": [ +{ "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, +{ "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, +{ "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, +{ "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, +{ "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, +{ "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, +{ "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, +{ "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, +{ "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, +{ "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, +{ "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, +{ "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, +{ "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } +] } diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson index 87a19b468ee..162a1a88884 100644 --- a/nav2_route/graphs/sample_graph.geojson +++ b/nav2_route/graphs/sample_graph.geojson @@ -1,504 +1,121 @@ { - "crs": { - "properties": { - "name": "urn:ogc:def:crs:EPSG::3857" - }, - "type": "name" - }, + "type": "FeatureCollection", + "name": "graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Tue Feb 28 07:48:03 PM EST 2025", "features": [ { - "geometry": { - "coordinates": [ - 0.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", + "type": "Feature", + "properties": + { "id": 0, - "metadata": { - "region": { + "frame": "map", + "metadata": + { + "region": + { + "x_values": [1.0, -1.0, -1.0, 1.0], + "y_values": [1.0, 1.0, -1.0, -1.0], "properties": { "class_type": "living_room", "number_of_lights": 10 - }, - "x_values": [ - 1.0, - -1.0, - -1.0, - 1.0 - ], - "y_values": [ - 1.0, - 1.0, - -1.0, - -1.0 - ] + } } }, - "operations": { - "stop": { - "metadata": { - "wait_for": 5.0 - }, + "operations": + { + "stop": + { + "type": "stop", "trigger": "NODE", - "type": "stop" + "metadata": + { + "wait_for": 5.0 + } } } }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 10.0, - "endid": 1, + "geometry": + { + "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 2.0 ] } }, + { + "type": "Feature", + "properties": + { "id": 9, - "metadata": { - "aisle_number": 14, - "speed_limit": 0.8500000238418579 + "startid": 0, + "endid": 1, + "overridable": false, + "cost": 10.0, + "metadata": + { + "speed_limit": 0.85, + "aisle_number": 14 }, - "operations": { - "open_door": { - "metadata": { + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_ENTER", + "metadata": + { "door_id": 54, "service_name": "open_door" - }, - "trigger": "ON_ENTER", - "type": "open_door" + } }, - "take_picture": { - "metadata": { - "resolution": [ - 1080, - 720 - ], - "type": "jpg" - }, + "take_picture": + { + "type": "take_picture", "trigger": "ON_EXIT", - "type": "take_picture" + "metadata": + { + "type": "jpg", + "resolution": [1080, 720] + } } - }, - "overridable": false, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 13, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 10, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 11, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 15, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 12, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 17, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 14, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 19, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 23, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 16, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 20, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 21, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 25, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 18, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 22, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 27, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 24, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 29, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 26, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 30, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 31, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 28, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 32, - "overridable": true, - "startid": 8 + } }, - "type": "Feature" - } - ], - "name": "graph", - "type": "FeatureCollection" + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + }, + { "type": "Feature", "properties": { "id": 10, "startid": 1, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 11, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 12, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 13, "startid": 0, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 0.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 14, "startid": 3, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 16, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 17, "startid": 2, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 18, "startid": 5, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 19, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 20, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 22, "startid": 5, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 23, "startid": 3, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 24, "startid": 6, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 25, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 26, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 27, "startid": 5, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 28, "startid": 8, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 29, "startid": 6, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 30, "startid": 7, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 31, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 32, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 1.0, 2.0 ] ] ] } } + + ] } From 318e9175591a46d6a9de3fad6d90fab70efb784c Mon Sep 17 00:00:00 2001 From: Decwest Date: Sat, 11 Oct 2025 22:04:28 +0900 Subject: [PATCH 06/29] :recycle: reformat & revert unintentional file change Signed-off-by: Decwest --- .../src/regulated_pure_pursuit_controller.cpp | 72 ++++++++++--------- 1 file changed, 37 insertions(+), 35 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index e4239c938ee..85084c3c01f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -164,26 +164,26 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point) } void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( - const double curvature, - const geometry_msgs::msg::Twist current_speed, - const double regulated_linear_vel, - double & optimal_linear_vel, - double & optimal_angular_vel - ) + const double curvature, + const geometry_msgs::msg::Twist current_speed, + const double regulated_linear_vel, + double & optimal_linear_vel, + double & optimal_angular_vel +) { // ---- Parameters (assumed available in this scope) ---- - const double A_MAX = params_->max_linear_accel; // A_MAX - const double V_MAX = params_->desired_linear_vel; // V_MAX + const double A_MAX = params_->max_linear_accel; // A_MAX + const double V_MAX = params_->desired_linear_vel; // V_MAX const double AW_MAX = params_->max_angular_accel; // AW_MAX - const double W_MAX = params_->desired_angular_vel; // W_MAX - const double DT = control_duration_; // DT + const double W_MAX = params_->desired_angular_vel; // W_MAX + const double DT = control_duration_; // DT const double V_MIN = -V_MAX; const double W_MIN = -W_MAX; // ---- 1) Dynamic Window ---- - double dw_vmax = std::min(current_speed.linear.x + A_MAX * DT, V_MAX); - const double dw_vmin = std::max(current_speed.linear.x - A_MAX * DT, V_MIN); + double dw_vmax = std::min(current_speed.linear.x + A_MAX * DT, V_MAX); + const double dw_vmin = std::max(current_speed.linear.x - A_MAX * DT, V_MIN); double dw_wmax = std::min(current_speed.angular.z + AW_MAX * DT, W_MAX); const double dw_wmin = std::max(current_speed.angular.z - AW_MAX * DT, W_MIN); @@ -198,13 +198,13 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( if (k == 0.0) { // If w=0 is within DW, then the maximum linear speed is adopted as it is. if (dw_wmin <= 0.0 && 0.0 <= dw_wmax) { - optimal_linear_vel = dw_vmax; // Always maximum v + optimal_linear_vel = dw_vmax; // Always maximum v optimal_angular_vel = 0.0; return; } // If w=0 is outside, choose the side closer to w=0 and with smaller |w|. const double w_choice = (std::abs(dw_wmin) <= std::abs(dw_wmax)) ? dw_wmin : dw_wmax; - optimal_linear_vel = dw_vmax; // Always maximum v + optimal_linear_vel = dw_vmax; // Always maximum v optimal_angular_vel = w_choice; return; } @@ -218,14 +218,14 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double v1 = dw_vmin; const double w1 = k * v1; if (w1 >= dw_wmin && w1 <= dw_wmax) { - if (v1 > best_v) { best_v = v1; best_w = w1; } + if (v1 > best_v) {best_v = v1; best_w = w1;} } } { const double v2 = dw_vmax; const double w2 = k * v2; if (w2 >= dw_wmin && w2 <= dw_wmax) { - if (v2 > best_v) { best_v = v2; best_w = w2; } + if (v2 > best_v) {best_v = v2; best_w = w2;} } } @@ -234,25 +234,26 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double v3 = dw_wmin / k; if (v3 >= dw_vmin && v3 <= dw_vmax) { const double w3 = dw_wmin; - if (v3 > best_v) { best_v = v3; best_w = w3; } + if (v3 > best_v) {best_v = v3; best_w = w3;} } } { const double v4 = dw_wmax / k; if (v4 >= dw_vmin && v4 <= dw_vmax) { const double w4 = dw_wmax; - if (v4 > best_v) { best_v = v4; best_w = w4; } + if (v4 > best_v) {best_v = v4; best_w = w4;} } } if (best_v > -1e290) { // Intersection found → Adopt max. v - optimal_linear_vel = best_v; + optimal_linear_vel = best_v; optimal_angular_vel = best_w; return; } - // ---- 3) If no intersection exists: Select the one with the smallest Euclidean distance to the line w = k v among the 4 corners + // ---- 3) If no intersection exists: Select the one with the smallest + // Euclidean distance to the line w = k v among the 4 corners struct Corner { double v; double w; }; const Corner corners[4] = { {dw_vmin, dw_wmin}, @@ -261,12 +262,12 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( {dw_vmax, dw_wmax} }; - const double denom = std::sqrt(k * k + 1.0); // Just sqrt once + const double denom = std::sqrt(k * k + 1.0); // Just sqrt once - auto euclid_dist = [&](const Corner &c) -> double { + auto euclid_dist = [&](const Corner & c) -> double { // Distance from point (v, w) to line w - k v = 0 - return std::abs(k * c.v - c.w) / denom; - }; + return std::abs(k * c.v - c.w) / denom; + }; double best_dist = 1e300; best_v = corners[0].v; @@ -283,7 +284,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( } } - optimal_linear_vel = best_v; + optimal_linear_vel = best_v; optimal_angular_vel = best_w; } @@ -394,19 +395,20 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Apply curvature to angular velocity after constraining linear velocity - if (params_->use_dynamic_window == false){ + if (params_->use_dynamic_window == false) { angular_vel = linear_vel * regulation_curvature; - } - else{ - // compute optimal path tracking velocity commands considering velocity and acceleration constraints + } else { + // compute optimal path tracking velocity commands + // considering velocity and acceleration constraints const double regulated_linear_vel = linear_vel; - if (params_->velocity_feedback == "CLOSED_LOOP"){ + if (params_->velocity_feedback == "CLOSED_LOOP") { // using odom velocity as a current velocity (not recommended) - computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, regulated_linear_vel, linear_vel, angular_vel); - } - else{ + computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, regulated_linear_vel, + linear_vel, angular_vel); + } else { // using last command velocity as a current velocity (recommended) - computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, regulated_linear_vel, linear_vel, angular_vel); + computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, + regulated_linear_vel, linear_vel, angular_vel); } } } @@ -429,7 +431,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity cmd_vel.header = pose.header; cmd_vel.twist.linear.x = linear_vel; cmd_vel.twist.angular.z = angular_vel; - + last_command_velocity_ = cmd_vel.twist; return cmd_vel; From 68b6dd40431ae2e11cd071be1864cc04e243df81 Mon Sep 17 00:00:00 2001 From: Decwest Date: Wed, 22 Oct 2025 20:13:07 +0900 Subject: [PATCH 07/29] :recycle: change variable names for consistency Signed-off-by: Decwest --- .../src/regulated_pure_pursuit_controller.cpp | 63 +++++++++---------- 1 file changed, 31 insertions(+), 32 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 85084c3c01f..04b52d0811b 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -171,25 +171,24 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( double & optimal_angular_vel ) { - // ---- Parameters (assumed available in this scope) ---- - const double A_MAX = params_->max_linear_accel; // A_MAX - const double V_MAX = params_->desired_linear_vel; // V_MAX - const double AW_MAX = params_->max_angular_accel; // AW_MAX - const double W_MAX = params_->desired_angular_vel; // W_MAX - const double DT = control_duration_; // DT + const double & max_linear_accel = params_->max_linear_accel; + const double & desired_linear_vel = params_->desired_linear_vel; + const double & max_angular_accel = params_->max_angular_accel; + const double & desired_angular_vel = params_->desired_angular_vel; + const double & dt = control_duration_; - const double V_MIN = -V_MAX; - const double W_MIN = -W_MAX; + const double min_linear_vel = -desired_linear_vel; + const double min_angular_vel = -desired_angular_vel; // ---- 1) Dynamic Window ---- - double dw_vmax = std::min(current_speed.linear.x + A_MAX * DT, V_MAX); - const double dw_vmin = std::max(current_speed.linear.x - A_MAX * DT, V_MIN); - double dw_wmax = std::min(current_speed.angular.z + AW_MAX * DT, W_MAX); - const double dw_wmin = std::max(current_speed.angular.z - AW_MAX * DT, W_MIN); + double dynamic_window_max_linear_vel = std::min(current_speed.linear.x + max_linear_accel * dt, desired_linear_vel); + const double dynamic_window_min_linear_vel = std::max(current_speed.linear.x - max_linear_accel * dt, min_linear_vel); + double dynamic_window_max_angular_vel = std::min(current_speed.angular.z + max_angular_accel * dt, desired_angular_vel); + const double dynamic_window_min_angular_vel = std::max(current_speed.angular.z - max_angular_accel * dt, min_angular_vel); // Reflect regulated v (tighten upper limit) - if (dw_vmax > regulated_linear_vel) { - dw_vmax = std::max(dw_vmin, regulated_linear_vel); + if (dynamic_window_max_linear_vel > regulated_linear_vel) { + dynamic_window_max_linear_vel = std::max(dynamic_window_min_linear_vel, regulated_linear_vel); } const double k = curvature; @@ -197,14 +196,14 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( // ---- Curvature is 0 (w = 0) ---- if (k == 0.0) { // If w=0 is within DW, then the maximum linear speed is adopted as it is. - if (dw_wmin <= 0.0 && 0.0 <= dw_wmax) { - optimal_linear_vel = dw_vmax; // Always maximum v + if (dynamic_window_min_angular_vel <= 0.0 && 0.0 <= dynamic_window_max_angular_vel) { + optimal_linear_vel = dynamic_window_max_linear_vel; // Always maximum v optimal_angular_vel = 0.0; return; } // If w=0 is outside, choose the side closer to w=0 and with smaller |w|. - const double w_choice = (std::abs(dw_wmin) <= std::abs(dw_wmax)) ? dw_wmin : dw_wmax; - optimal_linear_vel = dw_vmax; // Always maximum v + const double w_choice = (std::abs(dynamic_window_min_angular_vel) <= std::abs(dynamic_window_max_angular_vel)) ? dynamic_window_min_angular_vel : dynamic_window_max_angular_vel; + optimal_linear_vel = dynamic_window_max_linear_vel; // Always maximum v optimal_angular_vel = w_choice; return; } @@ -215,32 +214,32 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( // Intersection with vertical edges { - const double v1 = dw_vmin; + const double v1 = dynamic_window_min_linear_vel; const double w1 = k * v1; - if (w1 >= dw_wmin && w1 <= dw_wmax) { + if (w1 >= dynamic_window_min_angular_vel && w1 <= dynamic_window_max_angular_vel) { if (v1 > best_v) {best_v = v1; best_w = w1;} } } { - const double v2 = dw_vmax; + const double v2 = dynamic_window_max_linear_vel; const double w2 = k * v2; - if (w2 >= dw_wmin && w2 <= dw_wmax) { + if (w2 >= dynamic_window_min_angular_vel && w2 <= dynamic_window_max_angular_vel) { if (v2 > best_v) {best_v = v2; best_w = w2;} } } // Intersection with horizontal edge (k ! = 0) { - const double v3 = dw_wmin / k; - if (v3 >= dw_vmin && v3 <= dw_vmax) { - const double w3 = dw_wmin; + const double v3 = dynamic_window_min_angular_vel / k; + if (v3 >= dynamic_window_min_linear_vel && v3 <= dynamic_window_max_linear_vel) { + const double w3 = dynamic_window_min_angular_vel; if (v3 > best_v) {best_v = v3; best_w = w3;} } } { - const double v4 = dw_wmax / k; - if (v4 >= dw_vmin && v4 <= dw_vmax) { - const double w4 = dw_wmax; + const double v4 = dynamic_window_max_angular_vel / k; + if (v4 >= dynamic_window_min_linear_vel && v4 <= dynamic_window_max_linear_vel) { + const double w4 = dynamic_window_max_angular_vel; if (v4 > best_v) {best_v = v4; best_w = w4;} } } @@ -256,10 +255,10 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( // Euclidean distance to the line w = k v among the 4 corners struct Corner { double v; double w; }; const Corner corners[4] = { - {dw_vmin, dw_wmin}, - {dw_vmin, dw_wmax}, - {dw_vmax, dw_wmin}, - {dw_vmax, dw_wmax} + {dynamic_window_min_linear_vel, dynamic_window_min_angular_vel}, + {dynamic_window_min_linear_vel, dynamic_window_max_angular_vel}, + {dynamic_window_max_linear_vel, dynamic_window_min_angular_vel}, + {dynamic_window_max_linear_vel, dynamic_window_max_angular_vel} }; const double denom = std::sqrt(k * k + 1.0); // Just sqrt once From 3d123ac6ed3065eae8f0d3dfe26148bf94352796 Mon Sep 17 00:00:00 2001 From: Decwest Date: Wed, 22 Oct 2025 20:21:45 +0900 Subject: [PATCH 08/29] :refactor: modify value handling Signed-off-by: Decwest --- .../src/regulated_pure_pursuit_controller.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 04b52d0811b..d1be92ee808 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -194,7 +194,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double k = curvature; // ---- Curvature is 0 (w = 0) ---- - if (k == 0.0) { + if (abs(k) < 1e-3) { // If w=0 is within DW, then the maximum linear speed is adopted as it is. if (dynamic_window_min_angular_vel <= 0.0 && 0.0 <= dynamic_window_max_angular_vel) { optimal_linear_vel = dynamic_window_max_linear_vel; // Always maximum v @@ -209,7 +209,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( } // ---- 2) Select 'max v' from the candidates in the DW among the intersections. ---- - double best_v = -1e300; // Initial value for maximization + double best_v = -std::numeric_limits::infinity(); // Initial value for maximization double best_w = 0.0; // Intersection with vertical edges @@ -244,7 +244,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( } } - if (best_v > -1e290) { + if (best_v > -std::numeric_limits::infinity()) { // Intersection found → Adopt max. v optimal_linear_vel = best_v; optimal_angular_vel = best_w; @@ -272,7 +272,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( best_v = corners[0].v; best_w = corners[0].w; - for (int i = 0; i < 4; ++i) { + for (int i = 0; i < corners.size(); ++i) { const double d = euclid_dist(corners[i]); // 1) Smaller distance → Adopted // 2) If distances are equal (~1e-12) → Choose the one with larger v (acceleration policy) @@ -394,7 +394,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Apply curvature to angular velocity after constraining linear velocity - if (params_->use_dynamic_window == false) { + if (!params_->use_dynamic_window) { angular_vel = linear_vel * regulation_curvature; } else { // compute optimal path tracking velocity commands From 7d2485fa450de571755660e2bcb4a78934680ea2 Mon Sep 17 00:00:00 2001 From: Decwest Date: Wed, 22 Oct 2025 21:31:07 +0900 Subject: [PATCH 09/29] :bug: fix type issue Signed-off-by: Decwest --- .../src/regulated_pure_pursuit_controller.cpp | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index d1be92ee808..c890a5c9e1f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -253,33 +253,32 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( // ---- 3) If no intersection exists: Select the one with the smallest // Euclidean distance to the line w = k v among the 4 corners - struct Corner { double v; double w; }; - const Corner corners[4] = { + const std::array, 4> corners = {{ {dynamic_window_min_linear_vel, dynamic_window_min_angular_vel}, {dynamic_window_min_linear_vel, dynamic_window_max_angular_vel}, {dynamic_window_max_linear_vel, dynamic_window_min_angular_vel}, {dynamic_window_max_linear_vel, dynamic_window_max_angular_vel} - }; + }}; const double denom = std::sqrt(k * k + 1.0); // Just sqrt once - auto euclid_dist = [&](const Corner & c) -> double { + auto euclid_dist = [&](const std::array & corner) -> double { // Distance from point (v, w) to line w - k v = 0 - return std::abs(k * c.v - c.w) / denom; + return std::abs(k * corner[0] - corner[1]) / denom; }; - double best_dist = 1e300; - best_v = corners[0].v; - best_w = corners[0].w; + double best_dist = std::numeric_limits::infinity(); + best_v = corners[0][0]; + best_w = corners[0][1]; - for (int i = 0; i < corners.size(); ++i) { - const double d = euclid_dist(corners[i]); + for (const auto &corner : corners) { + const double d = euclid_dist(corner); // 1) Smaller distance → Adopted // 2) If distances are equal (~1e-12) → Choose the one with larger v (acceleration policy) - if (d < best_dist || (std::abs(d - best_dist) <= 1e-12 && corners[i].v > best_v)) { + if (d < best_dist || (std::abs(d - best_dist) <= 1e-3 && corner[0] > best_v)) { best_dist = d; - best_v = corners[i].v; - best_w = corners[i].w; + best_v = corner[0]; + best_w = corner[1]; } } From e72591757cbceed274563a8c11df35cdedfdbc1c Mon Sep 17 00:00:00 2001 From: Decwest Date: Wed, 22 Oct 2025 21:34:06 +0900 Subject: [PATCH 10/29] :recycle: reformat Signed-off-by: Decwest --- .../src/regulated_pure_pursuit_controller.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index c890a5c9e1f..4e70d68e707 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -181,10 +181,14 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double min_angular_vel = -desired_angular_vel; // ---- 1) Dynamic Window ---- - double dynamic_window_max_linear_vel = std::min(current_speed.linear.x + max_linear_accel * dt, desired_linear_vel); - const double dynamic_window_min_linear_vel = std::max(current_speed.linear.x - max_linear_accel * dt, min_linear_vel); - double dynamic_window_max_angular_vel = std::min(current_speed.angular.z + max_angular_accel * dt, desired_angular_vel); - const double dynamic_window_min_angular_vel = std::max(current_speed.angular.z - max_angular_accel * dt, min_angular_vel); + double dynamic_window_max_linear_vel = std::min(current_speed.linear.x + max_linear_accel * dt, + desired_linear_vel); + const double dynamic_window_min_linear_vel = std::max(current_speed.linear.x - max_linear_accel * + dt, min_linear_vel); + double dynamic_window_max_angular_vel = std::min(current_speed.angular.z + max_angular_accel * dt, + desired_angular_vel); + const double dynamic_window_min_angular_vel = std::max(current_speed.angular.z - + max_angular_accel * dt, min_angular_vel); // Reflect regulated v (tighten upper limit) if (dynamic_window_max_linear_vel > regulated_linear_vel) { @@ -202,7 +206,9 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( return; } // If w=0 is outside, choose the side closer to w=0 and with smaller |w|. - const double w_choice = (std::abs(dynamic_window_min_angular_vel) <= std::abs(dynamic_window_max_angular_vel)) ? dynamic_window_min_angular_vel : dynamic_window_max_angular_vel; + const double w_choice = (std::abs(dynamic_window_min_angular_vel) <= + std::abs(dynamic_window_max_angular_vel)) ? dynamic_window_min_angular_vel : + dynamic_window_max_angular_vel; optimal_linear_vel = dynamic_window_max_linear_vel; // Always maximum v optimal_angular_vel = w_choice; return; @@ -271,7 +277,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( best_v = corners[0][0]; best_w = corners[0][1]; - for (const auto &corner : corners) { + for (const auto & corner : corners) { const double d = euclid_dist(corner); // 1) Smaller distance → Adopted // 2) If distances are equal (~1e-12) → Choose the one with larger v (acceleration policy) From ff2b3cca054def2c67396be01ea36a25e83e832a Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 23 Oct 2025 02:40:37 +0900 Subject: [PATCH 11/29] :zap: add minimum velocity configuration Signed-off-by: Decwest --- .../README.md | 19 +++++++++++---- .../parameter_handler.hpp | 4 ++-- .../src/parameter_handler.cpp | 22 +++++++++++------- .../src/regulated_pure_pursuit_controller.cpp | 23 ++++++++++--------- .../test/test_regulated_pp.cpp | 6 ++--- 5 files changed, 46 insertions(+), 28 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 2caec97ff32..0f226cab768 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -65,7 +65,12 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | Parameter | Description | |-----|----| -| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `max_linear_vel` | The maximum linear velocity to use. | +| `min_linear_vel` | The minimum linear velocity to use. | +| `max_angular_vel` | The maximum angular velocity to use. | +| `min_angular_vel` | The minimum angular velocity to use. | +| `max_linear_accel` | The maximum linear acceleration to use. | +| `max_angular_accel` | The maximum angular acceleration to use. | | `lookahead_dist` | The lookahead distance to use to find the lookahead point | | `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | | `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | @@ -88,10 +93,11 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `curvature_lookahead_dist` | Distance to lookahead to determine curvature for velocity regulation purposes. Only used if `use_fixed_curvature_lookahead` is enabled. | | `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | -| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | | `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | | `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. | +| `use_dynamic_window ` | Whether to use the Dynamic Window Pure Pursuit (DWPP) Algorithm. This algorithm computes optimal path tracking velocity commands under velocity and acceleration constraints. | +| `velocity_feedback ` | How the current velocity is obtained during dynamic window computation. "OPEN_LOOP" uses the last commanded velocity (recommended). "CLOSED_LOOP" uses odometry velocity (may hinder proper acceleration/deceleration) | Example fully-described XML with default parameter values: @@ -117,7 +123,12 @@ controller_server: stateful: True FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 0.5 + max_linear_vel: 0.5 + min_linear_vel: 0.0 + max_angular_vel: 1.0 + min_angular_vel: -1.0 + max_linear_accel: 0.5 + max_angular_accel: 1.0 lookahead_dist: 0.6 min_lookahead_dist: 0.3 max_lookahead_dist: 0.9 @@ -137,12 +148,12 @@ controller_server: curvature_lookahead_dist: 1.0 use_rotate_to_heading: true rotate_to_heading_min_angle: 0.785 - max_angular_accel: 3.2 max_robot_pose_search_dist: 10.0 interpolate_curvature_after_goal: false cost_scaling_dist: 0.3 cost_scaling_gain: 1.0 inflation_cost_scaling_factor: 3.0 + use_dynamic_window: true ``` ## Topics diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index a9d45d4223f..cf101cfac81 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -33,8 +33,8 @@ namespace nav2_regulated_pure_pursuit_controller struct Parameters { - double desired_linear_vel, base_desired_linear_vel; - double desired_angular_vel; + double max_linear_vel, base_max_linear_vel, min_linear_vel; + double max_angular_vel, min_angular_vel; double lookahead_dist; double rotate_to_heading_angular_vel; double max_lookahead_dist; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index a3c015bca0f..96383d591db 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -38,9 +38,13 @@ ParameterHandler::ParameterHandler( logger_ = logger; declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + node, plugin_name_ + ".max_linear_vel", rclcpp::ParameterValue(0.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_angular_vel", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".min_linear_vel", rclcpp::ParameterValue(0.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_angular_vel", rclcpp::ParameterValue(-1.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( @@ -116,9 +120,11 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".velocity_feedback", rclcpp::ParameterValue(std::string("OPEN_LOOP"))); - node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); - params_.base_desired_linear_vel = params_.desired_linear_vel; - node->get_parameter(plugin_name_ + ".desired_angular_vel", params_.desired_angular_vel); + node->get_parameter(plugin_name_ + ".max_linear_vel", params_.max_linear_vel); + params_.base_max_linear_vel = params_.max_linear_vel; + node->get_parameter(plugin_name_ + ".min_linear_vel", params_.min_linear_vel); + node->get_parameter(plugin_name_ + ".max_angular_vel", params_.max_angular_vel); + node->get_parameter(plugin_name_ + ".min_angular_vel", params_.min_angular_vel); node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist); node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist); node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist); @@ -285,9 +291,9 @@ ParameterHandler::updateParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") { params_.inflation_cost_scaling_factor = parameter.as_double(); - } else if (param_name == plugin_name_ + ".desired_linear_vel") { - params_.desired_linear_vel = parameter.as_double(); - params_.base_desired_linear_vel = parameter.as_double(); + } else if (param_name == plugin_name_ + ".max_linear_vel") { + params_.max_linear_vel = parameter.as_double(); + params_.base_max_linear_vel = parameter.as_double(); } else if (param_name == plugin_name_ + ".lookahead_dist") { params_.lookahead_dist = parameter.as_double(); } else if (param_name == plugin_name_ + ".max_lookahead_dist") { diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 4e70d68e707..217e7482d36 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -171,22 +171,23 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( double & optimal_angular_vel ) { + const double & max_linear_vel = params_->max_linear_vel; + const double & min_linear_vel = params_->min_linear_vel; + const double & max_angular_vel = params_->max_angular_vel; + const double & min_angular_vel = params_->min_angular_vel; + const double & max_linear_accel = params_->max_linear_accel; - const double & desired_linear_vel = params_->desired_linear_vel; const double & max_angular_accel = params_->max_angular_accel; - const double & desired_angular_vel = params_->desired_angular_vel; const double & dt = control_duration_; - const double min_linear_vel = -desired_linear_vel; - const double min_angular_vel = -desired_angular_vel; // ---- 1) Dynamic Window ---- double dynamic_window_max_linear_vel = std::min(current_speed.linear.x + max_linear_accel * dt, - desired_linear_vel); + max_linear_vel); const double dynamic_window_min_linear_vel = std::max(current_speed.linear.x - max_linear_accel * dt, min_linear_vel); double dynamic_window_max_angular_vel = std::min(current_speed.angular.z + max_angular_accel * dt, - desired_angular_vel); + max_angular_vel); const double dynamic_window_min_angular_vel = std::max(current_speed.angular.z - max_angular_accel * dt, min_angular_vel); @@ -359,7 +360,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity x_vel_sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; } - linear_vel = params_->desired_linear_vel; + linear_vel = params_->max_linear_vel; // Make sure we're in compliance with basic constraints // For shouldRotateToPath, using x_vel_sign in order to support allow_reversing @@ -534,7 +535,7 @@ void RegulatedPurePursuitController::applyConstraints( params_->approach_velocity_scaling_dist); // Limit linear velocities to be valid - linear_vel = std::clamp(fabs(linear_vel), 0.0, params_->desired_linear_vel); + linear_vel = std::clamp(fabs(linear_vel), 0.0, params_->max_linear_vel); linear_vel = sign * linear_vel; } @@ -552,14 +553,14 @@ void RegulatedPurePursuitController::setSpeedLimit( if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { // Restore default value - params_->desired_linear_vel = params_->base_desired_linear_vel; + params_->max_linear_vel = params_->base_max_linear_vel; } else { if (percentage) { // Speed limit is expressed in % from maximum speed of robot - params_->desired_linear_vel = params_->base_desired_linear_vel * speed_limit / 100.0; + params_->max_linear_vel = params_->base_max_linear_vel * speed_limit / 100.0; } else { // Speed limit is expressed in absolute value - params_->desired_linear_vel = speed_limit; + params_->max_linear_vel = speed_limit; } } } diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 4b3dbb905f8..4d593b35186 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -35,7 +35,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} - double getSpeed() {return params_->desired_linear_vel;} + double getSpeed() {return params_->max_linear_vel;} std::unique_ptr createCarrotMsgWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose) @@ -439,7 +439,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) node->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("test.desired_linear_vel", 1.0), + {rclcpp::Parameter("test.max_linear_vel", 1.0), rclcpp::Parameter("test.lookahead_dist", 7.0), rclcpp::Parameter("test.max_lookahead_dist", 7.0), rclcpp::Parameter("test.min_lookahead_dist", 6.0), @@ -467,7 +467,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) node->get_node_base_interface(), results); - EXPECT_EQ(node->get_parameter("test.desired_linear_vel").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("test.max_linear_vel").as_double(), 1.0); EXPECT_EQ(node->get_parameter("test.lookahead_dist").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.max_lookahead_dist").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.min_lookahead_dist").as_double(), 6.0); From 3a57631ec073734e5ab95da21e3385dc9b9c99a3 Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 23 Oct 2025 19:28:53 +0900 Subject: [PATCH 12/29] :zap: handle different accel and decel considering bidirectionality Signed-off-by: Decwest --- .../README.md | 4 + .../parameter_handler.hpp | 1 + .../regulated_pure_pursuit_controller.hpp | 40 ++++++- .../src/parameter_handler.cpp | 6 ++ .../src/regulated_pure_pursuit_controller.cpp | 102 +++++++++++++++--- 5 files changed, 137 insertions(+), 16 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 0f226cab768..905645ff89c 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -70,7 +70,9 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `max_angular_vel` | The maximum angular velocity to use. | | `min_angular_vel` | The minimum angular velocity to use. | | `max_linear_accel` | The maximum linear acceleration to use. | +| `max_linear_decel` | The maximum linear deceleration to use. | | `max_angular_accel` | The maximum angular acceleration to use. | +| `max_angular_decel` | The maximum angular deceleration to use. | | `lookahead_dist` | The lookahead distance to use to find the lookahead point | | `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | | `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | @@ -128,7 +130,9 @@ controller_server: max_angular_vel: 1.0 min_angular_vel: -1.0 max_linear_accel: 0.5 + max_linear_decel: 0.5 max_angular_accel: 1.0 + max_angular_decel: 1.0 lookahead_dist: 0.6 min_lookahead_dist: 0.3 max_lookahead_dist: 0.9 diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index cf101cfac81..de6dc35b179 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -56,6 +56,7 @@ struct Parameters double curvature_lookahead_dist; bool use_rotate_to_heading; double max_linear_accel, max_angular_accel; + double max_linear_decel, max_angular_decel; bool use_cancel_deceleration; double cancel_deceleration; double rotate_to_heading_min_angle; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7c4d83c439e..ee31be9847a 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -81,6 +81,40 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ void deactivate() override; + /** + * @brief Compute the dynamic window (feasible velocity bounds) based on the current speed and the given velocity and acceleration constraints. + * @param current_speed Current linear and angular velocity of the robot + * @param max_linear_vel Maximum linear velocity + * @param min_linear_vel Minimum linear velocity + * @param max_angular_vel Maximum angular velocity + * @param min_angular_vel Minimum angular velocity + * @param max_linear_accel Maximum linear acceleration + * @param max_linear_decel Maximum linear deceleration + * @param max_angular_accel Maximum angular acceleration + * @param max_angular_decel Maximum angular deceleration + * @param dt Control loop sampling period + * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window + * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window + * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window + * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window + */ + void computeDynamicWindow( + const geometry_msgs::msg::Twist & current_speed, + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel, + const double & dt, + double & dynamic_window_max_linear_vel, + double & dynamic_window_min_linear_vel, + double & dynamic_window_max_angular_vel, + double & dynamic_window_min_angular_vel + ); + /** * @brief Compute the optimal command given the current pose, velocity and acceleration constraints using dynamic window * @param curvature Curvature of the path to follow @@ -90,9 +124,9 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param optimal_angular_vel Optimal angular velocity to follow the path under velocity and acceleration constraints */ void computeOptimalVelocityUsingDynamicWindow( - const double curvature, - const geometry_msgs::msg::Twist current_speed, - const double regulated_linear_vel, + const double & curvature, + const geometry_msgs::msg::Twist & current_speed, + const double & regulated_linear_vel, double & optimal_linear_vel, double & optimal_angular_vel ); diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 96383d591db..29e3931088f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -96,8 +96,12 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(0.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( @@ -180,7 +184,9 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); node->get_parameter(plugin_name_ + ".max_linear_accel", params_.max_linear_accel); + node->get_parameter(plugin_name_ + ".max_linear_decel", params_.max_linear_decel); node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); + node->get_parameter(plugin_name_ + ".max_angular_decel", params_.max_angular_decel); node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 217e7482d36..c539fe62fe4 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -163,10 +163,72 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point) } } +void RegulatedPurePursuitController::computeDynamicWindow( + const geometry_msgs::msg::Twist & current_speed, + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel, + const double & dt, + double & dynamic_window_max_linear_vel, + double & dynamic_window_min_linear_vel, + double & dynamic_window_max_angular_vel, + double & dynamic_window_min_angular_vel +) +{ + constexpr double Eps = 1e-2; + + // function to compute dynamic window for a single dimension + auto compute_window = [&](const double & current_vel, const double & max_vel, + const double & min_vel, + const double & max_accel, const double & max_decel, + double & dynamic_window_max_vel, double & dynamic_window_min_vel) + { + double candidate_max_vel = 0.0; + double candidate_min_vel = 0.0; + + if (current_vel > Eps) { + // if the current velocity is positive, acceleration means an increase in speed + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel - max_decel * dt; + } else if (current_vel < -Eps) { + // if the current velocity is negative, acceleration means a decrease in speed + candidate_max_vel = current_vel + max_decel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } else { + // if the current velocity is zero, allow acceleration in both directions. + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } + + // clip to max/min velocity limits + dynamic_window_max_vel = std::min(candidate_max_vel, max_vel); + dynamic_window_min_vel = std::max(candidate_min_vel, min_vel); + }; + + // linear velocity + compute_window(current_speed.linear.x, + max_linear_vel, min_linear_vel, + max_linear_accel, max_linear_decel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + // angular velocity + compute_window(current_speed.angular.z, + max_angular_vel, min_angular_vel, + max_angular_accel, max_angular_decel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel); +} + void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( - const double curvature, - const geometry_msgs::msg::Twist current_speed, - const double regulated_linear_vel, + const double & curvature, + const geometry_msgs::msg::Twist & current_speed, + const double & regulated_linear_vel, double & optimal_linear_vel, double & optimal_angular_vel ) @@ -177,19 +239,33 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double & min_angular_vel = params_->min_angular_vel; const double & max_linear_accel = params_->max_linear_accel; + const double & max_linear_decel = params_->max_linear_decel; const double & max_angular_accel = params_->max_angular_accel; - const double & dt = control_duration_; + const double & max_angular_decel = params_->max_angular_decel; + const double & dt = control_duration_; - // ---- 1) Dynamic Window ---- - double dynamic_window_max_linear_vel = std::min(current_speed.linear.x + max_linear_accel * dt, - max_linear_vel); - const double dynamic_window_min_linear_vel = std::max(current_speed.linear.x - max_linear_accel * - dt, min_linear_vel); - double dynamic_window_max_angular_vel = std::min(current_speed.angular.z + max_angular_accel * dt, - max_angular_vel); - const double dynamic_window_min_angular_vel = std::max(current_speed.angular.z - - max_angular_accel * dt, min_angular_vel); + double dynamic_window_max_linear_vel; + double dynamic_window_min_linear_vel; + double dynamic_window_max_angular_vel; + double dynamic_window_min_angular_vel; + + // compute Dynamic Window + computeDynamicWindow( + current_speed, + max_linear_vel, + min_linear_vel, + max_angular_vel, + min_angular_vel, + max_linear_accel, + max_linear_decel, + max_angular_accel, + max_angular_decel, + dt, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel); // Reflect regulated v (tighten upper limit) if (dynamic_window_max_linear_vel > regulated_linear_vel) { From 9033141a4d6e8bdbf9ef1699ab3ce99e12a6d939 Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 23 Oct 2025 21:42:02 +0900 Subject: [PATCH 13/29] :recycle: refactor dwpp function to improve readability Signed-off-by: Decwest --- .../src/regulated_pure_pursuit_controller.cpp | 136 +++++++++--------- 1 file changed, 67 insertions(+), 69 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index a30a8ff5a53..70b5a7e04d6 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -269,75 +269,73 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); - // Reflect regulated v (tighten upper limit) + // clip dynamic window's max linear velocity to regulated linear velocity if (dynamic_window_max_linear_vel > regulated_linear_vel) { dynamic_window_max_linear_vel = std::max(dynamic_window_min_linear_vel, regulated_linear_vel); } - const double k = curvature; + // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) + // Select the closest point to the line angular_vel = curvature * linear_vel within the dynamic window. + // If multiple points are equally close, select the one with the highest linear_vel. - // ---- Curvature is 0 (w = 0) ---- - if (abs(k) < 1e-3) { - // If w=0 is within DW, then the maximum linear speed is adopted as it is. + // When curvature == 0, the line is angular_vel = 0 + if (abs(curvature) < 1e-3) { + // If the line angular_vel = 0 intersects the dynamic window, select (dynamic_window_max_linear_vel, 0) if (dynamic_window_min_angular_vel <= 0.0 && 0.0 <= dynamic_window_max_angular_vel) { - optimal_linear_vel = dynamic_window_max_linear_vel; // Always maximum v + optimal_linear_vel = dynamic_window_max_linear_vel; optimal_angular_vel = 0.0; - return; + } else { + // If not, select (dynamic_window_max_linear_vel, angular vel within dynamic window closest to 0) + optimal_linear_vel = dynamic_window_max_linear_vel; + if (std::abs(dynamic_window_min_angular_vel) <= std::abs(dynamic_window_max_angular_vel)) { + optimal_angular_vel = dynamic_window_min_angular_vel; + } else { + optimal_angular_vel = dynamic_window_max_angular_vel; + } } - // If w=0 is outside, choose the side closer to w=0 and with smaller |w|. - const double w_choice = (std::abs(dynamic_window_min_angular_vel) <= - std::abs(dynamic_window_max_angular_vel)) ? dynamic_window_min_angular_vel : - dynamic_window_max_angular_vel; - optimal_linear_vel = dynamic_window_max_linear_vel; // Always maximum v - optimal_angular_vel = w_choice; return; } - // ---- 2) Select 'max v' from the candidates in the DW among the intersections. ---- - double best_v = -std::numeric_limits::infinity(); // Initial value for maximization - double best_w = 0.0; - - // Intersection with vertical edges - { - const double v1 = dynamic_window_min_linear_vel; - const double w1 = k * v1; - if (w1 >= dynamic_window_min_angular_vel && w1 <= dynamic_window_max_angular_vel) { - if (v1 > best_v) {best_v = v1; best_w = w1;} - } - } - { - const double v2 = dynamic_window_max_linear_vel; - const double w2 = k * v2; - if (w2 >= dynamic_window_min_angular_vel && w2 <= dynamic_window_max_angular_vel) { - if (v2 > best_v) {best_v = v2; best_w = w2;} - } - } - - // Intersection with horizontal edge (k ! = 0) - { - const double v3 = dynamic_window_min_angular_vel / k; - if (v3 >= dynamic_window_min_linear_vel && v3 <= dynamic_window_max_linear_vel) { - const double w3 = dynamic_window_min_angular_vel; - if (v3 > best_v) {best_v = v3; best_w = w3;} - } - } - { - const double v4 = dynamic_window_max_angular_vel / k; - if (v4 >= dynamic_window_min_linear_vel && v4 <= dynamic_window_max_linear_vel) { - const double w4 = dynamic_window_max_angular_vel; - if (v4 > best_v) {best_v = v4; best_w = w4;} + // When the dynamic window and the line angular_vel = curvature * linear_vel intersect, + // select the intersection point that yields the highest linear velocity. + + // List the four candidate intersection points + std::pair candidates[] = { + {dynamic_window_min_linear_vel, curvature * dynamic_window_min_linear_vel}, + {dynamic_window_max_linear_vel, curvature * dynamic_window_max_linear_vel}, + {dynamic_window_min_angular_vel / curvature, dynamic_window_min_angular_vel}, + {dynamic_window_max_angular_vel / curvature, dynamic_window_max_angular_vel} + }; + + double best_linear_vel = -std::numeric_limits::infinity(); + double best_angular_vel = 0.0; + + for (auto [linear_vel, angular_vel] : candidates) { + // Check whether the candidate lies within the dynamic window + if (linear_vel >= dynamic_window_min_linear_vel && + linear_vel <= dynamic_window_max_linear_vel && + angular_vel >= dynamic_window_min_angular_vel && + angular_vel <= dynamic_window_max_angular_vel) + { + // Update if this candidate has the highest linear velocity so far + if (linear_vel > best_linear_vel) { + best_linear_vel = linear_vel; + best_angular_vel = angular_vel; + } } } - if (best_v > -std::numeric_limits::infinity()) { - // Intersection found → Adopt max. v - optimal_linear_vel = best_v; - optimal_angular_vel = best_w; + // If best_linear_vel was updated, it means that a valid intersection exists + if (best_linear_vel > -std::numeric_limits::infinity()) { + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; return; } - // ---- 3) If no intersection exists: Select the one with the smallest - // Euclidean distance to the line w = k v among the 4 corners + // When the dynamic window and the line angular_vel = curvature * linear_vel have no intersection, + // select the point within the dynamic window that is closest to the line. + + // Because the dynamic window is a convex region, the closest point must be one of its four corners. const std::array, 4> corners = {{ {dynamic_window_min_linear_vel, dynamic_window_min_angular_vel}, {dynamic_window_min_linear_vel, dynamic_window_max_angular_vel}, @@ -345,30 +343,30 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( {dynamic_window_max_linear_vel, dynamic_window_max_angular_vel} }}; - const double denom = std::sqrt(k * k + 1.0); // Just sqrt once - - auto euclid_dist = [&](const std::array & corner) -> double { - // Distance from point (v, w) to line w - k v = 0 - return std::abs(k * corner[0] - corner[1]) / denom; + // Compute the distance from a point (linear_vel, angular_vel) to the line angular_vel = curvature * linear_vel + const double denom = std::sqrt(curvature * curvature + 1.0); + auto compute_dist = [&](const std::array & corner) -> double { + return std::abs(curvature * corner[0] - corner[1]) / denom; }; - double best_dist = std::numeric_limits::infinity(); - best_v = corners[0][0]; - best_w = corners[0][1]; + double closest_dist = std::numeric_limits::infinity(); + best_linear_vel = -std::numeric_limits::infinity(); + best_angular_vel = 0.0; for (const auto & corner : corners) { - const double d = euclid_dist(corner); - // 1) Smaller distance → Adopted - // 2) If distances are equal (~1e-12) → Choose the one with larger v (acceleration policy) - if (d < best_dist || (std::abs(d - best_dist) <= 1e-3 && corner[0] > best_v)) { - best_dist = d; - best_v = corner[0]; - best_w = corner[1]; + const double dist = compute_dist(corner); + // Update if this corner is closer to the line, or equally close but has a higher linear velocity + if (dist < closest_dist || + (std::abs(dist - closest_dist) <= 1e-3 && corner[0] > best_linear_vel)) + { + closest_dist = dist; + best_linear_vel = corner[0]; + best_angular_vel = corner[1]; } } - optimal_linear_vel = best_v; - optimal_angular_vel = best_w; + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; } geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( From 79ea88b31f37de1e2a1a6a9bbae6332975430101 Mon Sep 17 00:00:00 2001 From: Decwest Date: Wed, 29 Oct 2025 22:19:16 +0900 Subject: [PATCH 14/29] :bug: fix regulation procedure of dynamic window during negative velocity, add test (WIP) Signed-off-by: Decwest --- .../regulated_pure_pursuit_controller.hpp | 11 ++ .../src/regulated_pure_pursuit_controller.cpp | 48 +++++- .../test/test_regulated_pp.cpp | 149 ++++++++++++++++++ 3 files changed, 204 insertions(+), 4 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index ee31be9847a..1e97a1ea9ea 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -115,6 +115,17 @@ class RegulatedPurePursuitController : public nav2_core::Controller double & dynamic_window_min_angular_vel ); + /** + * @brief Apply regulated linear velocity to the dynamic window + * @param regulated_linear_vel Regulated linear velocity + * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window + * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window + */ + void applyRegulationToDynamicWindow( + const double & regulated_linear_vel, + double & dynamic_window_max_linear_vel, + double & dynamic_window_min_linear_vel); + /** * @brief Compute the optimal command given the current pose, velocity and acceleration constraints using dynamic window * @param curvature Curvature of the path to follow diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 70b5a7e04d6..bb3312f3526 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -227,6 +227,45 @@ void RegulatedPurePursuitController::computeDynamicWindow( dynamic_window_min_angular_vel); } +void RegulatedPurePursuitController::applyRegulationToDynamicWindow( + const double & regulated_linear_vel, + double & dynamic_window_max_linear_vel, + double & dynamic_window_min_linear_vel) +{ + // Extract the portion of the dynamic window that lies within the range [0, regulated_linear_vel] + double dynamic_window_max_linear_vel_temp; + double dynamic_window_min_linear_vel_temp; + if (regulated_linear_vel >= 0.0) { + dynamic_window_max_linear_vel_temp = std::min( + dynamic_window_max_linear_vel, regulated_linear_vel); + dynamic_window_min_linear_vel_temp = std::max( + dynamic_window_min_linear_vel, 0.0); + } else { + dynamic_window_max_linear_vel_temp = std::min( + dynamic_window_max_linear_vel, 0.0); + dynamic_window_min_linear_vel_temp = std::max( + dynamic_window_min_linear_vel, regulated_linear_vel); + } + + if (dynamic_window_max_linear_vel_temp >= dynamic_window_min_linear_vel_temp) { + dynamic_window_max_linear_vel = dynamic_window_max_linear_vel_temp; + dynamic_window_min_linear_vel = dynamic_window_min_linear_vel_temp; + } else { + // No valid portion of the dynamic window remains after applying the regulation + if (dynamic_window_min_linear_vel > 0.0) { + // If the dynamic window is entirely in the positive range, + // collapse both bounds to dynamic_window_min_linear_vel + dynamic_window_max_linear_vel = dynamic_window_min_linear_vel; + } else { + // If the dynamic window is entirely in the negative range, + // collapse both bounds to dynamic_window_max_linear_vel + dynamic_window_min_linear_vel = dynamic_window_max_linear_vel; + } + } + + return; +} + void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double & curvature, const geometry_msgs::msg::Twist & current_speed, @@ -269,10 +308,11 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); - // clip dynamic window's max linear velocity to regulated linear velocity - if (dynamic_window_max_linear_vel > regulated_linear_vel) { - dynamic_window_max_linear_vel = std::max(dynamic_window_min_linear_vel, regulated_linear_vel); - } + // apply regulation to Dynamic Window + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) // Select the closest point to the line angular_vel = curvature * linear_vel within the dynamic window. diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 4d593b35186..4395559eea2 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -91,6 +91,40 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure { return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); } + + void computeDynamicWindowWrapper( + const geometry_msgs::msg::Twist & current_speed, + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel, + const double & dt, + double & dynamic_window_max_linear_vel, + double & dynamic_window_min_linear_vel, + double & dynamic_window_max_angular_vel, + double & dynamic_window_min_angular_vel) + { + return computeDynamicWindow(current_speed, max_linear_vel, min_linear_vel, max_angular_vel, + min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, dt, + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel); + } + + void computeOptimalVelocityUsingDynamicWindowWrapper( + const double & curvature, + const geometry_msgs::msg::Twist & current_speed, + const double & regulated_linear_vel, + double & optimal_linear_vel, + double & optimal_angular_vel) + { + return computeOptimalVelocityUsingDynamicWindow( + curvature, current_speed, regulated_linear_vel, + optimal_linear_vel, optimal_angular_vel); + } }; TEST(RegulatedPurePursuitTest, basicAPI) @@ -422,6 +456,121 @@ TEST(RegulatedPurePursuitTest, applyConstraints) // EXPECT_NEAR(linear_vel, 0.5, 0.01); } +TEST(RegulatedPurePursuitTest, computeDynamicWindow) +{ + auto ctrl = std::make_shared(); + + geometry_msgs::msg::Twist current_speed = geometry_msgs::msg::Twist(); + current_speed.linear.x = 0.5; + current_speed.angular.z = 0.2; + + double max_linear_vel = 0.5; + double min_linear_vel = -0.5; + double max_angular_vel = 1.0; + double min_angular_vel = -1.0; + + double max_linear_accel = 0.5; + double max_linear_decel = 0.5; + double max_angular_accel = 1.0; + double max_angular_decel = 1.0; + + double dt = 0.05; + + double dynamic_window_max_linear_vel; + double dynamic_window_min_linear_vel; + double dynamic_window_max_angular_vel; + double dynamic_window_min_angular_vel; + + ctrl->computeDynamicWindowWrapper( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, dt, + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.525); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.475); + EXPECT_EQ(dynamic_window_max_angular_vel, 1.05); + EXPECT_EQ(dynamic_window_min_angular_vel, 0.15); + +} + +TEST(RegulatedPurePursuitTest, computeOptimalVelocityUsingDynamicWindow) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + + // declare parameters + constexpr double max_linear_vel = 0.6; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_linear_vel", + rclcpp::ParameterValue(max_linear_vel)); + constexpr double min_linear_vel = 0.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".min_linear_vel", + rclcpp::ParameterValue(min_linear_vel)); + constexpr double max_angular_vel = 1.2; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_angular_vel", + rclcpp::ParameterValue(max_angular_vel)); + constexpr double min_angular_vel = -1.2; + nav2::declare_parameter_if_not_declared( + node, + name + ".min_angular_vel", + rclcpp::ParameterValue(min_angular_vel)); + constexpr double max_linear_accel = 0.5; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_linear_accel", + rclcpp::ParameterValue(max_linear_accel)); + constexpr double max_linear_decel = 0.5; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_linear_decel", + rclcpp::ParameterValue(max_linear_decel)); + constexpr double max_angular_accel = 1.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_angular_accel", + rclcpp::ParameterValue(max_angular_accel)); + constexpr double max_angular_decel = 1.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_angular_decel", + rclcpp::ParameterValue(max_angular_decel)); + constexpr double control_frequency = 20.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".controller_frequency", + rclcpp::ParameterValue(control_frequency)); + + ctrl->configure(node, name, tf, costmap); + + double curvature = 0.5; + geometry_msgs::msg::Twist current_speed = geometry_msgs::msg::Twist(); + current_speed.linear.x = 0.5; + current_speed.angular.z = 0.2; + double regulated_linear_vel = 0.5; + double optimal_linear_vel = 0.0; + double optimal_angular_vel = 0.0; + + ctrl->computeOptimalVelocityUsingDynamicWindow( + curvature, current_speed, regulated_linear_vel, + optimal_linear_vel, optimal_angular_vel); + + EXPECT_EQ(optimal_linear_vel, 0.525); + EXPECT_EQ(optimal_angular_vel, 1.05); + +} + TEST(RegulatedPurePursuitTest, testDynamicParameter) { auto node = std::make_shared("Smactest"); From 5d21f30daeb8b6cf01bd4c654eb1e5c85c7d15e7 Mon Sep 17 00:00:00 2001 From: Decwest Date: Wed, 29 Oct 2025 22:55:14 +0900 Subject: [PATCH 15/29] :bug: fix optimal velocity selection when moving backward Signed-off-by: Decwest --- .../regulated_pure_pursuit_controller.hpp | 1 + .../src/regulated_pure_pursuit_controller.cpp | 18 +++++----- .../test/test_regulated_pp.cpp | 34 +++++++++++++++++-- 3 files changed, 43 insertions(+), 10 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 1e97a1ea9ea..7e4cdd4a4e3 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -138,6 +138,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller const double & curvature, const geometry_msgs::msg::Twist & current_speed, const double & regulated_linear_vel, + const double & sign, double & optimal_linear_vel, double & optimal_angular_vel ); diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index bb3312f3526..7ab5f7d2a11 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -270,6 +270,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( const double & curvature, const geometry_msgs::msg::Twist & current_speed, const double & regulated_linear_vel, + const double & sign, double & optimal_linear_vel, double & optimal_angular_vel ) @@ -347,7 +348,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( {dynamic_window_max_angular_vel / curvature, dynamic_window_max_angular_vel} }; - double best_linear_vel = -std::numeric_limits::infinity(); + double best_linear_vel = -std::numeric_limits::infinity() * sign; double best_angular_vel = 0.0; for (auto [linear_vel, angular_vel] : candidates) { @@ -357,8 +358,8 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( angular_vel >= dynamic_window_min_angular_vel && angular_vel <= dynamic_window_max_angular_vel) { - // Update if this candidate has the highest linear velocity so far - if (linear_vel > best_linear_vel) { + // Select the candidate with the largest linear velocity (considering moving direction) + if (linear_vel * sign > best_linear_vel * sign) { best_linear_vel = linear_vel; best_angular_vel = angular_vel; } @@ -366,7 +367,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( } // If best_linear_vel was updated, it means that a valid intersection exists - if (best_linear_vel > -std::numeric_limits::infinity()) { + if (best_linear_vel != -std::numeric_limits::infinity() * sign) { optimal_linear_vel = best_linear_vel; optimal_angular_vel = best_angular_vel; return; @@ -390,14 +391,14 @@ void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( }; double closest_dist = std::numeric_limits::infinity(); - best_linear_vel = -std::numeric_limits::infinity(); + best_linear_vel = -std::numeric_limits::infinity() * sign; best_angular_vel = 0.0; for (const auto & corner : corners) { const double dist = compute_dist(corner); - // Update if this corner is closer to the line, or equally close but has a higher linear velocity + // Update if this corner is closer to the line, or equally close but has a larger linear velocity (considering moving direction) if (dist < closest_dist || - (std::abs(dist - closest_dist) <= 1e-3 && corner[0] > best_linear_vel)) + (std::abs(dist - closest_dist) <= 1e-3 && corner[0] * sign > best_linear_vel * sign)) { closest_dist = dist; best_linear_vel = corner[0]; @@ -525,11 +526,12 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity if (params_->velocity_feedback == "CLOSED_LOOP") { // using odom velocity as a current velocity (not recommended) computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, regulated_linear_vel, + x_vel_sign, linear_vel, angular_vel); } else { // using last command velocity as a current velocity (recommended) computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, - regulated_linear_vel, linear_vel, angular_vel); + regulated_linear_vel, x_vel_sign, linear_vel, angular_vel); } } } diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 4395559eea2..bc53e891f94 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -114,15 +114,27 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure dynamic_window_min_angular_vel); } + void applyRegulationToDynamicWindowWrapper( + const double & regulated_linear_vel, + double & dynamic_window_max_linear_vel, + double & dynamic_window_min_linear_vel) + { + return applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + } + void computeOptimalVelocityUsingDynamicWindowWrapper( const double & curvature, const geometry_msgs::msg::Twist & current_speed, const double & regulated_linear_vel, + const double & sign, double & optimal_linear_vel, double & optimal_angular_vel) { return computeOptimalVelocityUsingDynamicWindow( - curvature, current_speed, regulated_linear_vel, + curvature, current_speed, regulated_linear_vel, sign, optimal_linear_vel, optimal_angular_vel); } }; @@ -495,6 +507,23 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) } +TEST(RegulatedPurePursuitTest, applyRegulationToDynamicWindow) +{ + auto ctrl = std::make_shared(); + + double regulated_linear_vel = 0.5; + double dynamic_window_max_linear_vel; + double dynamic_window_min_linear_vel; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.525); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.475); +} + TEST(RegulatedPurePursuitTest, computeOptimalVelocityUsingDynamicWindow) { auto ctrl = std::make_shared(); @@ -559,11 +588,12 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityUsingDynamicWindow) current_speed.linear.x = 0.5; current_speed.angular.z = 0.2; double regulated_linear_vel = 0.5; + double sign = 1.0; double optimal_linear_vel = 0.0; double optimal_angular_vel = 0.0; ctrl->computeOptimalVelocityUsingDynamicWindow( - curvature, current_speed, regulated_linear_vel, + curvature, current_speed, regulated_linear_vel, sign, optimal_linear_vel, optimal_angular_vel); EXPECT_EQ(optimal_linear_vel, 0.525); From 0795c7dee9dadec2d2b316975733475067c4b0ae Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 30 Oct 2025 01:19:43 +0900 Subject: [PATCH 16/29] :recycle: refactor for easy unit test Signed-off-by: Decwest --- .../regulated_pure_pursuit_controller.hpp | 17 ++- .../src/regulated_pure_pursuit_controller.cpp | 126 ++++++++++-------- .../test/test_regulated_pp.cpp | 82 +++--------- 3 files changed, 100 insertions(+), 125 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7e4cdd4a4e3..d929087b0c7 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -127,17 +127,22 @@ class RegulatedPurePursuitController : public nav2_core::Controller double & dynamic_window_min_linear_vel); /** - * @brief Compute the optimal command given the current pose, velocity and acceleration constraints using dynamic window + * @brief Compute the optimal velocity to follow the path within the dynamic window + * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window + * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window + * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window + * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window * @param curvature Curvature of the path to follow - * @param current_speed Current robot velocity - * @param regulated_linear_vel Regulated linear velocity + * @param sign Velocity sign (forward or backward) * @param optimal_linear_vel Optimal linear velocity to follow the path under velocity and acceleration constraints * @param optimal_angular_vel Optimal angular velocity to follow the path under velocity and acceleration constraints */ - void computeOptimalVelocityUsingDynamicWindow( + void computeOptimalVelocityWithinDynamicWindow( + const double & dynamic_window_max_linear_vel, + const double & dynamic_window_min_linear_vel, + const double & dynamic_window_max_angular_vel, + const double & dynamic_window_min_angular_vel, const double & curvature, - const geometry_msgs::msg::Twist & current_speed, - const double & regulated_linear_vel, const double & sign, double & optimal_linear_vel, double & optimal_angular_vel diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 7ab5f7d2a11..fc0e3728f7d 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -266,68 +266,38 @@ void RegulatedPurePursuitController::applyRegulationToDynamicWindow( return; } -void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow( +void RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( + const double & dynamic_window_max_linear_vel, + const double & dynamic_window_min_linear_vel, + const double & dynamic_window_max_angular_vel, + const double & dynamic_window_min_angular_vel, const double & curvature, - const geometry_msgs::msg::Twist & current_speed, - const double & regulated_linear_vel, const double & sign, double & optimal_linear_vel, double & optimal_angular_vel ) { - const double & max_linear_vel = params_->max_linear_vel; - const double & min_linear_vel = params_->min_linear_vel; - const double & max_angular_vel = params_->max_angular_vel; - const double & min_angular_vel = params_->min_angular_vel; - - const double & max_linear_accel = params_->max_linear_accel; - const double & max_linear_decel = params_->max_linear_decel; - const double & max_angular_accel = params_->max_angular_accel; - const double & max_angular_decel = params_->max_angular_decel; - - const double & dt = control_duration_; - - double dynamic_window_max_linear_vel; - double dynamic_window_min_linear_vel; - double dynamic_window_max_angular_vel; - double dynamic_window_min_angular_vel; - - // compute Dynamic Window - computeDynamicWindow( - current_speed, - max_linear_vel, - min_linear_vel, - max_angular_vel, - min_angular_vel, - max_linear_accel, - max_linear_decel, - max_angular_accel, - max_angular_decel, - dt, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel); - - // apply regulation to Dynamic Window - applyRegulationToDynamicWindow( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) // Select the closest point to the line angular_vel = curvature * linear_vel within the dynamic window. - // If multiple points are equally close, select the one with the highest linear_vel. + // If multiple points are equally close, select the one with the largest linear_vel. // When curvature == 0, the line is angular_vel = 0 if (abs(curvature) < 1e-3) { - // If the line angular_vel = 0 intersects the dynamic window, select (dynamic_window_max_linear_vel, 0) - if (dynamic_window_min_angular_vel <= 0.0 && 0.0 <= dynamic_window_max_angular_vel) { + // linear velocity + if (sign >= 0.0) { + // If moving forward, select the max linear vel optimal_linear_vel = dynamic_window_max_linear_vel; + } else { + // If moving backward, select the min linear vel + optimal_linear_vel = dynamic_window_min_linear_vel; + } + + // angular velocity + // If the line angular_vel = 0 intersects the dynamic window,angular_vel = 0.0 + if (dynamic_window_min_angular_vel <= 0.0 && 0.0 <= dynamic_window_max_angular_vel) { optimal_angular_vel = 0.0; } else { - // If not, select (dynamic_window_max_linear_vel, angular vel within dynamic window closest to 0) - optimal_linear_vel = dynamic_window_max_linear_vel; + // If not, select angular vel within dynamic window closest to 0 if (std::abs(dynamic_window_min_angular_vel) <= std::abs(dynamic_window_max_angular_vel)) { optimal_angular_vel = dynamic_window_min_angular_vel; } else { @@ -523,16 +493,66 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // compute optimal path tracking velocity commands // considering velocity and acceleration constraints const double regulated_linear_vel = linear_vel; + geometry_msgs::msg::Twist current_speed; if (params_->velocity_feedback == "CLOSED_LOOP") { // using odom velocity as a current velocity (not recommended) - computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, regulated_linear_vel, - x_vel_sign, - linear_vel, angular_vel); + current_speed = speed; } else { // using last command velocity as a current velocity (recommended) - computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, - regulated_linear_vel, x_vel_sign, linear_vel, angular_vel); + current_speed = last_command_velocity_; } + + const double & max_linear_vel = params_->max_linear_vel; + const double & min_linear_vel = params_->min_linear_vel; + const double & max_angular_vel = params_->max_angular_vel; + const double & min_angular_vel = params_->min_angular_vel; + + const double & max_linear_accel = params_->max_linear_accel; + const double & max_linear_decel = params_->max_linear_decel; + const double & max_angular_accel = params_->max_angular_accel; + const double & max_angular_decel = params_->max_angular_decel; + + const double & dt = control_duration_; + + double dynamic_window_max_linear_vel; + double dynamic_window_min_linear_vel; + double dynamic_window_max_angular_vel; + double dynamic_window_min_angular_vel; + + // compute Dynamic Window + computeDynamicWindow( + current_speed, + max_linear_vel, + min_linear_vel, + max_angular_vel, + min_angular_vel, + max_linear_accel, + max_linear_decel, + max_angular_accel, + max_angular_decel, + dt, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel); + + // apply regulation to Dynamic Window + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + // compute optimal velocity within Dynamic Window + computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, + regulation_curvature, + x_vel_sign, + linear_vel, + angular_vel + ); } } diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index bc53e891f94..57c08f32275 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -125,16 +125,19 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure dynamic_window_min_linear_vel); } - void computeOptimalVelocityUsingDynamicWindowWrapper( + void computeOptimalVelocityWithinDynamicWindowWrapper( + const double & dynamic_window_max_linear_vel, + const double & dynamic_window_min_linear_vel, + const double & dynamic_window_max_angular_vel, + const double & dynamic_window_min_angular_vel, const double & curvature, - const geometry_msgs::msg::Twist & current_speed, - const double & regulated_linear_vel, const double & sign, double & optimal_linear_vel, double & optimal_angular_vel) { - return computeOptimalVelocityUsingDynamicWindow( - curvature, current_speed, regulated_linear_vel, sign, + return computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign, optimal_linear_vel, optimal_angular_vel); } }; @@ -524,76 +527,23 @@ TEST(RegulatedPurePursuitTest, applyRegulationToDynamicWindow) EXPECT_EQ(dynamic_window_min_linear_vel, 0.475); } -TEST(RegulatedPurePursuitTest, computeOptimalVelocityUsingDynamicWindow) +TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) { auto ctrl = std::make_shared(); - auto node = std::make_shared("testRPP"); - std::string name = "PathFollower"; - auto tf = std::make_shared(node->get_clock()); - auto costmap = std::make_shared("fake_costmap"); - rclcpp_lifecycle::State state; - costmap->on_configure(state); - // declare parameters - constexpr double max_linear_vel = 0.6; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_linear_vel", - rclcpp::ParameterValue(max_linear_vel)); - constexpr double min_linear_vel = 0.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".min_linear_vel", - rclcpp::ParameterValue(min_linear_vel)); - constexpr double max_angular_vel = 1.2; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_angular_vel", - rclcpp::ParameterValue(max_angular_vel)); - constexpr double min_angular_vel = -1.2; - nav2::declare_parameter_if_not_declared( - node, - name + ".min_angular_vel", - rclcpp::ParameterValue(min_angular_vel)); - constexpr double max_linear_accel = 0.5; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_linear_accel", - rclcpp::ParameterValue(max_linear_accel)); - constexpr double max_linear_decel = 0.5; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_linear_decel", - rclcpp::ParameterValue(max_linear_decel)); - constexpr double max_angular_accel = 1.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_angular_accel", - rclcpp::ParameterValue(max_angular_accel)); - constexpr double max_angular_decel = 1.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_angular_decel", - rclcpp::ParameterValue(max_angular_decel)); - constexpr double control_frequency = 20.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".controller_frequency", - rclcpp::ParameterValue(control_frequency)); - - ctrl->configure(node, name, tf, costmap); + double dynamic_window_max_linear_vel = 0.5; + double dynamic_window_min_linear_vel = 0.0; + double dynamic_window_max_angular_vel = 0.5; + double dynamic_window_min_angular_vel = -0.5; double curvature = 0.5; - geometry_msgs::msg::Twist current_speed = geometry_msgs::msg::Twist(); - current_speed.linear.x = 0.5; - current_speed.angular.z = 0.2; - double regulated_linear_vel = 0.5; double sign = 1.0; double optimal_linear_vel = 0.0; double optimal_angular_vel = 0.0; - ctrl->computeOptimalVelocityUsingDynamicWindow( - curvature, current_speed, regulated_linear_vel, sign, + ctrl->computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign, optimal_linear_vel, optimal_angular_vel); EXPECT_EQ(optimal_linear_vel, 0.525); From abb6c095aa21d87631fff5cec7b00efc0989db13 Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 30 Oct 2025 12:32:10 +0900 Subject: [PATCH 17/29] :zap: add unit test of computeDynamicWindow() Signed-off-by: Decwest --- .../regulated_pure_pursuit_controller.hpp | 18 -- .../src/regulated_pure_pursuit_controller.cpp | 51 ++---- .../test/test_regulated_pp.cpp | 161 +++++++++++++----- 3 files changed, 134 insertions(+), 96 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index d929087b0c7..5c05f95215b 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -84,15 +84,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller /** * @brief Compute the dynamic window (feasible velocity bounds) based on the current speed and the given velocity and acceleration constraints. * @param current_speed Current linear and angular velocity of the robot - * @param max_linear_vel Maximum linear velocity - * @param min_linear_vel Minimum linear velocity - * @param max_angular_vel Maximum angular velocity - * @param min_angular_vel Minimum angular velocity - * @param max_linear_accel Maximum linear acceleration - * @param max_linear_decel Maximum linear deceleration - * @param max_angular_accel Maximum angular acceleration - * @param max_angular_decel Maximum angular deceleration - * @param dt Control loop sampling period * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window @@ -100,15 +91,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ void computeDynamicWindow( const geometry_msgs::msg::Twist & current_speed, - const double & max_linear_vel, - const double & min_linear_vel, - const double & max_angular_vel, - const double & min_angular_vel, - const double & max_linear_accel, - const double & max_linear_decel, - const double & max_angular_accel, - const double & max_angular_decel, - const double & dt, double & dynamic_window_max_linear_vel, double & dynamic_window_min_linear_vel, double & dynamic_window_max_angular_vel, diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index fc0e3728f7d..5f396326a33 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -167,21 +167,24 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point) void RegulatedPurePursuitController::computeDynamicWindow( const geometry_msgs::msg::Twist & current_speed, - const double & max_linear_vel, - const double & min_linear_vel, - const double & max_angular_vel, - const double & min_angular_vel, - const double & max_linear_accel, - const double & max_linear_decel, - const double & max_angular_accel, - const double & max_angular_decel, - const double & dt, double & dynamic_window_max_linear_vel, double & dynamic_window_min_linear_vel, double & dynamic_window_max_angular_vel, double & dynamic_window_min_angular_vel ) { + const double & max_linear_vel = params_->max_linear_vel; + const double & min_linear_vel = params_->min_linear_vel; + const double & max_angular_vel = params_->max_angular_vel; + const double & min_angular_vel = params_->min_angular_vel; + + const double & max_linear_accel = params_->max_linear_accel; + const double & max_linear_decel = params_->max_linear_decel; + const double & max_angular_accel = params_->max_angular_accel; + const double & max_angular_decel = params_->max_angular_decel; + + const double & dt = control_duration_; + constexpr double Eps = 1e-2; // function to compute dynamic window for a single dimension @@ -491,7 +494,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity angular_vel = linear_vel * regulation_curvature; } else { // compute optimal path tracking velocity commands - // considering velocity and acceleration constraints + // considering velocity and acceleration constraints (DWPP) const double regulated_linear_vel = linear_vel; geometry_msgs::msg::Twist current_speed; if (params_->velocity_feedback == "CLOSED_LOOP") { @@ -501,36 +504,12 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // using last command velocity as a current velocity (recommended) current_speed = last_command_velocity_; } - - const double & max_linear_vel = params_->max_linear_vel; - const double & min_linear_vel = params_->min_linear_vel; - const double & max_angular_vel = params_->max_angular_vel; - const double & min_angular_vel = params_->min_angular_vel; - - const double & max_linear_accel = params_->max_linear_accel; - const double & max_linear_decel = params_->max_linear_decel; - const double & max_angular_accel = params_->max_angular_accel; - const double & max_angular_decel = params_->max_angular_decel; - - const double & dt = control_duration_; - - double dynamic_window_max_linear_vel; - double dynamic_window_min_linear_vel; - double dynamic_window_max_angular_vel; - double dynamic_window_min_angular_vel; + double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel; // compute Dynamic Window computeDynamicWindow( current_speed, - max_linear_vel, - min_linear_vel, - max_angular_vel, - min_angular_vel, - max_linear_accel, - max_linear_decel, - max_angular_accel, - max_angular_decel, - dt, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 57c08f32275..17bd6e71f69 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -94,22 +94,12 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure void computeDynamicWindowWrapper( const geometry_msgs::msg::Twist & current_speed, - const double & max_linear_vel, - const double & min_linear_vel, - const double & max_angular_vel, - const double & min_angular_vel, - const double & max_linear_accel, - const double & max_linear_decel, - const double & max_angular_accel, - const double & max_angular_decel, - const double & dt, double & dynamic_window_max_linear_vel, double & dynamic_window_min_linear_vel, double & dynamic_window_max_angular_vel, double & dynamic_window_min_angular_vel) { - return computeDynamicWindow(current_speed, max_linear_vel, min_linear_vel, max_angular_vel, - min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, dt, + return computeDynamicWindow(current_speed, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); } @@ -474,39 +464,130 @@ TEST(RegulatedPurePursuitTest, applyConstraints) TEST(RegulatedPurePursuitTest, computeDynamicWindow) { auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + + // declare parameters + constexpr double max_linear_vel = 0.5; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_linear_vel", + rclcpp::ParameterValue(max_linear_vel)); + constexpr double min_linear_vel = -0.5; + nav2::declare_parameter_if_not_declared( + node, + name + ".min_linear_vel", + rclcpp::ParameterValue(min_linear_vel)); + constexpr double max_angular_vel = 1.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_angular_vel", + rclcpp::ParameterValue(max_angular_vel)); + constexpr double min_angular_vel = -1.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".min_angular_vel", + rclcpp::ParameterValue(min_angular_vel)); + constexpr double max_linear_accel = 0.5; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_linear_accel", + rclcpp::ParameterValue(max_linear_accel)); + constexpr double max_linear_decel = 1.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_linear_decel", + rclcpp::ParameterValue(max_linear_decel)); + constexpr double max_angular_accel = 1.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_angular_accel", + rclcpp::ParameterValue(max_angular_accel)); + constexpr double max_angular_decel = 2.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".max_angular_decel", + rclcpp::ParameterValue(max_angular_decel)); + constexpr double control_frequency = 10.0; + nav2::declare_parameter_if_not_declared( + node, + name + ".controller_frequency", + rclcpp::ParameterValue(control_frequency)); + ctrl->configure(node, name, tf, costmap); + + double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel; geometry_msgs::msg::Twist current_speed = geometry_msgs::msg::Twist(); - current_speed.linear.x = 0.5; - current_speed.angular.z = 0.2; - double max_linear_vel = 0.5; - double min_linear_vel = -0.5; - double max_angular_vel = 1.0; - double min_angular_vel = -1.0; + // case1: current linear velocity is positive, angular velocity is positive, clip by max linear vel + current_speed.linear.x = 0.5; current_speed.angular.z = 0.2; - double max_linear_accel = 0.5; - double max_linear_decel = 0.5; - double max_angular_accel = 1.0; - double max_angular_decel = 1.0; + ctrl->computeDynamicWindowWrapper( + current_speed, + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); - double dt = 0.05; + EXPECT_EQ(dynamic_window_max_linear_vel, 0.5); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.4); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.3); + EXPECT_EQ(dynamic_window_min_angular_vel, 0.0); - double dynamic_window_max_linear_vel; - double dynamic_window_min_linear_vel; - double dynamic_window_max_angular_vel; - double dynamic_window_min_angular_vel; + // case2: current linear velocity is positive, angular velocity is negative + current_speed.linear.x = 0.3; current_speed.angular.z = -0.2; ctrl->computeDynamicWindowWrapper( - current_speed, max_linear_vel, min_linear_vel, - max_angular_vel, min_angular_vel, max_linear_accel, max_linear_decel, - max_angular_accel, max_angular_decel, dt, + current_speed, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.525); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.475); - EXPECT_EQ(dynamic_window_max_angular_vel, 1.05); - EXPECT_EQ(dynamic_window_min_angular_vel, 0.15); + EXPECT_EQ(dynamic_window_max_linear_vel, 0.35); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.2); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.0); + EXPECT_EQ(dynamic_window_min_angular_vel, -0.3); + + // case3: current linear velocity is zero, angular velocity is zero + current_speed.linear.x = 0.0; current_speed.angular.z = -0.0; + + ctrl->computeDynamicWindowWrapper( + current_speed, + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.05); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.05); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.1); + EXPECT_EQ(dynamic_window_min_angular_vel, -0.1); + + // case4: current linear velocity is negative, angular velocity is positive + current_speed.linear.x = -0.3; current_speed.angular.z = 0.2; + + ctrl->computeDynamicWindowWrapper( + current_speed, + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, -0.2); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.35); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.3); + EXPECT_EQ(dynamic_window_min_angular_vel, 0.0); + + // case5: current linear velocity is negative, angular velocity is negative, clipped by min lenear vel + current_speed.linear.x = -0.3; current_speed.angular.z = 0.2; + + ctrl->computeDynamicWindowWrapper( + current_speed, + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, -0.4); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.5); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.0); + EXPECT_EQ(dynamic_window_min_angular_vel, -0.3); } @@ -515,8 +596,7 @@ TEST(RegulatedPurePursuitTest, applyRegulationToDynamicWindow) auto ctrl = std::make_shared(); double regulated_linear_vel = 0.5; - double dynamic_window_max_linear_vel; - double dynamic_window_min_linear_vel; + double dynamic_window_max_linear_vel = 0.5, dynamic_window_min_linear_vel = 0.1; ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, @@ -531,15 +611,12 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) { auto ctrl = std::make_shared(); - double dynamic_window_max_linear_vel = 0.5; - double dynamic_window_min_linear_vel = 0.0; - double dynamic_window_max_angular_vel = 0.5; - double dynamic_window_min_angular_vel = -0.5; - + double dynamic_window_max_linear_vel = 0.5, dynamic_window_min_linear_vel = 0.0; + double dynamic_window_max_angular_vel = 0.5, dynamic_window_min_angular_vel = -0.5; double curvature = 0.5; double sign = 1.0; - double optimal_linear_vel = 0.0; - double optimal_angular_vel = 0.0; + double optimal_linear_vel; + double optimal_angular_vel; ctrl->computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, From 3da3e29e45bcea1a5e99071674c7d7cac0bd4bc6 Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 30 Oct 2025 15:50:12 +0900 Subject: [PATCH 18/29] :zap: add and pass all unit test Signed-off-by: Decwest --- .../src/regulated_pure_pursuit_controller.cpp | 12 +- .../test/test_regulated_pp.cpp | 243 +++++++++++++++--- 2 files changed, 215 insertions(+), 40 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 5f396326a33..9949ab553d3 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -281,7 +281,8 @@ void RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( ) { // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) - // Select the closest point to the line angular_vel = curvature * linear_vel within the dynamic window. + // Select the closest point to the line + // angular_vel = curvature * linear_vel within the dynamic window. // If multiple points are equally close, select the one with the largest linear_vel. // When curvature == 0, the line is angular_vel = 0 @@ -349,7 +350,8 @@ void RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( // When the dynamic window and the line angular_vel = curvature * linear_vel have no intersection, // select the point within the dynamic window that is closest to the line. - // Because the dynamic window is a convex region, the closest point must be one of its four corners. + // Because the dynamic window is a convex region, + // the closest point must be one of its four corners. const std::array, 4> corners = {{ {dynamic_window_min_linear_vel, dynamic_window_min_angular_vel}, {dynamic_window_min_linear_vel, dynamic_window_max_angular_vel}, @@ -357,7 +359,8 @@ void RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( {dynamic_window_max_linear_vel, dynamic_window_max_angular_vel} }}; - // Compute the distance from a point (linear_vel, angular_vel) to the line angular_vel = curvature * linear_vel + // Compute the distance from a point (linear_vel, angular_vel) + // to the line angular_vel = curvature * linear_vel const double denom = std::sqrt(curvature * curvature + 1.0); auto compute_dist = [&](const std::array & corner) -> double { return std::abs(curvature * corner[0] - corner[1]) / denom; @@ -369,7 +372,8 @@ void RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( for (const auto & corner : corners) { const double dist = compute_dist(corner); - // Update if this corner is closer to the line, or equally close but has a larger linear velocity (considering moving direction) + // Update if this corner is closer to the line, + // or equally close but has a larger linear velocity (considering moving direction) if (dist < closest_dist || (std::abs(dist - closest_dist) <= 1e-3 && corner[0] * sign > best_linear_vel * sign)) { diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 17bd6e71f69..994f1e15ba1 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -512,11 +512,6 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) node, name + ".max_angular_decel", rclcpp::ParameterValue(max_angular_decel)); - constexpr double control_frequency = 10.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".controller_frequency", - rclcpp::ParameterValue(control_frequency)); ctrl->configure(node, name, tf, costmap); @@ -524,7 +519,8 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) dynamic_window_max_angular_vel, dynamic_window_min_angular_vel; geometry_msgs::msg::Twist current_speed = geometry_msgs::msg::Twist(); - // case1: current linear velocity is positive, angular velocity is positive, clip by max linear vel + // case1: current linear velocity is positive, angular velocity is positive, + // clip by max linear vel current_speed.linear.x = 0.5; current_speed.angular.z = 0.2; ctrl->computeDynamicWindowWrapper( @@ -533,9 +529,9 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); EXPECT_EQ(dynamic_window_max_linear_vel, 0.5); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.4); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.3); - EXPECT_EQ(dynamic_window_min_angular_vel, 0.0); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.45); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.25); + EXPECT_EQ(dynamic_window_min_angular_vel, 0.10); // case2: current linear velocity is positive, angular velocity is negative current_speed.linear.x = 0.3; current_speed.angular.z = -0.2; @@ -545,10 +541,10 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.35); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.2); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.0); - EXPECT_EQ(dynamic_window_min_angular_vel, -0.3); + EXPECT_EQ(dynamic_window_max_linear_vel, 0.325); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.25); + EXPECT_EQ(dynamic_window_max_angular_vel, -0.1); + EXPECT_EQ(dynamic_window_min_angular_vel, -0.25); // case3: current linear velocity is zero, angular velocity is zero current_speed.linear.x = 0.0; current_speed.angular.z = -0.0; @@ -558,10 +554,10 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.05); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.05); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.1); - EXPECT_EQ(dynamic_window_min_angular_vel, -0.1); + EXPECT_EQ(dynamic_window_max_linear_vel, 0.025); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.025); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.05); + EXPECT_EQ(dynamic_window_min_angular_vel, -0.05); // case4: current linear velocity is negative, angular velocity is positive current_speed.linear.x = -0.3; current_speed.angular.z = 0.2; @@ -571,49 +567,143 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, -0.2); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.35); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.3); - EXPECT_EQ(dynamic_window_min_angular_vel, 0.0); + EXPECT_EQ(dynamic_window_max_linear_vel, -0.25); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.325); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.25); + EXPECT_EQ(dynamic_window_min_angular_vel, 0.10); - // case5: current linear velocity is negative, angular velocity is negative, clipped by min lenear vel - current_speed.linear.x = -0.3; current_speed.angular.z = 0.2; + // case5: current linear velocity is negative, angular velocity is negative, + // clipped by min lenear vel + current_speed.linear.x = -0.5; current_speed.angular.z = -0.2; ctrl->computeDynamicWindowWrapper( current_speed, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, -0.4); + EXPECT_EQ(dynamic_window_max_linear_vel, -0.45); EXPECT_EQ(dynamic_window_min_linear_vel, -0.5); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.0); - EXPECT_EQ(dynamic_window_min_angular_vel, -0.3); - + EXPECT_EQ(dynamic_window_max_angular_vel, -0.1); + EXPECT_EQ(dynamic_window_min_angular_vel, -0.25); } TEST(RegulatedPurePursuitTest, applyRegulationToDynamicWindow) { auto ctrl = std::make_shared(); - double regulated_linear_vel = 0.5; - double dynamic_window_max_linear_vel = 0.5, dynamic_window_min_linear_vel = 0.1; + double regulated_linear_vel; + double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel; + + regulated_linear_vel = 0.3; + dynamic_window_max_linear_vel = 0.5, dynamic_window_min_linear_vel = 0.4; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.4); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.4); + + dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.2; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.3); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.2); + + dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.1; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.2); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.1); + + dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.1); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.0); + + dynamic_window_max_linear_vel = -0.1, dynamic_window_min_linear_vel = -0.3; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, -0.1); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.1); + + regulated_linear_vel = -0.3; + dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.1); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.1); + + dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.0); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.2); + + dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.3; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, -0.2); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.3); + + dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.4; + + ctrl->applyRegulationToDynamicWindowWrapper( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(dynamic_window_max_linear_vel, -0.2); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.3); + + dynamic_window_max_linear_vel = -0.4, dynamic_window_min_linear_vel = -0.5; ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.525); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.475); + EXPECT_EQ(dynamic_window_max_linear_vel, -0.4); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.4); } TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) { auto ctrl = std::make_shared(); - double dynamic_window_max_linear_vel = 0.5, dynamic_window_min_linear_vel = 0.0; - double dynamic_window_max_angular_vel = 0.5, dynamic_window_min_angular_vel = -0.5; - double curvature = 0.5; + double dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + double dynamic_window_max_angular_vel = 0.1, dynamic_window_min_angular_vel = -0.1; + double curvature = 0.0; double sign = 1.0; double optimal_linear_vel; double optimal_angular_vel; @@ -623,9 +713,90 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) dynamic_window_min_angular_vel, curvature, sign, optimal_linear_vel, optimal_angular_vel); - EXPECT_EQ(optimal_linear_vel, 0.525); - EXPECT_EQ(optimal_angular_vel, 1.05); + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.0); + + + dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + + ctrl->computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign, + optimal_linear_vel, optimal_angular_vel); + + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.1); + + curvature = 1.0; + dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; + dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + + ctrl->computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign, + optimal_linear_vel, optimal_angular_vel); + + EXPECT_EQ(optimal_linear_vel, 0.2); + EXPECT_EQ(optimal_angular_vel, 0.2); + + dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; + dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; + + ctrl->computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign, + optimal_linear_vel, optimal_angular_vel); + + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.2); + + sign = -1.0; + curvature = 0.0; + dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + dynamic_window_max_angular_vel = 0.1, dynamic_window_min_angular_vel = -0.1; + + ctrl->computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign, + optimal_linear_vel, optimal_angular_vel); + + EXPECT_EQ(optimal_linear_vel, 0.1); + EXPECT_EQ(optimal_angular_vel, 0.0); + + dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + + ctrl->computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign, + optimal_linear_vel, optimal_angular_vel); + + EXPECT_EQ(optimal_linear_vel, 0.1); + EXPECT_EQ(optimal_angular_vel, 0.1); + + curvature = 1.0; + dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; + dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + + ctrl->computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign, + optimal_linear_vel, optimal_angular_vel); + + EXPECT_EQ(optimal_linear_vel, 0.1); + EXPECT_EQ(optimal_angular_vel, 0.1); + + dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; + dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; + + ctrl->computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign, + optimal_linear_vel, optimal_angular_vel); + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.2); } TEST(RegulatedPurePursuitTest, testDynamicParameter) From 17852334ffb8945997fd6d72669dfd2c4a3ef738 Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 30 Oct 2025 18:13:54 +0900 Subject: [PATCH 19/29] :zap: update README Signed-off-by: Decwest --- nav2_regulated_pure_pursuit_controller/README.md | 4 ++++ .../src/parameter_handler.cpp | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 905645ff89c..4c2fa363d39 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -180,3 +180,7 @@ To tune to get Pure Pursuit behaviors, set all boolean parameters to false and m Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way. The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it. + +When `use_dynamic_window` is set to True, the velocity, acceleration, and deceleration limits are enforced during the velocity command computation. +Note that the velocity smoother clips the velocity commands output by this controller based on its own velocity and acceleration constraints before publishing cmd_vel. +Therefore, the velocity smoother’s `max_velocity`, `min_velocity`, `max_accel`, and `max_decel` parameters must be consistent with this controller’s corresponding velocity, acceleration, and deceleration settings. diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 662ca02c931..eaa5bde328c 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -99,9 +99,9 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(0.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(3.2)); + node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( From e9585d478075cd485aee1be6524b79fedbcc3e81 Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 30 Oct 2025 20:14:30 +0900 Subject: [PATCH 20/29] :memo: update README Signed-off-by: Decwest --- .../README.md | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 4c2fa363d39..7c0ce1633a1 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -61,6 +61,14 @@ Mixing the proximity and curvature regulated linear velocities with the time-sca Note: The maximum allowed time to collision is thresholded by the lookahead point, starting in Humble. This is such that collision checking isn't significantly overshooting the path, which can cause issues in constrained environments. For example, if there were a straight-line path going towards a wall that then turned left, if this parameter was set to high, then it would detect a collision past the point of actual robot intended motion. Thusly, if a robot is moving fast, selecting further out lookahead points is not only a matter of behavioral stability for Pure Pursuit, but also gives a robot further predictive collision detection capabilities. The max allowable time parameter is still in place for slow commands, as described in detail above. +## Dynamic Window Pure Pursuit Features + +This controller also implements the Dynamic Window Pure Pursuit (DWPP) algorithm, developed by [Fumiya Ohnishi](www.linkedin.com/in/fumiya-ohnishi-23b124202). +Unlike the standard Pure Pursuit, DWPP enables the consideration of velocity and acceleration constraints when computing velocity commands. +An overview of the algorithm can be found here: [DWPP Algorithm](https://github.com/Decwest/nav2_dynamic_window_pure_pursuit_controller/blob/main/algorithm.md). + +A link to the paper and its citation will be provided once it becomes publicly available. + ## Configuration | Parameter | Description | @@ -98,8 +106,8 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | | `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | | `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. | -| `use_dynamic_window ` | Whether to use the Dynamic Window Pure Pursuit (DWPP) Algorithm. This algorithm computes optimal path tracking velocity commands under velocity and acceleration constraints. | -| `velocity_feedback ` | How the current velocity is obtained during dynamic window computation. "OPEN_LOOP" uses the last commanded velocity (recommended). "CLOSED_LOOP" uses odometry velocity (may hinder proper acceleration/deceleration) | +| `use_dynamic_window` | Whether to use the Dynamic Window Pure Pursuit (DWPP) Algorithm. This algorithm computes optimal path tracking velocity commands under velocity and acceleration constraints. | +| `velocity_feedback` | How the current velocity is obtained during dynamic window computation. "OPEN_LOOP" uses the last commanded velocity (recommended). "CLOSED_LOOP" uses odometry velocity (may hinder proper acceleration/deceleration) | Example fully-described XML with default parameter values: @@ -141,7 +149,7 @@ controller_server: transform_tolerance: 0.1 use_velocity_scaled_lookahead_dist: false min_approach_linear_velocity: 0.05 - approach_velocity_scaling_dist: 1.0 + approach_velocity_scaling_dist: 0.6 use_collision_detection: true max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true @@ -149,15 +157,18 @@ controller_server: regulated_linear_scaling_min_radius: 0.9 regulated_linear_scaling_min_speed: 0.25 use_fixed_curvature_lookahead: false - curvature_lookahead_dist: 1.0 + curvature_lookahead_dist: 0.6 use_rotate_to_heading: true + allow_reversing: false rotate_to_heading_min_angle: 0.785 max_robot_pose_search_dist: 10.0 interpolate_curvature_after_goal: false cost_scaling_dist: 0.3 cost_scaling_gain: 1.0 inflation_cost_scaling_factor: 3.0 + min_distance_to_obstacle: 0.0 use_dynamic_window: true + velocity_feedback: "OPEN_LOOP" ``` ## Topics From cd170abc9bb4d73929ebc071864323b7b385c5dc Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 30 Oct 2025 21:08:02 +0900 Subject: [PATCH 21/29] :adhesive_bandage: fix hyperlink of README Signed-off-by: Decwest --- nav2_regulated_pure_pursuit_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 7c0ce1633a1..3ec14873b80 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -63,7 +63,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin ## Dynamic Window Pure Pursuit Features -This controller also implements the Dynamic Window Pure Pursuit (DWPP) algorithm, developed by [Fumiya Ohnishi](www.linkedin.com/in/fumiya-ohnishi-23b124202). +This controller also implements the Dynamic Window Pure Pursuit (DWPP) algorithm, developed by [Fumiya Ohnishi](https://www.linkedin.com/in/fumiya-ohnishi-23b124202). Unlike the standard Pure Pursuit, DWPP enables the consideration of velocity and acceleration constraints when computing velocity commands. An overview of the algorithm can be found here: [DWPP Algorithm](https://github.com/Decwest/nav2_dynamic_window_pure_pursuit_controller/blob/main/algorithm.md). From de292dfa4faab9fa5329e5f32bf21ad2d8c85724 Mon Sep 17 00:00:00 2001 From: Decwest Date: Tue, 25 Nov 2025 15:43:56 +0900 Subject: [PATCH 22/29] :adhesive_bandage: dwpp defalut option set to false Signed-off-by: Decwest --- nav2_regulated_pure_pursuit_controller/README.md | 12 ++++++------ .../src/parameter_handler.cpp | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 3ec14873b80..fa278662ad1 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -74,13 +74,13 @@ A link to the paper and its citation will be provided once it becomes publicly a | Parameter | Description | |-----|----| | `max_linear_vel` | The maximum linear velocity to use. | -| `min_linear_vel` | The minimum linear velocity to use. | +| `min_linear_vel` | The minimum linear velocity used when `use_dynamic_window` is `true`. | | `max_angular_vel` | The maximum angular velocity to use. | -| `min_angular_vel` | The minimum angular velocity to use. | -| `max_linear_accel` | The maximum linear acceleration to use. | -| `max_linear_decel` | The maximum linear deceleration to use. | +| `min_angular_vel` | The minimum angular velocity used when `use_dynamic_window` is `true`.| +| `max_linear_accel` | The maximum linear acceleration used when `use_dynamic_window` is `true`.| +| `max_linear_decel` | The maximum linear deceleration used when `use_dynamic_window` is `true`. | | `max_angular_accel` | The maximum angular acceleration to use. | -| `max_angular_decel` | The maximum angular deceleration to use. | +| `max_angular_decel` | The maximum angular deceleration used when `use_dynamic_window` is `true`. | | `lookahead_dist` | The lookahead distance to use to find the lookahead point | | `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | | `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | @@ -167,7 +167,7 @@ controller_server: cost_scaling_gain: 1.0 inflation_cost_scaling_factor: 3.0 min_distance_to_obstacle: 0.0 - use_dynamic_window: true + use_dynamic_window: false velocity_feedback: "OPEN_LOOP" ``` diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index b97caf2a4ee..97501d19cb0 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -119,7 +119,7 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node, plugin_name_ + ".use_dynamic_window", rclcpp::ParameterValue(true)); + node, plugin_name_ + ".use_dynamic_window", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".velocity_feedback", rclcpp::ParameterValue(std::string("OPEN_LOOP"))); From d69470a700c39f9d0b1955c96589cdfb379cd2e4 Mon Sep 17 00:00:00 2001 From: Decwest Date: Tue, 25 Nov 2025 18:46:21 +0900 Subject: [PATCH 23/29] :adhesive_bandage: fix default values Signed-off-by: Decwest --- nav2_regulated_pure_pursuit_controller/README.md | 8 ++++---- .../src/parameter_handler.cpp | 14 +++++++------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index fa278662ad1..546b450e483 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -74,12 +74,12 @@ A link to the paper and its citation will be provided once it becomes publicly a | Parameter | Description | |-----|----| | `max_linear_vel` | The maximum linear velocity to use. | +| `max_angular_accel` | The maximum angular acceleration to use. | | `min_linear_vel` | The minimum linear velocity used when `use_dynamic_window` is `true`. | -| `max_angular_vel` | The maximum angular velocity to use. | -| `min_angular_vel` | The minimum angular velocity used when `use_dynamic_window` is `true`.| -| `max_linear_accel` | The maximum linear acceleration used when `use_dynamic_window` is `true`.| +| `max_angular_vel` | The maximum angular velocity used when `use_dynamic_window` is `true`. | +| `min_angular_vel` | The minimum angular velocity used when `use_dynamic_window` is `true`. | +| `max_linear_accel` | The maximum linear acceleration used when `use_dynamic_window` is `true`. | | `max_linear_decel` | The maximum linear deceleration used when `use_dynamic_window` is `true`. | -| `max_angular_accel` | The maximum angular acceleration to use. | | `max_angular_decel` | The maximum angular deceleration used when `use_dynamic_window` is `true`. | | `lookahead_dist` | The lookahead distance to use to find the lookahead point | | `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 97501d19cb0..9f3c1187678 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -39,11 +39,11 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".max_linear_vel", rclcpp::ParameterValue(0.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".min_linear_vel", rclcpp::ParameterValue(0.0)); + node, plugin_name_ + ".min_linear_vel", rclcpp::ParameterValue(-0.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(2.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".min_angular_vel", rclcpp::ParameterValue(-1.0)); + node, plugin_name_ + ".min_angular_vel", rclcpp::ParameterValue(-2.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( @@ -94,13 +94,13 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(0.5)); + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(0.5)); + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(2.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( From 469f8aa8096d7a62e274bb68a5aef1c4d734e21c Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 27 Nov 2025 15:30:12 +0900 Subject: [PATCH 24/29] :zap: add migration log of desired_linear_vel Signed-off-by: Decwest --- .../src/parameter_handler.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 9f3c1187678..dc6fe936662 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -36,8 +36,24 @@ ParameterHandler::ParameterHandler( { plugin_name_ = plugin_name; + // Declare max_linear_vel with backward compatibility + const std::string old_name = plugin_name_ + ".desired_linear_vel"; + const std::string new_name = plugin_name_ + ".max_linear_vel"; + node->declare_parameter(old_name, + std::numeric_limits::quiet_NaN()); + double old_val = std::numeric_limits::quiet_NaN(); + if (node->get_parameter(old_name, old_val) && + !std::isnan(old_val)) + { + RCLCPP_WARN( + logger_, + "Parameter '%s' is deprecated. Use '%s' instead.", + old_name.c_str(), new_name.c_str()); + } + double default_val = std::isnan(old_val) ? 0.5 : old_val; declare_parameter_if_not_declared( - node, plugin_name_ + ".max_linear_vel", rclcpp::ParameterValue(0.5)); + node, plugin_name_ + ".max_linear_vel", rclcpp::ParameterValue(default_val)); + declare_parameter_if_not_declared( node, plugin_name_ + ".min_linear_vel", rclcpp::ParameterValue(-0.5)); declare_parameter_if_not_declared( From c1fc5196da185c9867d3723b947e10418b9ef98d Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 27 Nov 2025 16:23:39 +0900 Subject: [PATCH 25/29] :fire: remove velocity feedback parameter Signed-off-by: Decwest --- .../parameter_handler.hpp | 1 - .../src/parameter_handler.cpp | 3 --- .../src/regulated_pure_pursuit_controller.cpp | 10 ++-------- 3 files changed, 2 insertions(+), 12 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index a9642f409f0..83bb8a66315 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -66,7 +66,6 @@ struct Parameters double transform_tolerance; bool stateful; bool use_dynamic_window; - std::string velocity_feedback; }; /** diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index dc6fe936662..849140401c0 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -136,8 +136,6 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_dynamic_window", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".velocity_feedback", rclcpp::ParameterValue(std::string("OPEN_LOOP"))); node->get_parameter(plugin_name_ + ".max_linear_vel", params_.max_linear_vel); params_.base_max_linear_vel = params_.max_linear_vel; @@ -229,7 +227,6 @@ ParameterHandler::ParameterHandler( params_.use_collision_detection); node->get_parameter(plugin_name_ + ".stateful", params_.stateful); node->get_parameter(plugin_name_ + ".use_dynamic_window", params_.use_dynamic_window); - node->get_parameter(plugin_name_ + ".velocity_feedback", params_.velocity_feedback); if (params_.inflation_cost_scaling_factor <= 0.0) { RCLCPP_WARN( diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 9949ab553d3..1700c882f9e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -500,14 +500,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // compute optimal path tracking velocity commands // considering velocity and acceleration constraints (DWPP) const double regulated_linear_vel = linear_vel; - geometry_msgs::msg::Twist current_speed; - if (params_->velocity_feedback == "CLOSED_LOOP") { - // using odom velocity as a current velocity (not recommended) - current_speed = speed; - } else { - // using last command velocity as a current velocity (recommended) - current_speed = last_command_velocity_; - } + // using last command velocity as a current velocity + const geometry_msgs::msg::Twist current_speed = last_command_velocity_; double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel; From 6c6db208289d0354b7ea96742f8259d5734b71e2 Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 27 Nov 2025 17:26:01 +0900 Subject: [PATCH 26/29] :recycle: remove implicit return using tuple Signed-off-by: Decwest --- .../regulated_pure_pursuit_controller.hpp | 26 +- .../src/regulated_pure_pursuit_controller.cpp | 142 +++++----- .../test/test_regulated_pp.cpp | 265 +++++++++--------- 3 files changed, 214 insertions(+), 219 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 5c05f95215b..b01daf6eaaf 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -89,12 +89,16 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window */ - void computeDynamicWindow( + std::tuple computeDynamicWindow( const geometry_msgs::msg::Twist & current_speed, - double & dynamic_window_max_linear_vel, - double & dynamic_window_min_linear_vel, - double & dynamic_window_max_angular_vel, - double & dynamic_window_min_angular_vel + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel ); /** @@ -103,10 +107,10 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window */ - void applyRegulationToDynamicWindow( + std::tuple applyRegulationToDynamicWindow( const double & regulated_linear_vel, - double & dynamic_window_max_linear_vel, - double & dynamic_window_min_linear_vel); + const double & dynamic_window_max_linear_vel, + const double & dynamic_window_min_linear_vel); /** * @brief Compute the optimal velocity to follow the path within the dynamic window @@ -119,15 +123,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param optimal_linear_vel Optimal linear velocity to follow the path under velocity and acceleration constraints * @param optimal_angular_vel Optimal angular velocity to follow the path under velocity and acceleration constraints */ - void computeOptimalVelocityWithinDynamicWindow( + std::tuple computeOptimalVelocityWithinDynamicWindow( const double & dynamic_window_max_linear_vel, const double & dynamic_window_min_linear_vel, const double & dynamic_window_max_angular_vel, const double & dynamic_window_min_angular_vel, const double & curvature, - const double & sign, - double & optimal_linear_vel, - double & optimal_angular_vel + const double & sign ); /** diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 1700c882f9e..578d78030eb 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -165,33 +165,26 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point) } } -void RegulatedPurePursuitController::computeDynamicWindow( +std::tuple RegulatedPurePursuitController::computeDynamicWindow( const geometry_msgs::msg::Twist & current_speed, - double & dynamic_window_max_linear_vel, - double & dynamic_window_min_linear_vel, - double & dynamic_window_max_angular_vel, - double & dynamic_window_min_angular_vel + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel ) { - const double & max_linear_vel = params_->max_linear_vel; - const double & min_linear_vel = params_->min_linear_vel; - const double & max_angular_vel = params_->max_angular_vel; - const double & min_angular_vel = params_->min_angular_vel; - - const double & max_linear_accel = params_->max_linear_accel; - const double & max_linear_decel = params_->max_linear_decel; - const double & max_angular_accel = params_->max_angular_accel; - const double & max_angular_decel = params_->max_angular_decel; const double & dt = control_duration_; - constexpr double Eps = 1e-2; + constexpr double Eps = 1e-3; // function to compute dynamic window for a single dimension auto compute_window = [&](const double & current_vel, const double & max_vel, - const double & min_vel, - const double & max_accel, const double & max_decel, - double & dynamic_window_max_vel, double & dynamic_window_min_vel) + const double & min_vel, const double & max_accel, const double & max_decel) { double candidate_max_vel = 0.0; double candidate_min_vel = 0.0; @@ -211,75 +204,79 @@ void RegulatedPurePursuitController::computeDynamicWindow( } // clip to max/min velocity limits - dynamic_window_max_vel = std::min(candidate_max_vel, max_vel); - dynamic_window_min_vel = std::max(candidate_min_vel, min_vel); + double dynamic_window_max_vel = std::min(candidate_max_vel, max_vel); + double dynamic_window_min_vel = std::max(candidate_min_vel, min_vel); + return std::make_tuple(dynamic_window_max_vel, dynamic_window_min_vel); }; // linear velocity - compute_window(current_speed.linear.x, + auto [dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel] = compute_window(current_speed.linear.x, max_linear_vel, min_linear_vel, - max_linear_accel, max_linear_decel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); + max_linear_accel, max_linear_decel); // angular velocity - compute_window(current_speed.angular.z, + auto [dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel] = compute_window(current_speed.angular.z, max_angular_vel, min_angular_vel, - max_angular_accel, max_angular_decel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel); + max_angular_accel, max_angular_decel); + + return std::make_tuple(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); } -void RegulatedPurePursuitController::applyRegulationToDynamicWindow( +std::tuple RegulatedPurePursuitController::applyRegulationToDynamicWindow( const double & regulated_linear_vel, - double & dynamic_window_max_linear_vel, - double & dynamic_window_min_linear_vel) + const double & dynamic_window_max_linear_vel, + const double & dynamic_window_min_linear_vel) { + double regulated_dynamic_window_max_linear_vel; + double regulated_dynamic_window_min_linear_vel; + // Extract the portion of the dynamic window that lies within the range [0, regulated_linear_vel] - double dynamic_window_max_linear_vel_temp; - double dynamic_window_min_linear_vel_temp; if (regulated_linear_vel >= 0.0) { - dynamic_window_max_linear_vel_temp = std::min( + regulated_dynamic_window_max_linear_vel = std::min( dynamic_window_max_linear_vel, regulated_linear_vel); - dynamic_window_min_linear_vel_temp = std::max( + regulated_dynamic_window_min_linear_vel = std::max( dynamic_window_min_linear_vel, 0.0); } else { - dynamic_window_max_linear_vel_temp = std::min( + regulated_dynamic_window_max_linear_vel = std::min( dynamic_window_max_linear_vel, 0.0); - dynamic_window_min_linear_vel_temp = std::max( + regulated_dynamic_window_min_linear_vel = std::max( dynamic_window_min_linear_vel, regulated_linear_vel); } - if (dynamic_window_max_linear_vel_temp >= dynamic_window_min_linear_vel_temp) { - dynamic_window_max_linear_vel = dynamic_window_max_linear_vel_temp; - dynamic_window_min_linear_vel = dynamic_window_min_linear_vel_temp; - } else { + if (regulated_dynamic_window_max_linear_vel < regulated_dynamic_window_min_linear_vel) { // No valid portion of the dynamic window remains after applying the regulation - if (dynamic_window_min_linear_vel > 0.0) { + if (regulated_dynamic_window_min_linear_vel > 0.0) { // If the dynamic window is entirely in the positive range, // collapse both bounds to dynamic_window_min_linear_vel - dynamic_window_max_linear_vel = dynamic_window_min_linear_vel; + regulated_dynamic_window_max_linear_vel = regulated_dynamic_window_min_linear_vel; } else { // If the dynamic window is entirely in the negative range, // collapse both bounds to dynamic_window_max_linear_vel - dynamic_window_min_linear_vel = dynamic_window_max_linear_vel; + regulated_dynamic_window_min_linear_vel = regulated_dynamic_window_max_linear_vel; } } - return; + return std::make_tuple( + regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel); } -void RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( +std::tuple RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( const double & dynamic_window_max_linear_vel, const double & dynamic_window_min_linear_vel, const double & dynamic_window_max_angular_vel, const double & dynamic_window_min_angular_vel, const double & curvature, - const double & sign, - double & optimal_linear_vel, - double & optimal_angular_vel -) + const double & sign + ) { + double optimal_linear_vel; + double optimal_angular_vel; + // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) // Select the closest point to the line // angular_vel = curvature * linear_vel within the dynamic window. @@ -301,14 +298,14 @@ void RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( if (dynamic_window_min_angular_vel <= 0.0 && 0.0 <= dynamic_window_max_angular_vel) { optimal_angular_vel = 0.0; } else { - // If not, select angular vel within dynamic window closest to 0 + // If not, select angular vel within dynamic window closest to 0 if (std::abs(dynamic_window_min_angular_vel) <= std::abs(dynamic_window_max_angular_vel)) { optimal_angular_vel = dynamic_window_min_angular_vel; } else { optimal_angular_vel = dynamic_window_max_angular_vel; } } - return; + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); } // When the dynamic window and the line angular_vel = curvature * linear_vel intersect, @@ -344,7 +341,7 @@ void RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( if (best_linear_vel != -std::numeric_limits::infinity() * sign) { optimal_linear_vel = best_linear_vel; optimal_angular_vel = best_angular_vel; - return; + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); } // When the dynamic window and the line angular_vel = curvature * linear_vel have no intersection, @@ -385,6 +382,8 @@ void RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( optimal_linear_vel = best_linear_vel; optimal_angular_vel = best_angular_vel; + + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); } geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( @@ -498,37 +497,44 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity angular_vel = linear_vel * regulation_curvature; } else { // compute optimal path tracking velocity commands + // considering velocity and acceleration constraints (DWPP) const double regulated_linear_vel = linear_vel; + // using last command velocity as a current velocity const geometry_msgs::msg::Twist current_speed = last_command_velocity_; - double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel; + + const double & max_linear_vel = params_->max_linear_vel; + const double & min_linear_vel = params_->min_linear_vel; + const double & max_angular_vel = params_->max_angular_vel; + const double & min_angular_vel = params_->min_angular_vel; + + const double & max_linear_accel = params_->max_linear_accel; + const double & max_linear_decel = params_->max_linear_decel; + const double & max_angular_accel = params_->max_angular_accel; + const double & max_angular_decel = params_->max_angular_decel; // compute Dynamic Window - computeDynamicWindow( - current_speed, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel); + auto [dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel] = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel); // apply regulation to Dynamic Window - applyRegulationToDynamicWindow( + auto [regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel] = applyRegulationToDynamicWindow( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); // compute optimal velocity within Dynamic Window - computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel, + std::tie(linear_vel, angular_vel) = computeOptimalVelocityWithinDynamicWindow( + regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel, regulation_curvature, - x_vel_sign, - linear_vel, - angular_vel + x_vel_sign ); } } diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index b3e563dd5fd..a2cf32fdfe2 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -92,22 +92,26 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); } - void computeDynamicWindowWrapper( + std::tuple computeDynamicWindowWrapper( const geometry_msgs::msg::Twist & current_speed, - double & dynamic_window_max_linear_vel, - double & dynamic_window_min_linear_vel, - double & dynamic_window_max_angular_vel, - double & dynamic_window_min_angular_vel) + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel) { return computeDynamicWindow(current_speed, - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel); + max_linear_vel, min_linear_vel, max_angular_vel, + min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel); } - void applyRegulationToDynamicWindowWrapper( + std::tuple applyRegulationToDynamicWindowWrapper( const double & regulated_linear_vel, - double & dynamic_window_max_linear_vel, - double & dynamic_window_min_linear_vel) + const double & dynamic_window_max_linear_vel, + const double & dynamic_window_min_linear_vel) { return applyRegulationToDynamicWindow( regulated_linear_vel, @@ -115,20 +119,17 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure dynamic_window_min_linear_vel); } - void computeOptimalVelocityWithinDynamicWindowWrapper( + std::tuple computeOptimalVelocityWithinDynamicWindowWrapper( const double & dynamic_window_max_linear_vel, const double & dynamic_window_min_linear_vel, const double & dynamic_window_max_angular_vel, const double & dynamic_window_min_angular_vel, const double & curvature, - const double & sign, - double & optimal_linear_vel, - double & optimal_angular_vel) + const double & sign) { return computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign, - optimal_linear_vel, optimal_angular_vel); + dynamic_window_min_angular_vel, curvature, sign); } }; @@ -464,56 +465,15 @@ TEST(RegulatedPurePursuitTest, applyConstraints) TEST(RegulatedPurePursuitTest, computeDynamicWindow) { auto ctrl = std::make_shared(); - auto node = std::make_shared("testRPP"); - std::string name = "PathFollower"; - auto tf = std::make_shared(node->get_clock()); - auto costmap = std::make_shared("fake_costmap"); - rclcpp_lifecycle::State state; - costmap->on_configure(state); - // declare parameters - constexpr double max_linear_vel = 0.5; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_linear_vel", - rclcpp::ParameterValue(max_linear_vel)); - constexpr double min_linear_vel = -0.5; - nav2::declare_parameter_if_not_declared( - node, - name + ".min_linear_vel", - rclcpp::ParameterValue(min_linear_vel)); - constexpr double max_angular_vel = 1.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_angular_vel", - rclcpp::ParameterValue(max_angular_vel)); - constexpr double min_angular_vel = -1.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".min_angular_vel", - rclcpp::ParameterValue(min_angular_vel)); - constexpr double max_linear_accel = 0.5; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_linear_accel", - rclcpp::ParameterValue(max_linear_accel)); - constexpr double max_linear_decel = 1.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_linear_decel", - rclcpp::ParameterValue(max_linear_decel)); - constexpr double max_angular_accel = 1.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_angular_accel", - rclcpp::ParameterValue(max_angular_accel)); - constexpr double max_angular_decel = 2.0; - nav2::declare_parameter_if_not_declared( - node, - name + ".max_angular_decel", - rclcpp::ParameterValue(max_angular_decel)); - - ctrl->configure(node, name, tf, costmap); + double max_linear_vel = 0.5; + double min_linear_vel = -0.5; + double max_angular_vel = 1.0; + double min_angular_vel = -1.0; + double max_linear_accel = 0.5; + double max_linear_decel = 1.0; + double max_angular_accel = 1.0; + double max_angular_decel = 2.0; double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel; @@ -523,10 +483,13 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) // clip by max linear vel current_speed.linear.x = 0.5; current_speed.angular.z = 0.2; - ctrl->computeDynamicWindowWrapper( - current_speed, - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel); EXPECT_EQ(dynamic_window_max_linear_vel, 0.5); EXPECT_EQ(dynamic_window_min_linear_vel, 0.45); @@ -536,10 +499,13 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) // case2: current linear velocity is positive, angular velocity is negative current_speed.linear.x = 0.3; current_speed.angular.z = -0.2; - ctrl->computeDynamicWindowWrapper( - current_speed, - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel); EXPECT_EQ(dynamic_window_max_linear_vel, 0.325); EXPECT_EQ(dynamic_window_min_linear_vel, 0.25); @@ -549,10 +515,13 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) // case3: current linear velocity is zero, angular velocity is zero current_speed.linear.x = 0.0; current_speed.angular.z = -0.0; - ctrl->computeDynamicWindowWrapper( - current_speed, - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel); EXPECT_EQ(dynamic_window_max_linear_vel, 0.025); EXPECT_EQ(dynamic_window_min_linear_vel, -0.025); @@ -562,10 +531,13 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) // case4: current linear velocity is negative, angular velocity is positive current_speed.linear.x = -0.3; current_speed.angular.z = 0.2; - ctrl->computeDynamicWindowWrapper( - current_speed, - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel); EXPECT_EQ(dynamic_window_max_linear_vel, -0.25); EXPECT_EQ(dynamic_window_min_linear_vel, -0.325); @@ -576,10 +548,13 @@ TEST(RegulatedPurePursuitTest, computeDynamicWindow) // clipped by min lenear vel current_speed.linear.x = -0.5; current_speed.angular.z = -0.2; - ctrl->computeDynamicWindowWrapper( - current_speed, - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel); EXPECT_EQ(dynamic_window_max_linear_vel, -0.45); EXPECT_EQ(dynamic_window_min_linear_vel, -0.5); @@ -594,107 +569,119 @@ TEST(RegulatedPurePursuitTest, applyRegulationToDynamicWindow) double regulated_linear_vel; double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel; + double regulated_dynamic_window_max_linear_vel, regulated_dynamic_window_min_linear_vel; + regulated_linear_vel = 0.3; dynamic_window_max_linear_vel = 0.5, dynamic_window_min_linear_vel = 0.4; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.4); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.4); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.4); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.4); dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.2; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.3); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.2); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.3); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.2); dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.1; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.2); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.1); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.2); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.1); dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.1); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.0); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.1); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.0); dynamic_window_max_linear_vel = -0.1, dynamic_window_min_linear_vel = -0.3; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, -0.1); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.1); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.1); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.1); regulated_linear_vel = -0.3; dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.1); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.1); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.1); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.1); dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.0); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.2); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.0); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.2); dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.3; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, -0.2); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.3); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.2); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.3); dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.4; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, -0.2); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.3); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.2); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.3); dynamic_window_max_linear_vel = -0.4, dynamic_window_min_linear_vel = -0.5; - ctrl->applyRegulationToDynamicWindowWrapper( + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); - EXPECT_EQ(dynamic_window_max_linear_vel, -0.4); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.4); + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.4); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.4); } TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) @@ -708,10 +695,10 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) double optimal_linear_vel; double optimal_angular_vel; - ctrl->computeOptimalVelocityWithinDynamicWindow( + std::tie(optimal_linear_vel, + optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign, - optimal_linear_vel, optimal_angular_vel); + dynamic_window_min_angular_vel, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.3); EXPECT_EQ(optimal_angular_vel, 0.0); @@ -720,10 +707,10 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - ctrl->computeOptimalVelocityWithinDynamicWindow( + std::tie(optimal_linear_vel, + optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign, - optimal_linear_vel, optimal_angular_vel); + dynamic_window_min_angular_vel, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.3); EXPECT_EQ(optimal_angular_vel, 0.1); @@ -732,10 +719,10 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - ctrl->computeOptimalVelocityWithinDynamicWindow( + std::tie(optimal_linear_vel, + optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign, - optimal_linear_vel, optimal_angular_vel); + dynamic_window_min_angular_vel, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.2); EXPECT_EQ(optimal_angular_vel, 0.2); @@ -743,10 +730,10 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; - ctrl->computeOptimalVelocityWithinDynamicWindow( + std::tie(optimal_linear_vel, + optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign, - optimal_linear_vel, optimal_angular_vel); + dynamic_window_min_angular_vel, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.3); EXPECT_EQ(optimal_angular_vel, 0.2); @@ -756,10 +743,10 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; dynamic_window_max_angular_vel = 0.1, dynamic_window_min_angular_vel = -0.1; - ctrl->computeOptimalVelocityWithinDynamicWindow( + std::tie(optimal_linear_vel, + optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign, - optimal_linear_vel, optimal_angular_vel); + dynamic_window_min_angular_vel, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.1); EXPECT_EQ(optimal_angular_vel, 0.0); @@ -767,10 +754,10 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - ctrl->computeOptimalVelocityWithinDynamicWindow( + std::tie(optimal_linear_vel, + optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign, - optimal_linear_vel, optimal_angular_vel); + dynamic_window_min_angular_vel, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.1); EXPECT_EQ(optimal_angular_vel, 0.1); @@ -779,10 +766,10 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - ctrl->computeOptimalVelocityWithinDynamicWindow( + std::tie(optimal_linear_vel, + optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign, - optimal_linear_vel, optimal_angular_vel); + dynamic_window_min_angular_vel, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.1); EXPECT_EQ(optimal_angular_vel, 0.1); @@ -790,10 +777,10 @@ TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; - ctrl->computeOptimalVelocityWithinDynamicWindow( + std::tie(optimal_linear_vel, + optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign, - optimal_linear_vel, optimal_angular_vel); + dynamic_window_min_angular_vel, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.3); EXPECT_EQ(optimal_angular_vel, 0.2); From 735276c38acb591ad7eacbcedf1e56ad41a37016 Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 27 Nov 2025 19:32:06 +0900 Subject: [PATCH 27/29] :recycle: move dwpp functions to utils Signed-off-by: Decwest --- .../dynamic_window_pure_pursuit_functions.hpp | 290 ++++++++++++++ .../regulated_pure_pursuit_controller.hpp | 52 +-- .../src/parameter_handler.cpp | 7 +- .../src/regulated_pure_pursuit_controller.cpp | 235 +---------- .../test/CMakeLists.txt | 1 + .../test/test_dwpp_functions.cpp | 338 ++++++++++++++++ .../test/test_regulated_pp.cpp | 364 ------------------ 7 files changed, 643 insertions(+), 644 deletions(-) create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp new file mode 100644 index 00000000000..d937c57863d --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp @@ -0,0 +1,290 @@ +// Copyright (c) 2025 Fumiya Ohnishi +// +// 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. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/twist.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +namespace dynamic_window_pure_pursuit +{ + +struct DynamicWindowBounds +{ + double dynamic_window_max_linear_vel; + double dynamic_window_min_linear_vel; + double dynamic_window_max_angular_vel; + double dynamic_window_min_angular_vel; +}; + +/** + * @brief Compute the dynamic window (feasible velocity bounds) based on the current speed and the given velocity and acceleration constraints. + * @param current_speed Current linear and angular velocity of the robot + * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window + * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window + * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window + * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window + */ +inline std::tuple computeDynamicWindow( + const geometry_msgs::msg::Twist & current_speed, + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel, + const double & dt +) +{ + constexpr double Eps = 1e-3; + + // function to compute dynamic window for a single dimension + auto compute_window = [&](const double & current_vel, const double & max_vel, + const double & min_vel, const double & max_accel, const double & max_decel) + { + double candidate_max_vel = 0.0; + double candidate_min_vel = 0.0; + + if (current_vel > Eps) { + // if the current velocity is positive, acceleration means an increase in speed + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel - max_decel * dt; + } else if (current_vel < -Eps) { + // if the current velocity is negative, acceleration means a decrease in speed + candidate_max_vel = current_vel + max_decel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } else { + // if the current velocity is zero, allow acceleration in both directions. + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } + + // clip to max/min velocity limits + double dynamic_window_max_vel = std::min(candidate_max_vel, max_vel); + double dynamic_window_min_vel = std::max(candidate_min_vel, min_vel); + return std::make_tuple(dynamic_window_max_vel, dynamic_window_min_vel); + }; + + // linear velocity + auto [dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel] = compute_window(current_speed.linear.x, + max_linear_vel, min_linear_vel, + max_linear_accel, max_linear_decel); + + // angular velocity + auto [dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel] = compute_window(current_speed.angular.z, + max_angular_vel, min_angular_vel, + max_angular_accel, max_angular_decel); + + return std::make_tuple(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); +} + +/** + * @brief Apply regulated linear velocity to the dynamic window + * @param regulated_linear_vel Regulated linear velocity + * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window + * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window + */ +inline std::tuple applyRegulationToDynamicWindow( + const double & regulated_linear_vel, + const double & dynamic_window_max_linear_vel, + const double & dynamic_window_min_linear_vel) +{ + double regulated_dynamic_window_max_linear_vel; + double regulated_dynamic_window_min_linear_vel; + + // Extract the portion of the dynamic window that lies within the range [0, regulated_linear_vel] + if (regulated_linear_vel >= 0.0) { + regulated_dynamic_window_max_linear_vel = std::min( + dynamic_window_max_linear_vel, regulated_linear_vel); + regulated_dynamic_window_min_linear_vel = std::max( + dynamic_window_min_linear_vel, 0.0); + } else { + regulated_dynamic_window_max_linear_vel = std::min( + dynamic_window_max_linear_vel, 0.0); + regulated_dynamic_window_min_linear_vel = std::max( + dynamic_window_min_linear_vel, regulated_linear_vel); + } + + if (regulated_dynamic_window_max_linear_vel < regulated_dynamic_window_min_linear_vel) { + // No valid portion of the dynamic window remains after applying the regulation + if (regulated_dynamic_window_min_linear_vel > 0.0) { + // If the dynamic window is entirely in the positive range, + // collapse both bounds to dynamic_window_min_linear_vel + regulated_dynamic_window_max_linear_vel = regulated_dynamic_window_min_linear_vel; + } else { + // If the dynamic window is entirely in the negative range, + // collapse both bounds to dynamic_window_max_linear_vel + regulated_dynamic_window_min_linear_vel = regulated_dynamic_window_max_linear_vel; + } + } + + return std::make_tuple( + regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel); +} + + +/** + * @brief Compute the optimal velocity to follow the path within the dynamic window + * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window + * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window + * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window + * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window + * @param curvature Curvature of the path to follow + * @param sign Velocity sign (forward or backward) + * @param optimal_linear_vel Optimal linear velocity to follow the path under velocity and acceleration constraints + * @param optimal_angular_vel Optimal angular velocity to follow the path under velocity and acceleration constraints + */ +inline std::tuple computeOptimalVelocityWithinDynamicWindow( + const double & dynamic_window_max_linear_vel, + const double & dynamic_window_min_linear_vel, + const double & dynamic_window_max_angular_vel, + const double & dynamic_window_min_angular_vel, + const double & curvature, + const double & sign +) +{ + double optimal_linear_vel; + double optimal_angular_vel; + + // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) + // Select the closest point to the line + // angular_vel = curvature * linear_vel within the dynamic window. + // If multiple points are equally close, select the one with the largest linear_vel. + + // When curvature == 0, the line is angular_vel = 0 + if (abs(curvature) < 1e-3) { + // linear velocity + if (sign >= 0.0) { + // If moving forward, select the max linear vel + optimal_linear_vel = dynamic_window_max_linear_vel; + } else { + // If moving backward, select the min linear vel + optimal_linear_vel = dynamic_window_min_linear_vel; + } + + // angular velocity + // If the line angular_vel = 0 intersects the dynamic window,angular_vel = 0.0 + if (dynamic_window_min_angular_vel <= 0.0 && 0.0 <= dynamic_window_max_angular_vel) { + optimal_angular_vel = 0.0; + } else { + // If not, select angular vel within dynamic window closest to 0 + if (std::abs(dynamic_window_min_angular_vel) <= std::abs(dynamic_window_max_angular_vel)) { + optimal_angular_vel = dynamic_window_min_angular_vel; + } else { + optimal_angular_vel = dynamic_window_max_angular_vel; + } + } + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); + } + + // When the dynamic window and the line angular_vel = curvature * linear_vel intersect, + // select the intersection point that yields the highest linear velocity. + + // List the four candidate intersection points + std::pair candidates[] = { + {dynamic_window_min_linear_vel, curvature * dynamic_window_min_linear_vel}, + {dynamic_window_max_linear_vel, curvature * dynamic_window_max_linear_vel}, + {dynamic_window_min_angular_vel / curvature, dynamic_window_min_angular_vel}, + {dynamic_window_max_angular_vel / curvature, dynamic_window_max_angular_vel} + }; + + double best_linear_vel = -std::numeric_limits::infinity() * sign; + double best_angular_vel = 0.0; + + for (auto [linear_vel, angular_vel] : candidates) { + // Check whether the candidate lies within the dynamic window + if (linear_vel >= dynamic_window_min_linear_vel && + linear_vel <= dynamic_window_max_linear_vel && + angular_vel >= dynamic_window_min_angular_vel && + angular_vel <= dynamic_window_max_angular_vel) + { + // Select the candidate with the largest linear velocity (considering moving direction) + if (linear_vel * sign > best_linear_vel * sign) { + best_linear_vel = linear_vel; + best_angular_vel = angular_vel; + } + } + } + + // If best_linear_vel was updated, it means that a valid intersection exists + if (best_linear_vel != -std::numeric_limits::infinity() * sign) { + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); + } + + // When the dynamic window and the line angular_vel = curvature * linear_vel have no intersection, + // select the point within the dynamic window that is closest to the line. + + // Because the dynamic window is a convex region, + // the closest point must be one of its four corners. + const std::array, 4> corners = {{ + {dynamic_window_min_linear_vel, dynamic_window_min_angular_vel}, + {dynamic_window_min_linear_vel, dynamic_window_max_angular_vel}, + {dynamic_window_max_linear_vel, dynamic_window_min_angular_vel}, + {dynamic_window_max_linear_vel, dynamic_window_max_angular_vel} + }}; + + // Compute the distance from a point (linear_vel, angular_vel) + // to the line angular_vel = curvature * linear_vel + const double denom = std::sqrt(curvature * curvature + 1.0); + auto compute_dist = [&](const std::array & corner) -> double { + return std::abs(curvature * corner[0] - corner[1]) / denom; + }; + + double closest_dist = std::numeric_limits::infinity(); + best_linear_vel = -std::numeric_limits::infinity() * sign; + best_angular_vel = 0.0; + + for (const auto & corner : corners) { + const double dist = compute_dist(corner); + // Update if this corner is closer to the line, + // or equally close but has a larger linear velocity (considering moving direction) + if (dist < closest_dist || + (std::abs(dist - closest_dist) <= 1e-3 && corner[0] * sign > best_linear_vel * sign)) + { + closest_dist = dist; + best_linear_vel = corner[0]; + best_angular_vel = corner[1]; + } + } + + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; + + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); +} + +} // namespace dynamic_window_pure_pursuit + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index b01daf6eaaf..335996aeb68 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -33,6 +33,7 @@ #include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" #include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" #include "nav2_regulated_pure_pursuit_controller/regulation_functions.hpp" +#include "nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp" namespace nav2_regulated_pure_pursuit_controller { @@ -81,57 +82,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ void deactivate() override; - /** - * @brief Compute the dynamic window (feasible velocity bounds) based on the current speed and the given velocity and acceleration constraints. - * @param current_speed Current linear and angular velocity of the robot - * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window - * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window - * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window - * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window - */ - std::tuple computeDynamicWindow( - const geometry_msgs::msg::Twist & current_speed, - const double & max_linear_vel, - const double & min_linear_vel, - const double & max_angular_vel, - const double & min_angular_vel, - const double & max_linear_accel, - const double & max_linear_decel, - const double & max_angular_accel, - const double & max_angular_decel - ); - - /** - * @brief Apply regulated linear velocity to the dynamic window - * @param regulated_linear_vel Regulated linear velocity - * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window - * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window - */ - std::tuple applyRegulationToDynamicWindow( - const double & regulated_linear_vel, - const double & dynamic_window_max_linear_vel, - const double & dynamic_window_min_linear_vel); - - /** - * @brief Compute the optimal velocity to follow the path within the dynamic window - * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window - * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window - * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window - * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window - * @param curvature Curvature of the path to follow - * @param sign Velocity sign (forward or backward) - * @param optimal_linear_vel Optimal linear velocity to follow the path under velocity and acceleration constraints - * @param optimal_angular_vel Optimal angular velocity to follow the path under velocity and acceleration constraints - */ - std::tuple computeOptimalVelocityWithinDynamicWindow( - const double & dynamic_window_max_linear_vel, - const double & dynamic_window_min_linear_vel, - const double & dynamic_window_max_angular_vel, - const double & dynamic_window_min_angular_vel, - const double & curvature, - const double & sign - ); - /** * @brief Compute the best command given the current pose and velocity, with possible debug information * diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 849140401c0..1f7d4cf94a5 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -39,9 +39,10 @@ ParameterHandler::ParameterHandler( // Declare max_linear_vel with backward compatibility const std::string old_name = plugin_name_ + ".desired_linear_vel"; const std::string new_name = plugin_name_ + ".max_linear_vel"; - node->declare_parameter(old_name, - std::numeric_limits::quiet_NaN()); - double old_val = std::numeric_limits::quiet_NaN(); + const auto nan_val = std::numeric_limits::quiet_NaN(); + declare_parameter_if_not_declared( + node, old_name, rclcpp::ParameterValue(nan_val)); + double old_val = nan_val; if (node->get_parameter(old_name, old_val) && !std::isnan(old_val)) { diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 578d78030eb..de40e691823 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -165,227 +165,6 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point) } } -std::tuple RegulatedPurePursuitController::computeDynamicWindow( - const geometry_msgs::msg::Twist & current_speed, - const double & max_linear_vel, - const double & min_linear_vel, - const double & max_angular_vel, - const double & min_angular_vel, - const double & max_linear_accel, - const double & max_linear_decel, - const double & max_angular_accel, - const double & max_angular_decel -) -{ - - const double & dt = control_duration_; - - constexpr double Eps = 1e-3; - - // function to compute dynamic window for a single dimension - auto compute_window = [&](const double & current_vel, const double & max_vel, - const double & min_vel, const double & max_accel, const double & max_decel) - { - double candidate_max_vel = 0.0; - double candidate_min_vel = 0.0; - - if (current_vel > Eps) { - // if the current velocity is positive, acceleration means an increase in speed - candidate_max_vel = current_vel + max_accel * dt; - candidate_min_vel = current_vel - max_decel * dt; - } else if (current_vel < -Eps) { - // if the current velocity is negative, acceleration means a decrease in speed - candidate_max_vel = current_vel + max_decel * dt; - candidate_min_vel = current_vel - max_accel * dt; - } else { - // if the current velocity is zero, allow acceleration in both directions. - candidate_max_vel = current_vel + max_accel * dt; - candidate_min_vel = current_vel - max_accel * dt; - } - - // clip to max/min velocity limits - double dynamic_window_max_vel = std::min(candidate_max_vel, max_vel); - double dynamic_window_min_vel = std::max(candidate_min_vel, min_vel); - return std::make_tuple(dynamic_window_max_vel, dynamic_window_min_vel); - }; - - // linear velocity - auto [dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel] = compute_window(current_speed.linear.x, - max_linear_vel, min_linear_vel, - max_linear_accel, max_linear_decel); - - // angular velocity - auto [dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel] = compute_window(current_speed.angular.z, - max_angular_vel, min_angular_vel, - max_angular_accel, max_angular_decel); - - return std::make_tuple(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); -} - -std::tuple RegulatedPurePursuitController::applyRegulationToDynamicWindow( - const double & regulated_linear_vel, - const double & dynamic_window_max_linear_vel, - const double & dynamic_window_min_linear_vel) -{ - double regulated_dynamic_window_max_linear_vel; - double regulated_dynamic_window_min_linear_vel; - - // Extract the portion of the dynamic window that lies within the range [0, regulated_linear_vel] - if (regulated_linear_vel >= 0.0) { - regulated_dynamic_window_max_linear_vel = std::min( - dynamic_window_max_linear_vel, regulated_linear_vel); - regulated_dynamic_window_min_linear_vel = std::max( - dynamic_window_min_linear_vel, 0.0); - } else { - regulated_dynamic_window_max_linear_vel = std::min( - dynamic_window_max_linear_vel, 0.0); - regulated_dynamic_window_min_linear_vel = std::max( - dynamic_window_min_linear_vel, regulated_linear_vel); - } - - if (regulated_dynamic_window_max_linear_vel < regulated_dynamic_window_min_linear_vel) { - // No valid portion of the dynamic window remains after applying the regulation - if (regulated_dynamic_window_min_linear_vel > 0.0) { - // If the dynamic window is entirely in the positive range, - // collapse both bounds to dynamic_window_min_linear_vel - regulated_dynamic_window_max_linear_vel = regulated_dynamic_window_min_linear_vel; - } else { - // If the dynamic window is entirely in the negative range, - // collapse both bounds to dynamic_window_max_linear_vel - regulated_dynamic_window_min_linear_vel = regulated_dynamic_window_max_linear_vel; - } - } - - return std::make_tuple( - regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel); -} - -std::tuple RegulatedPurePursuitController::computeOptimalVelocityWithinDynamicWindow( - const double & dynamic_window_max_linear_vel, - const double & dynamic_window_min_linear_vel, - const double & dynamic_window_max_angular_vel, - const double & dynamic_window_min_angular_vel, - const double & curvature, - const double & sign - ) -{ - double optimal_linear_vel; - double optimal_angular_vel; - - // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) - // Select the closest point to the line - // angular_vel = curvature * linear_vel within the dynamic window. - // If multiple points are equally close, select the one with the largest linear_vel. - - // When curvature == 0, the line is angular_vel = 0 - if (abs(curvature) < 1e-3) { - // linear velocity - if (sign >= 0.0) { - // If moving forward, select the max linear vel - optimal_linear_vel = dynamic_window_max_linear_vel; - } else { - // If moving backward, select the min linear vel - optimal_linear_vel = dynamic_window_min_linear_vel; - } - - // angular velocity - // If the line angular_vel = 0 intersects the dynamic window,angular_vel = 0.0 - if (dynamic_window_min_angular_vel <= 0.0 && 0.0 <= dynamic_window_max_angular_vel) { - optimal_angular_vel = 0.0; - } else { - // If not, select angular vel within dynamic window closest to 0 - if (std::abs(dynamic_window_min_angular_vel) <= std::abs(dynamic_window_max_angular_vel)) { - optimal_angular_vel = dynamic_window_min_angular_vel; - } else { - optimal_angular_vel = dynamic_window_max_angular_vel; - } - } - return std::make_tuple(optimal_linear_vel, optimal_angular_vel); - } - - // When the dynamic window and the line angular_vel = curvature * linear_vel intersect, - // select the intersection point that yields the highest linear velocity. - - // List the four candidate intersection points - std::pair candidates[] = { - {dynamic_window_min_linear_vel, curvature * dynamic_window_min_linear_vel}, - {dynamic_window_max_linear_vel, curvature * dynamic_window_max_linear_vel}, - {dynamic_window_min_angular_vel / curvature, dynamic_window_min_angular_vel}, - {dynamic_window_max_angular_vel / curvature, dynamic_window_max_angular_vel} - }; - - double best_linear_vel = -std::numeric_limits::infinity() * sign; - double best_angular_vel = 0.0; - - for (auto [linear_vel, angular_vel] : candidates) { - // Check whether the candidate lies within the dynamic window - if (linear_vel >= dynamic_window_min_linear_vel && - linear_vel <= dynamic_window_max_linear_vel && - angular_vel >= dynamic_window_min_angular_vel && - angular_vel <= dynamic_window_max_angular_vel) - { - // Select the candidate with the largest linear velocity (considering moving direction) - if (linear_vel * sign > best_linear_vel * sign) { - best_linear_vel = linear_vel; - best_angular_vel = angular_vel; - } - } - } - - // If best_linear_vel was updated, it means that a valid intersection exists - if (best_linear_vel != -std::numeric_limits::infinity() * sign) { - optimal_linear_vel = best_linear_vel; - optimal_angular_vel = best_angular_vel; - return std::make_tuple(optimal_linear_vel, optimal_angular_vel); - } - - // When the dynamic window and the line angular_vel = curvature * linear_vel have no intersection, - // select the point within the dynamic window that is closest to the line. - - // Because the dynamic window is a convex region, - // the closest point must be one of its four corners. - const std::array, 4> corners = {{ - {dynamic_window_min_linear_vel, dynamic_window_min_angular_vel}, - {dynamic_window_min_linear_vel, dynamic_window_max_angular_vel}, - {dynamic_window_max_linear_vel, dynamic_window_min_angular_vel}, - {dynamic_window_max_linear_vel, dynamic_window_max_angular_vel} - }}; - - // Compute the distance from a point (linear_vel, angular_vel) - // to the line angular_vel = curvature * linear_vel - const double denom = std::sqrt(curvature * curvature + 1.0); - auto compute_dist = [&](const std::array & corner) -> double { - return std::abs(curvature * corner[0] - corner[1]) / denom; - }; - - double closest_dist = std::numeric_limits::infinity(); - best_linear_vel = -std::numeric_limits::infinity() * sign; - best_angular_vel = 0.0; - - for (const auto & corner : corners) { - const double dist = compute_dist(corner); - // Update if this corner is closer to the line, - // or equally close but has a larger linear velocity (considering moving direction) - if (dist < closest_dist || - (std::abs(dist - closest_dist) <= 1e-3 && corner[0] * sign > best_linear_vel * sign)) - { - closest_dist = dist; - best_linear_vel = corner[0]; - best_angular_vel = corner[1]; - } - } - - optimal_linear_vel = best_linear_vel; - optimal_angular_vel = best_angular_vel; - - return std::make_tuple(optimal_linear_vel, optimal_angular_vel); -} - geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & speed, @@ -516,26 +295,30 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // compute Dynamic Window auto [dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel] = computeDynamicWindow( + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel] = dynamic_window_pure_pursuit::computeDynamicWindow( current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, - max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel); + max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, + control_duration_); // apply regulation to Dynamic Window auto [regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel] = applyRegulationToDynamicWindow( + regulated_dynamic_window_min_linear_vel] = + dynamic_window_pure_pursuit::applyRegulationToDynamicWindow( regulated_linear_vel, dynamic_window_max_linear_vel, dynamic_window_min_linear_vel); // compute optimal velocity within Dynamic Window - std::tie(linear_vel, angular_vel) = computeOptimalVelocityWithinDynamicWindow( + std::tie(linear_vel, + angular_vel) = dynamic_window_pure_pursuit::computeOptimalVelocityWithinDynamicWindow( regulated_dynamic_window_max_linear_vel, regulated_dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, dynamic_window_min_angular_vel, regulation_curvature, x_vel_sign - ); + ); } } diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt index dc146c36cca..3f8f5ed6f2a 100644 --- a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -1,6 +1,7 @@ # tests for regulated PP nav2_add_gtest(test_regulated_pp test_regulated_pp.cpp + test_dwpp_functions.cpp path_utils/path_utils.cpp ) target_link_libraries(test_regulated_pp diff --git a/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp b/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp new file mode 100644 index 00000000000..1a5d892213b --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp @@ -0,0 +1,338 @@ +// Copyright (c) 2025 Fumiya Ohnishi +// +// 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 "nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp" +#include "gtest/gtest.h" + +using namespace nav2_regulated_pure_pursuit_controller::dynamic_window_pure_pursuit; // NOLINT + + +TEST(DynamicWindowPurePursuitTest, computeDynamicWindow) +{ + double max_linear_vel = 0.5; + double min_linear_vel = -0.5; + double max_angular_vel = 1.0; + double min_angular_vel = -1.0; + double max_linear_accel = 0.5; + double max_linear_decel = 1.0; + double max_angular_accel = 1.0; + double max_angular_decel = 2.0; + double control_duration = 1.0 / 20.0; + + double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, dynamic_window_min_angular_vel; + geometry_msgs::msg::Twist current_speed = geometry_msgs::msg::Twist(); + + // case1: current linear velocity is positive, angular velocity is positive, + // clip by max linear vel + current_speed.linear.x = 0.5; current_speed.angular.z = 0.2; + + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.5); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.45); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.25); + EXPECT_EQ(dynamic_window_min_angular_vel, 0.10); + + // case2: current linear velocity is positive, angular velocity is negative + current_speed.linear.x = 0.3; current_speed.angular.z = -0.2; + + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.325); + EXPECT_EQ(dynamic_window_min_linear_vel, 0.25); + EXPECT_EQ(dynamic_window_max_angular_vel, -0.1); + EXPECT_EQ(dynamic_window_min_angular_vel, -0.25); + + // case3: current linear velocity is zero, angular velocity is zero + current_speed.linear.x = 0.0; current_speed.angular.z = -0.0; + + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window_max_linear_vel, 0.025); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.025); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.05); + EXPECT_EQ(dynamic_window_min_angular_vel, -0.05); + + // case4: current linear velocity is negative, angular velocity is positive + current_speed.linear.x = -0.3; current_speed.angular.z = 0.2; + + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window_max_linear_vel, -0.25); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.325); + EXPECT_EQ(dynamic_window_max_angular_vel, 0.25); + EXPECT_EQ(dynamic_window_min_angular_vel, 0.10); + + // case5: current linear velocity is negative, angular velocity is negative, + // clipped by min lenear vel + current_speed.linear.x = -0.5; current_speed.angular.z = -0.2; + + std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, + dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel) = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window_max_linear_vel, -0.45); + EXPECT_EQ(dynamic_window_min_linear_vel, -0.5); + EXPECT_EQ(dynamic_window_max_angular_vel, -0.1); + EXPECT_EQ(dynamic_window_min_angular_vel, -0.25); +} + +TEST(DynamicWindowPurePursuitTest, applyRegulationToDynamicWindow) +{ + double regulated_linear_vel; + double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel; + + double regulated_dynamic_window_max_linear_vel, regulated_dynamic_window_min_linear_vel; + + regulated_linear_vel = 0.3; + dynamic_window_max_linear_vel = 0.5, dynamic_window_min_linear_vel = 0.4; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.4); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.4); + + dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.2; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.3); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.2); + + dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.1; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.2); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.1); + + dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.1); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.0); + + dynamic_window_max_linear_vel = -0.1, dynamic_window_min_linear_vel = -0.3; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.1); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.1); + + regulated_linear_vel = -0.3; + dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.1); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.1); + + dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.0); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.2); + + dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.3; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.2); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.3); + + dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.4; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.2); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.3); + + dynamic_window_max_linear_vel = -0.4, dynamic_window_min_linear_vel = -0.5; + + std::tie(regulated_dynamic_window_max_linear_vel, + regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window_max_linear_vel, + dynamic_window_min_linear_vel); + + EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.4); + EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.4); +} + +TEST(DynamicWindowPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) +{ + double dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + double dynamic_window_max_angular_vel = 0.1, dynamic_window_min_angular_vel = -0.1; + double curvature = 0.0; + double sign = 1.0; + double optimal_linear_vel; + double optimal_angular_vel; + + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign); + + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.0); + + + dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign); + + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.1); + + curvature = 1.0; + dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; + dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign); + + EXPECT_EQ(optimal_linear_vel, 0.2); + EXPECT_EQ(optimal_angular_vel, 0.2); + + dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; + dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; + + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign); + + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.2); + + sign = -1.0; + curvature = 0.0; + dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + dynamic_window_max_angular_vel = 0.1, dynamic_window_min_angular_vel = -0.1; + + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign); + + EXPECT_EQ(optimal_linear_vel, 0.1); + EXPECT_EQ(optimal_angular_vel, 0.0); + + dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; + dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign); + + EXPECT_EQ(optimal_linear_vel, 0.1); + EXPECT_EQ(optimal_angular_vel, 0.1); + + curvature = 1.0; + dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; + dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign); + + EXPECT_EQ(optimal_linear_vel, 0.1); + EXPECT_EQ(optimal_angular_vel, 0.1); + + dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; + dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; + + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, + dynamic_window_min_angular_vel, curvature, sign); + + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.2); +} diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index a2cf32fdfe2..f4d665bff56 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -91,46 +91,6 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure { return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); } - - std::tuple computeDynamicWindowWrapper( - const geometry_msgs::msg::Twist & current_speed, - const double & max_linear_vel, - const double & min_linear_vel, - const double & max_angular_vel, - const double & min_angular_vel, - const double & max_linear_accel, - const double & max_linear_decel, - const double & max_angular_accel, - const double & max_angular_decel) - { - return computeDynamicWindow(current_speed, - max_linear_vel, min_linear_vel, max_angular_vel, - min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel); - } - - std::tuple applyRegulationToDynamicWindowWrapper( - const double & regulated_linear_vel, - const double & dynamic_window_max_linear_vel, - const double & dynamic_window_min_linear_vel) - { - return applyRegulationToDynamicWindow( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - } - - std::tuple computeOptimalVelocityWithinDynamicWindowWrapper( - const double & dynamic_window_max_linear_vel, - const double & dynamic_window_min_linear_vel, - const double & dynamic_window_max_angular_vel, - const double & dynamic_window_min_angular_vel, - const double & curvature, - const double & sign) - { - return computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - } }; TEST(RegulatedPurePursuitTest, basicAPI) @@ -462,330 +422,6 @@ TEST(RegulatedPurePursuitTest, applyConstraints) // EXPECT_NEAR(linear_vel, 0.5, 0.01); } -TEST(RegulatedPurePursuitTest, computeDynamicWindow) -{ - auto ctrl = std::make_shared(); - - double max_linear_vel = 0.5; - double min_linear_vel = -0.5; - double max_angular_vel = 1.0; - double min_angular_vel = -1.0; - double max_linear_accel = 0.5; - double max_linear_decel = 1.0; - double max_angular_accel = 1.0; - double max_angular_decel = 2.0; - - double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel; - geometry_msgs::msg::Twist current_speed = geometry_msgs::msg::Twist(); - - // case1: current linear velocity is positive, angular velocity is positive, - // clip by max linear vel - current_speed.linear.x = 0.5; current_speed.angular.z = 0.2; - - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( - current_speed, max_linear_vel, min_linear_vel, - max_angular_vel, min_angular_vel, - max_linear_accel, max_linear_decel, - max_angular_accel, max_angular_decel); - - EXPECT_EQ(dynamic_window_max_linear_vel, 0.5); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.45); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.25); - EXPECT_EQ(dynamic_window_min_angular_vel, 0.10); - - // case2: current linear velocity is positive, angular velocity is negative - current_speed.linear.x = 0.3; current_speed.angular.z = -0.2; - - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( - current_speed, max_linear_vel, min_linear_vel, - max_angular_vel, min_angular_vel, - max_linear_accel, max_linear_decel, - max_angular_accel, max_angular_decel); - - EXPECT_EQ(dynamic_window_max_linear_vel, 0.325); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.25); - EXPECT_EQ(dynamic_window_max_angular_vel, -0.1); - EXPECT_EQ(dynamic_window_min_angular_vel, -0.25); - - // case3: current linear velocity is zero, angular velocity is zero - current_speed.linear.x = 0.0; current_speed.angular.z = -0.0; - - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( - current_speed, max_linear_vel, min_linear_vel, - max_angular_vel, min_angular_vel, - max_linear_accel, max_linear_decel, - max_angular_accel, max_angular_decel); - - EXPECT_EQ(dynamic_window_max_linear_vel, 0.025); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.025); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.05); - EXPECT_EQ(dynamic_window_min_angular_vel, -0.05); - - // case4: current linear velocity is negative, angular velocity is positive - current_speed.linear.x = -0.3; current_speed.angular.z = 0.2; - - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( - current_speed, max_linear_vel, min_linear_vel, - max_angular_vel, min_angular_vel, - max_linear_accel, max_linear_decel, - max_angular_accel, max_angular_decel); - - EXPECT_EQ(dynamic_window_max_linear_vel, -0.25); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.325); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.25); - EXPECT_EQ(dynamic_window_min_angular_vel, 0.10); - - // case5: current linear velocity is negative, angular velocity is negative, - // clipped by min lenear vel - current_speed.linear.x = -0.5; current_speed.angular.z = -0.2; - - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = ctrl->computeDynamicWindowWrapper( - current_speed, max_linear_vel, min_linear_vel, - max_angular_vel, min_angular_vel, - max_linear_accel, max_linear_decel, - max_angular_accel, max_angular_decel); - - EXPECT_EQ(dynamic_window_max_linear_vel, -0.45); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.5); - EXPECT_EQ(dynamic_window_max_angular_vel, -0.1); - EXPECT_EQ(dynamic_window_min_angular_vel, -0.25); -} - -TEST(RegulatedPurePursuitTest, applyRegulationToDynamicWindow) -{ - auto ctrl = std::make_shared(); - - double regulated_linear_vel; - double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel; - - double regulated_dynamic_window_max_linear_vel, regulated_dynamic_window_min_linear_vel; - - regulated_linear_vel = 0.3; - dynamic_window_max_linear_vel = 0.5, dynamic_window_min_linear_vel = 0.4; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.4); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.4); - - dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.2; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.3); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.2); - - dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.1; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.2); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.1); - - dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.1); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.0); - - dynamic_window_max_linear_vel = -0.1, dynamic_window_min_linear_vel = -0.3; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.1); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.1); - - regulated_linear_vel = -0.3; - dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.1); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.1); - - dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.0); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.2); - - dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.3; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.2); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.3); - - dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.4; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.2); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.3); - - dynamic_window_max_linear_vel = -0.4, dynamic_window_min_linear_vel = -0.5; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = ctrl->applyRegulationToDynamicWindowWrapper( - regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.4); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.4); -} - -TEST(RegulatedPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) -{ - auto ctrl = std::make_shared(); - - double dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - double dynamic_window_max_angular_vel = 0.1, dynamic_window_min_angular_vel = -0.1; - double curvature = 0.0; - double sign = 1.0; - double optimal_linear_vel; - double optimal_angular_vel; - - std::tie(optimal_linear_vel, - optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - - EXPECT_EQ(optimal_linear_vel, 0.3); - EXPECT_EQ(optimal_angular_vel, 0.0); - - - dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - - std::tie(optimal_linear_vel, - optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - - EXPECT_EQ(optimal_linear_vel, 0.3); - EXPECT_EQ(optimal_angular_vel, 0.1); - - curvature = 1.0; - dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; - dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - - std::tie(optimal_linear_vel, - optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - - EXPECT_EQ(optimal_linear_vel, 0.2); - EXPECT_EQ(optimal_angular_vel, 0.2); - - dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; - dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; - - std::tie(optimal_linear_vel, - optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - - EXPECT_EQ(optimal_linear_vel, 0.3); - EXPECT_EQ(optimal_angular_vel, 0.2); - - sign = -1.0; - curvature = 0.0; - dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - dynamic_window_max_angular_vel = 0.1, dynamic_window_min_angular_vel = -0.1; - - std::tie(optimal_linear_vel, - optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - - EXPECT_EQ(optimal_linear_vel, 0.1); - EXPECT_EQ(optimal_angular_vel, 0.0); - - dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - - std::tie(optimal_linear_vel, - optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - - EXPECT_EQ(optimal_linear_vel, 0.1); - EXPECT_EQ(optimal_angular_vel, 0.1); - - curvature = 1.0; - dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; - dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - - std::tie(optimal_linear_vel, - optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - - EXPECT_EQ(optimal_linear_vel, 0.1); - EXPECT_EQ(optimal_angular_vel, 0.1); - - dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; - dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; - - std::tie(optimal_linear_vel, - optimal_angular_vel) = ctrl->computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - - EXPECT_EQ(optimal_linear_vel, 0.3); - EXPECT_EQ(optimal_angular_vel, 0.2); -} - TEST(RegulatedPurePursuitTest, testDynamicParameter) { auto node = std::make_shared("Smactest"); From 36c540fd1d2dadf1cdbca061f1b38d5970a8d3c4 Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 27 Nov 2025 20:24:24 +0900 Subject: [PATCH 28/29] :recycle: use struct to represent dynamic window bounds Signed-off-by: Decwest --- .../dynamic_window_pure_pursuit_functions.hpp | 81 +++-- .../parameter_handler.hpp | 1 - .../regulated_pure_pursuit_controller.hpp | 1 - .../src/regulated_pure_pursuit_controller.cpp | 20 +- .../test/test_dwpp_functions.cpp | 292 +++++++----------- 5 files changed, 161 insertions(+), 234 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp index d937c57863d..5cafb8fa496 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp @@ -33,10 +33,10 @@ namespace dynamic_window_pure_pursuit struct DynamicWindowBounds { - double dynamic_window_max_linear_vel; - double dynamic_window_min_linear_vel; - double dynamic_window_max_angular_vel; - double dynamic_window_min_angular_vel; + double max_linear_vel; + double min_linear_vel; + double max_angular_vel; + double min_angular_vel; }; /** @@ -47,7 +47,7 @@ struct DynamicWindowBounds * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window */ -inline std::tuple computeDynamicWindow( +inline DynamicWindowBounds computeDynamicWindow( const geometry_msgs::msg::Twist & current_speed, const double & max_linear_vel, const double & min_linear_vel, @@ -60,6 +60,7 @@ inline std::tuple computeDynamicWindow( const double & dt ) { + DynamicWindowBounds dynamic_window; constexpr double Eps = 1e-3; // function to compute dynamic window for a single dimension @@ -90,19 +91,18 @@ inline std::tuple computeDynamicWindow( }; // linear velocity - auto [dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel] = compute_window(current_speed.linear.x, + std::tie(dynamic_window.max_linear_vel, + dynamic_window.min_linear_vel) = compute_window(current_speed.linear.x, max_linear_vel, min_linear_vel, max_linear_accel, max_linear_decel); // angular velocity - auto [dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel] = compute_window(current_speed.angular.z, + std::tie(dynamic_window.max_angular_vel, + dynamic_window.min_angular_vel) = compute_window(current_speed.angular.z, max_angular_vel, min_angular_vel, max_angular_accel, max_angular_decel); - return std::make_tuple(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel); + return dynamic_window; } /** @@ -111,10 +111,9 @@ inline std::tuple computeDynamicWindow( * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window */ -inline std::tuple applyRegulationToDynamicWindow( +inline void applyRegulationToDynamicWindow( const double & regulated_linear_vel, - const double & dynamic_window_max_linear_vel, - const double & dynamic_window_min_linear_vel) + DynamicWindowBounds & dynamic_window) { double regulated_dynamic_window_max_linear_vel; double regulated_dynamic_window_min_linear_vel; @@ -122,14 +121,14 @@ inline std::tuple applyRegulationToDynamicWindow( // Extract the portion of the dynamic window that lies within the range [0, regulated_linear_vel] if (regulated_linear_vel >= 0.0) { regulated_dynamic_window_max_linear_vel = std::min( - dynamic_window_max_linear_vel, regulated_linear_vel); + dynamic_window.max_linear_vel, regulated_linear_vel); regulated_dynamic_window_min_linear_vel = std::max( - dynamic_window_min_linear_vel, 0.0); + dynamic_window.min_linear_vel, 0.0); } else { regulated_dynamic_window_max_linear_vel = std::min( - dynamic_window_max_linear_vel, 0.0); + dynamic_window.max_linear_vel, 0.0); regulated_dynamic_window_min_linear_vel = std::max( - dynamic_window_min_linear_vel, regulated_linear_vel); + dynamic_window.min_linear_vel, regulated_linear_vel); } if (regulated_dynamic_window_max_linear_vel < regulated_dynamic_window_min_linear_vel) { @@ -145,9 +144,8 @@ inline std::tuple applyRegulationToDynamicWindow( } } - return std::make_tuple( - regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel); + dynamic_window.max_linear_vel = regulated_dynamic_window_max_linear_vel; + dynamic_window.min_linear_vel = regulated_dynamic_window_min_linear_vel; } @@ -163,10 +161,7 @@ inline std::tuple applyRegulationToDynamicWindow( * @param optimal_angular_vel Optimal angular velocity to follow the path under velocity and acceleration constraints */ inline std::tuple computeOptimalVelocityWithinDynamicWindow( - const double & dynamic_window_max_linear_vel, - const double & dynamic_window_min_linear_vel, - const double & dynamic_window_max_angular_vel, - const double & dynamic_window_min_angular_vel, + const DynamicWindowBounds & dynamic_window, const double & curvature, const double & sign ) @@ -184,22 +179,22 @@ inline std::tuple computeOptimalVelocityWithinDynamicWindow( // linear velocity if (sign >= 0.0) { // If moving forward, select the max linear vel - optimal_linear_vel = dynamic_window_max_linear_vel; + optimal_linear_vel = dynamic_window.max_linear_vel; } else { // If moving backward, select the min linear vel - optimal_linear_vel = dynamic_window_min_linear_vel; + optimal_linear_vel = dynamic_window.min_linear_vel; } // angular velocity // If the line angular_vel = 0 intersects the dynamic window,angular_vel = 0.0 - if (dynamic_window_min_angular_vel <= 0.0 && 0.0 <= dynamic_window_max_angular_vel) { + if (dynamic_window.min_angular_vel <= 0.0 && 0.0 <= dynamic_window.max_angular_vel) { optimal_angular_vel = 0.0; } else { // If not, select angular vel within dynamic window closest to 0 - if (std::abs(dynamic_window_min_angular_vel) <= std::abs(dynamic_window_max_angular_vel)) { - optimal_angular_vel = dynamic_window_min_angular_vel; + if (std::abs(dynamic_window.min_angular_vel) <= std::abs(dynamic_window.max_angular_vel)) { + optimal_angular_vel = dynamic_window.min_angular_vel; } else { - optimal_angular_vel = dynamic_window_max_angular_vel; + optimal_angular_vel = dynamic_window.max_angular_vel; } } return std::make_tuple(optimal_linear_vel, optimal_angular_vel); @@ -210,10 +205,10 @@ inline std::tuple computeOptimalVelocityWithinDynamicWindow( // List the four candidate intersection points std::pair candidates[] = { - {dynamic_window_min_linear_vel, curvature * dynamic_window_min_linear_vel}, - {dynamic_window_max_linear_vel, curvature * dynamic_window_max_linear_vel}, - {dynamic_window_min_angular_vel / curvature, dynamic_window_min_angular_vel}, - {dynamic_window_max_angular_vel / curvature, dynamic_window_max_angular_vel} + {dynamic_window.min_linear_vel, curvature * dynamic_window.min_linear_vel}, + {dynamic_window.max_linear_vel, curvature * dynamic_window.max_linear_vel}, + {dynamic_window.min_angular_vel / curvature, dynamic_window.min_angular_vel}, + {dynamic_window.max_angular_vel / curvature, dynamic_window.max_angular_vel} }; double best_linear_vel = -std::numeric_limits::infinity() * sign; @@ -221,10 +216,10 @@ inline std::tuple computeOptimalVelocityWithinDynamicWindow( for (auto [linear_vel, angular_vel] : candidates) { // Check whether the candidate lies within the dynamic window - if (linear_vel >= dynamic_window_min_linear_vel && - linear_vel <= dynamic_window_max_linear_vel && - angular_vel >= dynamic_window_min_angular_vel && - angular_vel <= dynamic_window_max_angular_vel) + if (linear_vel >= dynamic_window.min_linear_vel && + linear_vel <= dynamic_window.max_linear_vel && + angular_vel >= dynamic_window.min_angular_vel && + angular_vel <= dynamic_window.max_angular_vel) { // Select the candidate with the largest linear velocity (considering moving direction) if (linear_vel * sign > best_linear_vel * sign) { @@ -247,10 +242,10 @@ inline std::tuple computeOptimalVelocityWithinDynamicWindow( // Because the dynamic window is a convex region, // the closest point must be one of its four corners. const std::array, 4> corners = {{ - {dynamic_window_min_linear_vel, dynamic_window_min_angular_vel}, - {dynamic_window_min_linear_vel, dynamic_window_max_angular_vel}, - {dynamic_window_max_linear_vel, dynamic_window_min_angular_vel}, - {dynamic_window_max_linear_vel, dynamic_window_max_angular_vel} + {dynamic_window.min_linear_vel, dynamic_window.min_angular_vel}, + {dynamic_window.min_linear_vel, dynamic_window.max_angular_vel}, + {dynamic_window.max_linear_vel, dynamic_window.min_angular_vel}, + {dynamic_window.max_linear_vel, dynamic_window.max_angular_vel} }}; // Compute the distance from a point (linear_vel, angular_vel) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 83bb8a66315..962e0101a25 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -1,5 +1,4 @@ // Copyright (c) 2022 Samsung Research America -// Copyright (c) 2025 Fumiya Ohnishi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 335996aeb68..13796357ae7 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -1,6 +1,5 @@ // Copyright (c) 2020 Shrijit Singh // Copyright (c) 2020 Samsung Research America -// Copyright (c) 2025 Fumiya Ohnishi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index de40e691823..469b3b243f7 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -294,31 +294,23 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity const double & max_angular_decel = params_->max_angular_decel; // compute Dynamic Window - auto [dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel] = dynamic_window_pure_pursuit::computeDynamicWindow( + dynamic_window_pure_pursuit::DynamicWindowBounds dynamic_window = + dynamic_window_pure_pursuit::computeDynamicWindow( current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, control_duration_); // apply regulation to Dynamic Window - auto [regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel] = - dynamic_window_pure_pursuit::applyRegulationToDynamicWindow( + dynamic_window_pure_pursuit::applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); + dynamic_window); // compute optimal velocity within Dynamic Window std::tie(linear_vel, angular_vel) = dynamic_window_pure_pursuit::computeOptimalVelocityWithinDynamicWindow( - regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, + dynamic_window, regulation_curvature, - x_vel_sign - ); + x_vel_sign); } } diff --git a/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp b/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp index 1a5d892213b..ebca6673fd1 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp @@ -30,309 +30,251 @@ TEST(DynamicWindowPurePursuitTest, computeDynamicWindow) double max_angular_decel = 2.0; double control_duration = 1.0 / 20.0; - double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, dynamic_window_min_angular_vel; + DynamicWindowBounds dynamic_window; geometry_msgs::msg::Twist current_speed = geometry_msgs::msg::Twist(); // case1: current linear velocity is positive, angular velocity is positive, // clip by max linear vel current_speed.linear.x = 0.5; current_speed.angular.z = 0.2; - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = computeDynamicWindow( + dynamic_window = computeDynamicWindow( current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, control_duration); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.5); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.45); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.25); - EXPECT_EQ(dynamic_window_min_angular_vel, 0.10); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.5); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.45); + EXPECT_EQ(dynamic_window.max_angular_vel, 0.25); + EXPECT_EQ(dynamic_window.min_angular_vel, 0.10); // case2: current linear velocity is positive, angular velocity is negative current_speed.linear.x = 0.3; current_speed.angular.z = -0.2; - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = computeDynamicWindow( + dynamic_window = computeDynamicWindow( current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, control_duration); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.325); - EXPECT_EQ(dynamic_window_min_linear_vel, 0.25); - EXPECT_EQ(dynamic_window_max_angular_vel, -0.1); - EXPECT_EQ(dynamic_window_min_angular_vel, -0.25); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.325); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.25); + EXPECT_EQ(dynamic_window.max_angular_vel, -0.1); + EXPECT_EQ(dynamic_window.min_angular_vel, -0.25); // case3: current linear velocity is zero, angular velocity is zero current_speed.linear.x = 0.0; current_speed.angular.z = -0.0; - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = computeDynamicWindow( + dynamic_window = computeDynamicWindow( current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, control_duration); - EXPECT_EQ(dynamic_window_max_linear_vel, 0.025); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.025); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.05); - EXPECT_EQ(dynamic_window_min_angular_vel, -0.05); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.025); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.025); + EXPECT_EQ(dynamic_window.max_angular_vel, 0.05); + EXPECT_EQ(dynamic_window.min_angular_vel, -0.05); // case4: current linear velocity is negative, angular velocity is positive current_speed.linear.x = -0.3; current_speed.angular.z = 0.2; - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = computeDynamicWindow( + dynamic_window = computeDynamicWindow( current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, control_duration); - EXPECT_EQ(dynamic_window_max_linear_vel, -0.25); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.325); - EXPECT_EQ(dynamic_window_max_angular_vel, 0.25); - EXPECT_EQ(dynamic_window_min_angular_vel, 0.10); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.25); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.325); + EXPECT_EQ(dynamic_window.max_angular_vel, 0.25); + EXPECT_EQ(dynamic_window.min_angular_vel, 0.10); // case5: current linear velocity is negative, angular velocity is negative, // clipped by min lenear vel current_speed.linear.x = -0.5; current_speed.angular.z = -0.2; - std::tie(dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, - dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel) = computeDynamicWindow( + dynamic_window = computeDynamicWindow( current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, control_duration); - EXPECT_EQ(dynamic_window_max_linear_vel, -0.45); - EXPECT_EQ(dynamic_window_min_linear_vel, -0.5); - EXPECT_EQ(dynamic_window_max_angular_vel, -0.1); - EXPECT_EQ(dynamic_window_min_angular_vel, -0.25); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.45); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.5); + EXPECT_EQ(dynamic_window.max_angular_vel, -0.1); + EXPECT_EQ(dynamic_window.min_angular_vel, -0.25); } TEST(DynamicWindowPurePursuitTest, applyRegulationToDynamicWindow) { double regulated_linear_vel; - double dynamic_window_max_linear_vel, dynamic_window_min_linear_vel; - - double regulated_dynamic_window_max_linear_vel, regulated_dynamic_window_min_linear_vel; + DynamicWindowBounds dynamic_window; regulated_linear_vel = 0.3; - dynamic_window_max_linear_vel = 0.5, dynamic_window_min_linear_vel = 0.4; - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = 0.5; + dynamic_window.min_linear_vel = 0.4; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.4); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.4); - - dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.2; + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.4); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.4); - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = 0.4; + dynamic_window.min_linear_vel = 0.2; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.3); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.2); - - dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.1; + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.3); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.2); - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = 0.2; + dynamic_window.min_linear_vel = 0.1; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.2); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.1); - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.2); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.1); - - dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = 0.1; + dynamic_window.min_linear_vel = -0.2; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.1); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.0); + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.1); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.0); - dynamic_window_max_linear_vel = -0.1, dynamic_window_min_linear_vel = -0.3; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = -0.1; + dynamic_window.min_linear_vel = -0.3; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.1); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.1); + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.1); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.1); regulated_linear_vel = -0.3; - dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = 0.3; + dynamic_window.min_linear_vel = 0.1; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.1); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.1); - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.1); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, 0.1); - - dynamic_window_max_linear_vel = 0.1, dynamic_window_min_linear_vel = -0.2; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = 0.1; + dynamic_window.min_linear_vel = -0.2; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, 0.0); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.2); + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.0); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.2); - dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.3; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = -0.2; + dynamic_window.min_linear_vel = -0.3; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.2); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.3); + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.2); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.3); - dynamic_window_max_linear_vel = -0.2, dynamic_window_min_linear_vel = -0.4; - - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = -0.2; + dynamic_window.min_linear_vel = -0.4; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.2); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.3); - - dynamic_window_max_linear_vel = -0.4, dynamic_window_min_linear_vel = -0.5; + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.2); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.3); - std::tie(regulated_dynamic_window_max_linear_vel, - regulated_dynamic_window_min_linear_vel) = applyRegulationToDynamicWindow( + dynamic_window.max_linear_vel = -0.4; + dynamic_window.min_linear_vel = -0.5; + applyRegulationToDynamicWindow( regulated_linear_vel, - dynamic_window_max_linear_vel, - dynamic_window_min_linear_vel); - - EXPECT_EQ(regulated_dynamic_window_max_linear_vel, -0.4); - EXPECT_EQ(regulated_dynamic_window_min_linear_vel, -0.4); + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.4); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.4); } TEST(DynamicWindowPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) { - double dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - double dynamic_window_max_angular_vel = 0.1, dynamic_window_min_angular_vel = -0.1; - double curvature = 0.0; - double sign = 1.0; + DynamicWindowBounds dynamic_window; double optimal_linear_vel; double optimal_angular_vel; + double sign = 1.0; + + double curvature = 0.0; + + dynamic_window.max_linear_vel = 0.3, dynamic_window.min_linear_vel = 0.1; + dynamic_window.max_angular_vel = 0.1, dynamic_window.min_angular_vel = -0.1; std::tie(optimal_linear_vel, optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - + dynamic_window, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.3); EXPECT_EQ(optimal_angular_vel, 0.0); - - dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - + dynamic_window.max_linear_vel = 0.3, dynamic_window.min_linear_vel = 0.1; + dynamic_window.max_angular_vel = 0.3, dynamic_window.min_angular_vel = 0.1; std::tie(optimal_linear_vel, optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - + dynamic_window, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.3); EXPECT_EQ(optimal_angular_vel, 0.1); curvature = 1.0; - dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; - dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + dynamic_window.max_linear_vel = 0.2, dynamic_window.min_linear_vel = 0.0; + dynamic_window.max_angular_vel = 0.3, dynamic_window.min_angular_vel = 0.1; std::tie(optimal_linear_vel, optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - + dynamic_window, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.2); EXPECT_EQ(optimal_angular_vel, 0.2); - dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; - dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; - + dynamic_window.max_linear_vel = 0.4, dynamic_window.min_linear_vel = 0.3; + dynamic_window.max_angular_vel = 0.2, dynamic_window.min_angular_vel = 0.1; std::tie(optimal_linear_vel, optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - + dynamic_window, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.3); EXPECT_EQ(optimal_angular_vel, 0.2); sign = -1.0; + curvature = 0.0; - dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - dynamic_window_max_angular_vel = 0.1, dynamic_window_min_angular_vel = -0.1; + dynamic_window.max_linear_vel = 0.3, dynamic_window.min_linear_vel = 0.1; + dynamic_window.max_angular_vel = 0.1, dynamic_window.min_angular_vel = -0.1; std::tie(optimal_linear_vel, optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - + dynamic_window, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.1); EXPECT_EQ(optimal_angular_vel, 0.0); - dynamic_window_max_linear_vel = 0.3, dynamic_window_min_linear_vel = 0.1; - dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; - + dynamic_window.max_linear_vel = 0.3, dynamic_window.min_linear_vel = 0.1; + dynamic_window.max_angular_vel = 0.3, dynamic_window.min_angular_vel = 0.1; std::tie(optimal_linear_vel, optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - + dynamic_window, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.1); EXPECT_EQ(optimal_angular_vel, 0.1); curvature = 1.0; - dynamic_window_max_linear_vel = 0.2, dynamic_window_min_linear_vel = 0.0; - dynamic_window_max_angular_vel = 0.3, dynamic_window_min_angular_vel = 0.1; + dynamic_window.max_linear_vel = 0.2, dynamic_window.min_linear_vel = 0.0; + dynamic_window.max_angular_vel = 0.3, dynamic_window.min_angular_vel = 0.1; std::tie(optimal_linear_vel, optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - + dynamic_window, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.1); EXPECT_EQ(optimal_angular_vel, 0.1); - dynamic_window_max_linear_vel = 0.4, dynamic_window_min_linear_vel = 0.3; - dynamic_window_max_angular_vel = 0.2, dynamic_window_min_angular_vel = 0.1; - + dynamic_window.max_linear_vel = 0.4, dynamic_window.min_linear_vel = 0.3; + dynamic_window.max_angular_vel = 0.2, dynamic_window.min_angular_vel = 0.1; std::tie(optimal_linear_vel, optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( - dynamic_window_max_linear_vel, dynamic_window_min_linear_vel, dynamic_window_max_angular_vel, - dynamic_window_min_angular_vel, curvature, sign); - + dynamic_window, curvature, sign); EXPECT_EQ(optimal_linear_vel, 0.3); EXPECT_EQ(optimal_angular_vel, 0.2); } From 43acc07dfcbdc3b717a05d377ac2b25f2c0f051a Mon Sep 17 00:00:00 2001 From: Decwest Date: Thu, 27 Nov 2025 20:50:46 +0900 Subject: [PATCH 29/29] :memo: modify docstring and readme Signed-off-by: Decwest --- .../README.md | 2 -- .../dynamic_window_pure_pursuit_functions.hpp | 31 ++++++++++--------- .../src/regulated_pure_pursuit_controller.cpp | 4 +-- 3 files changed, 18 insertions(+), 19 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 546b450e483..96ca331e37d 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -107,7 +107,6 @@ A link to the paper and its citation will be provided once it becomes publicly a | `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | | `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. | | `use_dynamic_window` | Whether to use the Dynamic Window Pure Pursuit (DWPP) Algorithm. This algorithm computes optimal path tracking velocity commands under velocity and acceleration constraints. | -| `velocity_feedback` | How the current velocity is obtained during dynamic window computation. "OPEN_LOOP" uses the last commanded velocity (recommended). "CLOSED_LOOP" uses odometry velocity (may hinder proper acceleration/deceleration) | Example fully-described XML with default parameter values: @@ -168,7 +167,6 @@ controller_server: inflation_cost_scaling_factor: 3.0 min_distance_to_obstacle: 0.0 use_dynamic_window: false - velocity_feedback: "OPEN_LOOP" ``` ## Topics diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp index 5cafb8fa496..9677be534cb 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp @@ -41,11 +41,17 @@ struct DynamicWindowBounds /** * @brief Compute the dynamic window (feasible velocity bounds) based on the current speed and the given velocity and acceleration constraints. - * @param current_speed Current linear and angular velocity of the robot - * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window - * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window - * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window - * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window + * @param current_speed Current linear and angular velocity of the robot + * @param max_linear_vel Maximum allowable linear velocity + * @param min_linear_vel Minimum allowable linear velocity + * @param max_angular_vel Maximum allowable angular velocity + * @param min_angular_vel Minimum allowable angular velocity + * @param max_linear_accel Maximum allowable linear acceleration + * @param max_linear_decel Maximum allowable linear deceleration + * @param max_angular_accel Maximum allowable angular acceleration + * @param max_angular_decel Maximum allowable angular deceleration + * @param dt Control duration + * @return Computed dynamic window's velocity bounds */ inline DynamicWindowBounds computeDynamicWindow( const geometry_msgs::msg::Twist & current_speed, @@ -106,10 +112,9 @@ inline DynamicWindowBounds computeDynamicWindow( } /** - * @brief Apply regulated linear velocity to the dynamic window + * @brief Apply regulated linear velocity to the dynamic window * @param regulated_linear_vel Regulated linear velocity - * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window - * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window + * @param dynamic_window Dynamic window to be regulated */ inline void applyRegulationToDynamicWindow( const double & regulated_linear_vel, @@ -150,15 +155,11 @@ inline void applyRegulationToDynamicWindow( /** - * @brief Compute the optimal velocity to follow the path within the dynamic window - * @param dynamic_window_max_linear_vel Computed upper bound of the linear velocity within the dynamic window - * @param dynamic_window_min_linear_vel Computed lower bound of the linear velocity within the dynamic window - * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window - * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window + * @brief Compute the optimal velocity to follow the path within the dynamic window + * @param dynamic_window Dynamic window defining feasible velocity bounds * @param curvature Curvature of the path to follow * @param sign Velocity sign (forward or backward) - * @param optimal_linear_vel Optimal linear velocity to follow the path under velocity and acceleration constraints - * @param optimal_angular_vel Optimal angular velocity to follow the path under velocity and acceleration constraints + * @return Optimal linear and angular velocity */ inline std::tuple computeOptimalVelocityWithinDynamicWindow( const DynamicWindowBounds & dynamic_window, diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 469b3b243f7..809d49dcc6e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -306,8 +306,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity dynamic_window); // compute optimal velocity within Dynamic Window - std::tie(linear_vel, - angular_vel) = dynamic_window_pure_pursuit::computeOptimalVelocityWithinDynamicWindow( + std::tie(linear_vel, angular_vel) = + dynamic_window_pure_pursuit::computeOptimalVelocityWithinDynamicWindow( dynamic_window, regulation_curvature, x_vel_sign);