Integrate Nav2 for waypoint following and update documentation by qza36 · Pull Request #5 · kzm784/waypoint_editor

qza36 · 2026-03-01T06:55:02Z

This pull request adds two new bridge nodes and supporting launch files to enable seamless integration between the waypoint_editor and Nav2 navigation stack. The bridge nodes allow waypoints saved in CSV format by the editor to be sent to Nav2 for navigation, supporting both "follow waypoints" (stop at each point) and "navigate through poses" (smooth traversal). The documentation is updated to explain usage, and necessary dependencies are added.

New bridge node functionality:

Added src/waypoint_to_nav2.cpp node to read CSV waypoints and send them to Nav2 using the follow_waypoints action, causing the robot to stop at each waypoint.
Added src/waypoint_through_nav2.cpp node to read CSV waypoints and send them to Nav2 using the navigate_through_poses action, enabling smooth traversal through all waypoints without stopping.

Launch and build integration:

Added launch files launch/waypoint_to_nav2.launch.py and launch/waypoint_through_nav2.launch.py for easy startup and parameter configuration of the new bridge nodes. [1] [2]
Updated CMakeLists.txt to build and install the new nodes, and added rclcpp_action as a required package. [1] [2]
Updated package.xml to include rclcpp_action as a build and exec dependency. [1] [2]

Documentation:

Added USAGE_WITH_NAV2.md, providing clear instructions for using the waypoint editor with Nav2, including workflow and launch details for both bridge modes.

Copilot

Pull request overview

This PR adds Nav2 integration bridges so waypoint_editor-generated CSV waypoints can be executed by Nav2 either as “stop at each waypoint” (follow_waypoints) or “smoothly traverse all poses” (navigate_through_poses), along with launch and documentation updates.

Changes:

Added two new ROS 2 nodes that load waypoint CSVs and dispatch Nav2 actions (follow_waypoints / navigate_through_poses).
Added launch files to run each bridge node with configurable waypoint_file and frame_id.
Updated build/dependency metadata and added usage documentation (EN + ZH).

Reviewed changes

Copilot reviewed 8 out of 8 changed files in this pull request and generated 8 comments.

Show a summary per file

File	Description
src/waypoint_to_nav2.cpp	New bridge node sending CSV waypoints via Nav2 `follow_waypoints`.
src/waypoint_through_nav2.cpp	New bridge node sending CSV waypoints via Nav2 `navigate_through_poses`.
launch/waypoint_to_nav2.launch.py	Launch entrypoint for `waypoint_to_nav2` node.
launch/waypoint_through_nav2.launch.py	Launch entrypoint for `waypoint_through_nav2` node.
CMakeLists.txt	Builds/installs the two new executables; adds `rclcpp_action`.
package.xml	Adds `rclcpp_action` dependency.
USAGE_WITH_NAV2.md	English documentation for Nav2 workflow and both bridge modes.
USAGE_WITH_NAV2.zh.md	Chinese documentation for Nav2 workflow and both bridge modes.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-03-01T06:58:17Z

+        auto send_goal_options = rclcpp_action::Client<NavigateThroughPoses>::SendGoalOptions();
+        send_goal_options.goal_response_callback =
+            std::bind(&WaypointThroughNav2::goalResponseCallback, this, std::placeholders::_1);
+        send_goal_options.feedback_callback =
+            std::bind(&WaypointThroughNav2::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2);
+        send_goal_options.result_callback =
+            std::bind(&WaypointThroughNav2::resultCallback, this, std::placeholders::_1);
+
+        nav_through_client_->async_send_goal(goal_msg, send_goal_options);
+
+        response->success = true;
+        response->message = "Poses sent to Nav2 (navigate through)";
+    }


The Trigger service returns success right after async_send_goal, but the goal can still be rejected in goal_response_callback. This makes the service response unreliable for automation. Consider waiting for the goal handle future (with a timeout) and setting response->success/message based on acceptance (or explicitly document/encode that the service only enqueues the request).

Copilot · 2026-03-01T06:58:17Z

+        RCLCPP_INFO(this->get_logger(),
+                     "Distance remaining: %.2f, poses remaining: %d",
+                     feedback->distance_remaining,
+                     feedback->number_of_poses_remaining);


Logging action feedback at INFO for every update can generate a lot of output (Nav2 feedback is often high-frequency). Consider throttling (RCLCPP_INFO_THROTTLE) or dropping to DEBUG to avoid log spam.

Suggested change

RCLCPP_INFO(this->get_logger(),

"Distance remaining: %.2f, poses remaining: %d",

feedback->distance_remaining,

feedback->number_of_poses_remaining);

RCLCPP_INFO_THROTTLE(

this->get_logger(),

*this->get_clock(),

1000,

"Distance remaining: %.2f, poses remaining: %d",

feedback->distance_remaining,

feedback->number_of_poses_remaining);

Copilot · 2026-03-01T06:58:18Z

+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    pkg = get_package_share_directory('waypoint_editor')
+


Unused imports/variables: os and get_package_share_directory are imported and pkg is assigned but never used. Please remove them to avoid lint failures and keep the launch file minimal.

Suggested change

import os

from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

pkg = get_package_share_directory('waypoint_editor')

def generate_launch_description():

Copilot · 2026-03-01T06:58:18Z

+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    pkg = get_package_share_directory('waypoint_editor')


Unused imports/variables: os and get_package_share_directory are imported and pkg is assigned but never used. Please remove them to avoid lint failures and keep the launch file minimal.

Suggested change

import os

from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

pkg = get_package_share_directory('waypoint_editor')

def generate_launch_description():

Copilot · 2026-03-01T06:58:19Z

+| Bridge Node | Nav2 Action | Behavior | Use Case |
+|-------------|-------------|----------|----------|
+| `waypoint_to_nav2` | `follow_waypoints` | Stop at each waypoint before moving to the next | Inspection, point-by-point tasks |
+| `waypoint_through_nav2` | `navigate_through_poses` | Plan a smooth path through all waypoints without stopping | Patrol, fast traversal |


This Markdown table uses double pipes (e.g., "|| Bridge Node ..."), which renders incorrectly in most Markdown parsers. Replace the leading/trailing "||" with standard single-pipe table syntax ("| ... |").

Copilot · 2026-03-01T06:58:19Z

+| 桥接节点 | Nav2 Action | 行为 | 适用场景 |
+|---------|-------------|------|----------|
+| `waypoint_to_nav2` | `follow_waypoints` | 逐点停靠 — 到达每个航点后停下再前往下一个 | 巡检、逐点任务 |
+| `waypoint_through_nav2` | `navigate_through_poses` | 平滑穿越 — 规划连续路径经过所有航点，不停留 | 巡逻、快速穿越 |


该 Markdown 表格使用了双竖线（例如 "|| 桥接节点 ..."），多数 Markdown 渲染器会无法正确显示。建议改为标准单竖线表格语法（"| ... |"）。

Copilot · 2026-03-01T06:58:19Z

+        follow_waypoints_client_->async_send_goal(goal_msg, send_goal_options);
+
+        response->success = true;
+        response->message = "Waypoints sent to Nav2";


The Trigger service reports success immediately after calling async_send_goal, even though Nav2 may still reject the goal (goal_response_callback). This can mislead callers into thinking waypoint following started successfully. Consider waiting for the goal handle future (with a timeout) and setting response->success/message based on whether the goal was accepted, or change the service contract/message to clearly indicate the request was only queued.

Suggested change

response->message = "Waypoints sent to Nav2";

response->message = "Waypoints request sent to Nav2; goal acceptance not yet confirmed";

Copilot · 2026-03-01T06:58:19Z

+        GoalHandleFollowWaypoints::SharedPtr,
+        const std::shared_ptr<const FollowWaypoints::Feedback> feedback)
+    {
+        RCLCPP_INFO(this->get_logger(), "Current waypoint: %u", feedback->current_waypoint);


This feedback callback logs at INFO on every action feedback update, which can spam logs and impact runtime performance. Consider using RCLCPP_DEBUG, or RCLCPP_INFO_THROTTLE with a reasonable period.

Suggested change

RCLCPP_INFO(this->get_logger(), "Current waypoint: %u", feedback->current_waypoint);

RCLCPP_INFO_THROTTLE(

this->get_logger(),

*this->get_clock(),

1000,

"Current waypoint: %u",

feedback->current_waypoint);

qza36 added 4 commits November 6, 2025 18:52

feat:Add Nav2 integration bridge for waypoint following

23cca51

feat:nav through poses

be2e21f

doc:update USAGE_WITH_NAV2.md

0f719f7

doc:add en version USAGE_WITH_NAV2.md

75c17c2

Copilot AI review requested due to automatic review settings March 1, 2026 06:55

Copilot started reviewing on behalf of qza36 March 1, 2026 06:55 View session

Copilot AI reviewed Mar 1, 2026

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Integrate Nav2 for waypoint following and update documentation#5

Integrate Nav2 for waypoint following and update documentation#5
qza36 wants to merge 4 commits into
kzm784:mainfrom
qza36:feature/adaptToNav2

qza36 commented Mar 1, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Mar 1, 2026

Uh oh!

Copilot AI Mar 1, 2026

Uh oh!

Copilot AI Mar 1, 2026

Uh oh!

Copilot AI Mar 1, 2026

Uh oh!

Copilot AI Mar 1, 2026

Uh oh!

Copilot AI Mar 1, 2026

Uh oh!

Copilot AI Mar 1, 2026

Uh oh!

Copilot AI Mar 1, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

	response->message = "Waypoints sent to Nav2";
	response->message = "Waypoints request sent to Nav2; goal acceptance not yet confirmed";

Conversation

qza36 commented Mar 1, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Mar 1, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 1, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 1, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 1, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 1, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 1, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 1, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Mar 1, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants