From a35b5d4910022731d28b9b4cc550b27e5d3c7a4b Mon Sep 17 00:00:00 2001 From: Sanchit B Date: Mon, 20 Apr 2026 16:00:10 -0600 Subject: [PATCH] docs(planner): updating docs for global planner accepting viapoints. Signed-off-by: Sanchit B --- .../bt-plugins/actions/ComputePathToPose.rst | 11 ++++ migration/Kilted.rst | 7 +++ .../docs/writing_new_nav2planner_plugin.rst | 60 +++++++++++-------- 3 files changed, 53 insertions(+), 25 deletions(-) diff --git a/configuration/packages/bt-plugins/actions/ComputePathToPose.rst b/configuration/packages/bt-plugins/actions/ComputePathToPose.rst index 49aaf783a8..1f42ddf3ad 100644 --- a/configuration/packages/bt-plugins/actions/ComputePathToPose.rst +++ b/configuration/packages/bt-plugins/actions/ComputePathToPose.rst @@ -43,6 +43,17 @@ Input Ports Description Goal pose. Takes in a blackboard variable, e.g. "{goal}". +:viapoints: + + ============================================= ======= + Type Default + --------------------------------------------- ------- + std::vector N/A + ============================================= ======= + + Description + Optional. A list of intermediate viapoints (excluding goal) to consider for planning. Takes in a blackboard variable, e.g. "{viapoints}". + :planner_id: ============== ======= diff --git a/migration/Kilted.rst b/migration/Kilted.rst index ce0d2a0887..165c727e47 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -938,3 +938,10 @@ Key changes: - New parameters ``publish_scan``, ``odom_publish_dur``, and ``scan_noise_std`` are available. See :ref:`configuring_loopback_sim` for full parameter documentation. + +Global planner plugin natively accepts viapoints +------------------------------------------------- + +`PR #5995 `_ updates the ``createPath`` API for the ``BaseGlobalPlanner`` to include a vector ``std::vector`` argument that takes in a list of intermediate points and passes them to the planner plugin implementation. + +The function signature for ``createPath`` must be updated accordingly for all custom planner plugins inheriting from the ``BaseGlobalPlanner``. This change does not alter the behavior of ``ComputePathThroughPoses`` that connects consecutive segments end-to-end but does upgrade the ``ComputePathToPose`` action. diff --git a/plugin_tutorials/docs/writing_new_nav2planner_plugin.rst b/plugin_tutorials/docs/writing_new_nav2planner_plugin.rst index 8fa6eb00c7..859df22846 100644 --- a/plugin_tutorials/docs/writing_new_nav2planner_plugin.rst +++ b/plugin_tutorials/docs/writing_new_nav2planner_plugin.rst @@ -59,9 +59,10 @@ Let's learn more about the methods needed to write a planner plugin. | | method should clean up resources which are created for the planner. | | +----------------------+-----------------------------------------------------------------------------+-------------------------+ | createPlan() | Method is called when planner server demands a global plan for specified | Yes | -| | start and goal pose. This method returns `nav_msgs\:\:msg\:\:Path` carrying | | -| | global plan. This method takes 3 input params: start pose, goal pose and | | -| | a function to check if the action has been canceled. | | +| | start pose, goal pose, and intermediate viapoint poses. This method returns | | +| | `nav_msgs\:\:msg\:\:Path` carrying global plan. This method takes 4 input | | +| | params: start pose, goal pose, a vector of intermediate poses and a function| | +| | to check if the action has been canceled. | | +----------------------+-----------------------------------------------------------------------------+-------------------------+ For this tutorial, we will be using methods ``StraightLine::configure()`` and ``StraightLine::createPlan()`` to create straight-line planner. @@ -82,12 +83,16 @@ In planners, ``configure()`` method must set member variables from ROS parameter Here, ``name_ + ".interpolation_resolution"`` is fetching the ROS parameters ``interpolation_resolution`` which is specific to our planner. Nav2 allows the loading of multiple plugins, and to keep things organized, each plugin is mapped to some ID/name. Now if we want to retrieve the parameters for that specific plugin, we use ``.`` as done in the above snippet. For example, our example planner is mapped to the name "GridBased" and to retrieve the ``interpolation_resolution`` parameter which is specific to "GridBased", we used ``Gridbased.interpolation_resolution``. In other words, ``GridBased`` is used as a namespace for plugin-specific parameters. We will see more on this when we discuss the parameters file (or params file). -In ``createPlan()`` method, we need to create a path from the given start to goal poses. The ``StraightLine::createPlan()`` is called using start pose and goal pose to solve the global path planning problem. Upon succeeding, it converts the path to the ``nav_msgs::msg::Path`` and returns to the planner server. Below annotation shows the implementation of this method. +In ``createPlan()`` method, we need to create a path from the given start to goal poses, passing through intermediate viapoints if provided. The ``StraightLine::createPlan()`` is called with a start pose, a goal pose, and a vector of intermediate viapoints to solve the global path planning problem. Upon succeeding, it converts the path to the ``nav_msgs::msg::Path`` and returns to the planner server. Below annotation shows the implementation of this method. .. code-block:: c++ nav_msgs::msg::Path global_path; + // copy the viapoints and append the goal since intermediate points would not include the goal + std::vector goals = viapoints; + goals.push_back(goal); + // Checking if the goal and start state is in the global frame if (start.header.frame_id != global_frame_) { RCLCPP_ERROR( @@ -106,28 +111,33 @@ In ``createPlan()`` method, we need to create a path from the given start to goa global_path.poses.clear(); global_path.header.stamp = node_->now(); global_path.header.frame_id = global_frame_; - // calculating the number of loops for current value of interpolation_resolution_ - int total_number_of_loop = std::hypot( - goal.pose.position.x - start.pose.position.x, - goal.pose.position.y - start.pose.position.y) / - interpolation_resolution_; - double x_increment = (goal.pose.position.x - start.pose.position.x) / total_number_of_loop; - double y_increment = (goal.pose.position.y - start.pose.position.y) / total_number_of_loop; - - for (int i = 0; i < total_number_of_loop; ++i) { - geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = start.pose.position.x + x_increment * i; - pose.pose.position.y = start.pose.position.y + y_increment * i; - pose.pose.position.z = 0.0; - pose.pose.orientation.x = 0.0; - pose.pose.orientation.y = 0.0; - pose.pose.orientation.z = 0.0; - pose.pose.orientation.w = 1.0; - pose.header.stamp = node_->now(); - pose.header.frame_id = global_frame_; - global_path.poses.push_back(pose); - } + geometry_msgs::msg::PoseStamped start_i = start; + for (auto goal_i : goals) + { + // calculating the number of loops for current value of interpolation_resolution_ + int total_number_of_loop = std::hypot( + goal_i.pose.position.x - start_i.pose.position.x, + goal_i.pose.position.y - start_i.pose.position.y) / + interpolation_resolution_; + double x_increment = (goal_i.pose.position.x - start_i.pose.position.x) / total_number_of_loop; + double y_increment = (goal_i.pose.position.y - start_i.pose.position.y) / total_number_of_loop; + + for (int i = 0; i < total_number_of_loop; ++i) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = start_i.pose.position.x + x_increment * i; + pose.pose.position.y = start_i.pose.position.y + y_increment * i; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + pose.header.stamp = node_->now(); + pose.header.frame_id = global_frame_; + global_path.poses.push_back(pose); + } + start_i = goal_i; + } global_path.poses.push_back(goal); return global_path;