Add multi-point navigation node with rotation-drift correction and AMCL initial pose alignment

[Copilot]

Three-wheel differential drive robot has two navigation issues: x/y drift after rotation-only goals, and AMCL initial pose defaulting to (0,0,0) rather than the map-building start pose.

Changes

Problem 1 — x/y drift after in-place rotation (_is_rotation_only_goal + _correct_xy_drift)

Differential drive robots physically drift when rotating in place due to wheel tolerances, uneven friction, and off-center CoM. After a rotation-only waypoint completes:

• Detect rotation-only goals: adjacent waypoints with Δx/Δy within position_tolerance but Δθ > angle_tolerance (angle diff normalized via atan2 to handle ±π wrap)
• Wait rotation_settle_wait for the robot to fully stop
• Read current AMCL pose; if Euclidean drift > xy_correction_threshold, resend a position-correction goal to the same (x, y, θ)

drift = math.hypot(cur_x - target_x, cur_y - target_y)
if drift > self.xy_correction_threshold:
    self._send_goal(target_x, target_y, target_theta)

Problem 2 — Initial pose mismatch (_set_initial_pose)

AMCL defaults to (0,0,0) on startup. If the robot is placed at the map-building origin with the same orientation this is fine, but any deviation causes localization divergence. On node startup, publish a PoseWithCovarianceStamped to /initialpose using configurable initial_pose_x/y/theta params (matching the SLAM start pose), then wait amcl_convergence_wait seconds before sending the first goal.

New package: multi_point_nav

File	Purpose
scripts/multi_point_nav.py	Navigation node implementing both fixes
launch/multi_point_nav.launch	All params exposed as launch args
config/nav_points.yaml	Waypoint list including rotation-only example

All timing constants (amcl_convergence_wait, rotation_settle_wait, waypoint_pause) are ROS-param configurable to accommodate different robot dynamics. Publisher readiness uses a connection-count poll rather than a fixed sleep.

---

💡 You can make Copilot smarter by setting up custom instructions, customizing its development environment and configuring Model Context Protocol (MCP) servers. Learn more Copilot coding agent tips in the docs.