From be2904edf140112b46992918e994b4e9e44af05a Mon Sep 17 00:00:00 2001 From: pepisg Date: Thu, 2 Nov 2023 01:22:10 +0000 Subject: [PATCH 01/10] Support stitching paths in compute path to poses --- .../include/nav2_planner/planner_server.hpp | 1 + nav2_planner/src/planner_server.cpp | 15 ++++++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index c54b9ae829..8f48782842 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -240,6 +240,7 @@ class PlannerServer : public nav2_util::LifecycleNode std::vector planner_types_; double max_planner_duration_; std::string planner_ids_concat_; + bool allow_path_end_pose_deviation_; // Clock rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 84edc6e4df..6c068144eb 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -52,6 +52,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); + declare_parameter("allow_path_end_pose_deviation", false); declare_parameter("action_server_result_timeout", 10.0); @@ -137,6 +138,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); max_planner_duration_ = 0.0; } + get_parameter("allow_path_end_pose_deviation", allow_path_end_pose_deviation_); // Initialize pubs & subs plan_publisher_ = create_publisher("plan", 1); @@ -407,7 +409,14 @@ void PlannerServer::computePlanThroughPoses() if (i == 0) { curr_start = start; } else { - curr_start = goal->goals[i - 1]; + if (allow_path_end_pose_deviation_) { + // pick the end of the last planning task as the start for the next one + curr_start = concat_path.poses.back(); + curr_start.header = concat_path.header; + } else { + // pick the exact pose task as the start for the next one + curr_start = goal->goals[i - 1]; + } } curr_goal = goal->goals[i]; @@ -692,6 +701,10 @@ PlannerServer::dynamicParametersCallback(std::vector paramete max_planner_duration_ = 0.0; } } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == "allow_path_end_pose_deviation") { + allow_path_end_pose_deviation_ = parameter.as_bool(); + } } } From a939eab3f664f8318862735a3ee6b3e297fc818c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Alejandro=20Gonz=C3=A1lez?= <71234974+pepisg@users.noreply.github.com> Date: Thu, 2 Nov 2023 14:38:33 -0500 Subject: [PATCH 02/10] Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski --- nav2_planner/src/planner_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6c068144eb..f2cb4e73af 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -410,7 +410,8 @@ void PlannerServer::computePlanThroughPoses() curr_start = start; } else { if (allow_path_end_pose_deviation_) { - // pick the end of the last planning task as the start for the next one + // Pick the end of the last planning task as the start for the next one + // to allow for path tolerance deviations curr_start = concat_path.poses.back(); curr_start.header = concat_path.header; } else { From f2efccd7a577c3ad3f1849217598ed759f5256b2 Mon Sep 17 00:00:00 2001 From: pepisg Date: Sun, 10 Dec 2023 15:11:32 +0000 Subject: [PATCH 03/10] Rename parameter to allow_path_through_poses_goal_deviation --- nav2_planner/include/nav2_planner/planner_server.hpp | 2 +- nav2_planner/src/planner_server.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 8f48782842..f3344cdb0d 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -240,7 +240,7 @@ class PlannerServer : public nav2_util::LifecycleNode std::vector planner_types_; double max_planner_duration_; std::string planner_ids_concat_; - bool allow_path_end_pose_deviation_; + bool allow_path_through_poses_goal_deviation_; // Clock rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6c068144eb..6066bb9b1c 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -52,7 +52,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); - declare_parameter("allow_path_end_pose_deviation", false); + declare_parameter("allow_path_through_poses_goal_deviation", true); declare_parameter("action_server_result_timeout", 10.0); @@ -138,7 +138,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); max_planner_duration_ = 0.0; } - get_parameter("allow_path_end_pose_deviation", allow_path_end_pose_deviation_); + get_parameter("allow_path_through_poses_goal_deviation", allow_path_through_poses_goal_deviation_); // Initialize pubs & subs plan_publisher_ = create_publisher("plan", 1); @@ -409,7 +409,7 @@ void PlannerServer::computePlanThroughPoses() if (i == 0) { curr_start = start; } else { - if (allow_path_end_pose_deviation_) { + if (allow_path_through_poses_goal_deviation_) { // pick the end of the last planning task as the start for the next one curr_start = concat_path.poses.back(); curr_start.header = concat_path.header; @@ -702,8 +702,8 @@ PlannerServer::dynamicParametersCallback(std::vector paramete } } } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == "allow_path_end_pose_deviation") { - allow_path_end_pose_deviation_ = parameter.as_bool(); + if (name == "allow_path_through_poses_goal_deviation") { + allow_path_through_poses_goal_deviation_ = parameter.as_bool(); } } } From 0b6412263883792011a73f46864a6b47b54aeb5e Mon Sep 17 00:00:00 2001 From: pepisg Date: Sun, 10 Dec 2023 15:15:11 +0000 Subject: [PATCH 04/10] Fix description --- nav2_planner/src/planner_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6066bb9b1c..13f8dd9cb7 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -411,6 +411,7 @@ void PlannerServer::computePlanThroughPoses() } else { if (allow_path_through_poses_goal_deviation_) { // pick the end of the last planning task as the start for the next one + // to allow for path tolerance deviations curr_start = concat_path.poses.back(); curr_start.header = concat_path.header; } else { From 24b7a57c1208f78deebb2fdc8695d17196f3a20c Mon Sep 17 00:00:00 2001 From: pepisg Date: Sun, 10 Dec 2023 15:16:09 +0000 Subject: [PATCH 05/10] restore nav2_params --- nav2_bringup/params/nav2_params.yaml | 40 +++++----------------------- 1 file changed, 6 insertions(+), 34 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 05eebd40af..9cfa245e3f 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -37,11 +37,6 @@ amcl: z_rand: 0.5 z_short: 0.05 scan_topic: scan - set_initial_pose: true - initial_pose: - x: -2.0 - y: -0.5 - yaw: 0.0 bt_navigator: ros__parameters: @@ -280,36 +275,13 @@ map_saver: planner_server: ros__parameters: + expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] - use_sim_time: True - GridBased: - plugin: "nav2_smac_planner/SmacPlannerLattice" - allow_unknown: true # Allow traveling in unknown space - tolerance: 1.0 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # Max time in s for planner to plan, smooth - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them - retrospective_penalty: 0.015 - lattice_filepath: "/workspace/rover/ros2/src/navigation2/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json" # The filepath to the state lattice graph - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true smoother_server: ros__parameters: @@ -401,4 +373,4 @@ collision_monitor: topic: "scan" min_height: 0.15 max_height: 2.0 - enabled: True + enabled: True \ No newline at end of file From 409ead1344ec814648b697e770f3d9ac09f926bd Mon Sep 17 00:00:00 2001 From: pepisg Date: Sun, 10 Dec 2023 15:16:53 +0000 Subject: [PATCH 06/10] missing whitespace --- nav2_bringup/params/nav2_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 9cfa245e3f..39d72a5297 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -373,4 +373,4 @@ collision_monitor: topic: "scan" min_height: 0.15 max_height: 2.0 - enabled: True \ No newline at end of file + enabled: True From ba23449b74f8b13bbe4e79dccc4191369413a66f Mon Sep 17 00:00:00 2001 From: gg Date: Wed, 24 Jan 2024 19:32:48 -0500 Subject: [PATCH 07/10] lint fix --- nav2_planner/src/planner_server.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ba2551b176..69ed46635f 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -144,7 +144,9 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); max_planner_duration_ = 0.0; } - get_parameter("allow_path_through_poses_goal_deviation", allow_path_through_poses_goal_deviation_); + get_parameter( + "allow_path_through_poses_goal_deviation", + allow_path_through_poses_goal_deviation_); // Initialize pubs & subs plan_publisher_ = create_publisher("plan", 1); @@ -655,8 +657,8 @@ void PlannerServer::isPathValid( /** * The lethal check starts at the closest point to avoid points that have already been passed - * and may have become occupied. The method for collision detection is based on the shape of - * the footprint. + * and may have become occupied. The method for collision detection is based on the shape of + * the footprint. */ std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; From 95ea0aa45ed0ee3409dc094a9578c70d463e2511 Mon Sep 17 00:00:00 2001 From: gg Date: Thu, 25 Jan 2024 19:01:12 -0500 Subject: [PATCH 08/10] removed parameter Signed-off-by: gg --- nav2_planner/src/planner_server.cpp | 21 ++++----------------- 1 file changed, 4 insertions(+), 17 deletions(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 69ed46635f..ead48b4dae 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -52,7 +52,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); - declare_parameter("allow_path_through_poses_goal_deviation", true); declare_parameter("action_server_result_timeout", 10.0); @@ -144,9 +143,6 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); max_planner_duration_ = 0.0; } - get_parameter( - "allow_path_through_poses_goal_deviation", - allow_path_through_poses_goal_deviation_); // Initialize pubs & subs plan_publisher_ = create_publisher("plan", 1); @@ -405,15 +401,10 @@ void PlannerServer::computePlanThroughPoses() if (i == 0) { curr_start = start; } else { - if (allow_path_through_poses_goal_deviation_) { - // pick the end of the last planning task as the start for the next one - // to allow for path tolerance deviations - curr_start = concat_path.poses.back(); - curr_start.header = concat_path.header; - } else { - // pick the exact pose task as the start for the next one - curr_start = goal->goals[i - 1]; - } + // pick the end of the last planning task as the start for the next one + // to allow for path tolerance deviations + curr_start = concat_path.poses.back(); + curr_start.header = concat_path.header; } curr_goal = goal->goals[i]; @@ -717,10 +708,6 @@ PlannerServer::dynamicParametersCallback(std::vector paramete max_planner_duration_ = 0.0; } } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == "allow_path_through_poses_goal_deviation") { - allow_path_through_poses_goal_deviation_ = parameter.as_bool(); - } } } From 58c00f6da0c55519040a0e32e580da3f38adb35e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 Jan 2024 11:36:48 -0500 Subject: [PATCH 09/10] Update planner_server.hpp --- nav2_planner/include/nav2_planner/planner_server.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 53983d3cdb..7d2c9d03f0 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -241,7 +241,6 @@ class PlannerServer : public nav2_util::LifecycleNode std::vector planner_types_; double max_planner_duration_; std::string planner_ids_concat_; - bool allow_path_through_poses_goal_deviation_; // TF buffer std::shared_ptr tf_; From 9cc61f466148000e4df391491443bcdd9c3056db Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 Jan 2024 11:37:14 -0500 Subject: [PATCH 10/10] Update planner_server.cpp --- nav2_planner/src/planner_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ead48b4dae..de239b3587 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -52,7 +52,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); - declare_parameter("action_server_result_timeout", 10.0); get_parameter("planner_plugins", planner_ids_);