目前实现较快速从原点到达某个点，但是然后回到原点时有较大问题

src/planner/scripts/topology_pure_pursuit_node.py

@@ -10,7 +10,7 @@ from geometry_msgs.msg import PoseStamped, Twist
 from nav_msgs.msg import Odometry, Path as NavPath
 from rclpy.node import Node
 from rclpy.time import Time
-from std_msgs.msg import String
+from std_msgs.msg import Bool, String
 import yaml
 
 from rclpy.duration import Duration
@@ -53,6 +53,7 @@ class TopologyPurePursuit(Node):
         self.declare_parameter("plan_topic", "/plan")
         self.declare_parameter("cmd_vel_topic", "/cmd_vel")
         self.declare_parameter("status_topic", "/topology_status")
+        self.declare_parameter("cancel_topic", "/navigation_cancel")
         self.declare_parameter("enable_topology_planning", True)
         self.declare_parameter("accept_external_plan", False)
         self.declare_parameter("publish_cmd_vel", True)
@@ -69,11 +70,16 @@ class TopologyPurePursuit(Node):
         self.declare_parameter("curvature_slowdown_gain", 0.18)
         self.declare_parameter("max_angular_speed", 1.5)
         self.declare_parameter("min_turning_radius", 0.35)
+        self.declare_parameter("goal_yaw_tolerance", 0.10)
+        self.declare_parameter("align_goal_yaw", True)
+        self.declare_parameter("final_alignment_offset", 0.25)
+        self.declare_parameter("final_alignment_speed", 0.10)
         self.declare_parameter("latency_compensation", True)
         self.declare_parameter("max_latency_compensation", 0.25)
         self.declare_parameter("max_odom_age", 0.80)
         self.declare_parameter("tf_timeout", 0.05)
         self.declare_parameter("stop_without_plan", True)
+        self.declare_parameter("state_log_period", 1.0)
 
         self.map_frame = self.get_parameter("map_frame").value
         self.enable_topology_planning = bool(
@@ -107,6 +113,16 @@ class TopologyPurePursuit(Node):
         self.min_turning_radius = float(
             self.get_parameter("min_turning_radius").value
         )
+        self.goal_yaw_tolerance = float(
+            self.get_parameter("goal_yaw_tolerance").value
+        )
+        self.align_goal_yaw = bool(self.get_parameter("align_goal_yaw").value)
+        self.final_alignment_offset = max(
+            0.05, float(self.get_parameter("final_alignment_offset").value)
+        )
+        self.final_alignment_speed = max(
+            self.min_linear_speed, float(self.get_parameter("final_alignment_speed").value)
+        )
         self.latency_compensation = bool(
             self.get_parameter("latency_compensation").value
         )
@@ -116,6 +132,9 @@ class TopologyPurePursuit(Node):
         self.max_odom_age = float(self.get_parameter("max_odom_age").value)
         self.tf_timeout = float(self.get_parameter("tf_timeout").value)
         self.stop_without_plan = bool(self.get_parameter("stop_without_plan").value)
+        self.state_log_period = max(
+            0.0, float(self.get_parameter("state_log_period").value)
+        )
 
         self.nodes = {}
         self.edges = {}
@@ -139,6 +158,9 @@ class TopologyPurePursuit(Node):
         self.completed = True
         self.tried_default_goal = False
         self.last_tf_warn_ns = 0
+        self.last_state_log_ns = 0
+        self.last_cmd_linear = 0.0
+        self.last_cmd_angular = 0.0
 
         self.plan_pub = self.create_publisher(
             NavPath, self.get_parameter("plan_topic").value, 10
@@ -156,6 +178,12 @@ class TopologyPurePursuit(Node):
             self._odom_callback,
             10,
         )
+        self.cancel_sub = self.create_subscription(
+            Bool,
+            self.get_parameter("cancel_topic").value,
+            self._cancel_callback,
+            10,
+        )
         self.goal_pose_sub = None
         self.goal_node_sub = None
         self.external_plan_sub = None
@@ -183,7 +211,9 @@ class TopologyPurePursuit(Node):
         period = 1.0 / max(1.0, float(self.get_parameter("control_rate").value))
         self.timer = self.create_timer(period, self._control_loop)
         self._publish_status(
-            f"ready graph_nodes={len(self.nodes)} graph_edges={sum(len(v) for v in self.edges.values())}"
+            f"ready graph_nodes={len(self.nodes)} "
+            f"graph_edges={sum(len(v) for v in self.edges.values())} "
+            f"motion_enabled={self.publish_cmd_vel}"
         )
 
     def _load_graph(self, graph_file_value):
@@ -295,7 +325,23 @@ class TopologyPurePursuit(Node):
         self.active_goal_node = "external_plan"
         self.current_path_index = 0
         self.completed = False
-        self._publish_status(f"external_plan_ok points={len(self.active_path)}")
+        path_length = self._path_length(self.active_path)
+        goal_x, goal_y, goal_yaw = self.active_path[-1]
+        self._publish_status(
+            f"external_plan_ok points={len(self.active_path)} "
+            f"length={path_length:.3f} goal=({goal_x:.3f},{goal_y:.3f},{goal_yaw:.3f})"
+        )
+
+    def _cancel_callback(self, msg):
+        if not msg.data:
+            return
+        self.active_path = []
+        self.active_node_path = []
+        self.active_goal_node = ""
+        self.current_path_index = 0
+        self.completed = True
+        self._publish_cmd(0.0, 0.0)
+        self._publish_status("navigation_cancelled")
 
     def plan_to_node(self, goal_node):
         if goal_node not in self.nodes:
@@ -496,31 +542,57 @@ class TopologyPurePursuit(Node):
 
     def _control_loop(self):
         if not self.publish_cmd_vel:
+            pose = self._current_pose(compensate=False)
+            state_args = {"pose": pose, "mode": "motion_disabled"}
+            if pose is not None and self.active_path:
+                x, y, yaw, _, _ = pose
+                goal_x, goal_y, goal_yaw = self.active_path[-1]
+                state_args["goal_distance"] = math.hypot(goal_x - x, goal_y - y)
+                state_args["goal_yaw_error"] = abs(normalize_angle(goal_yaw - yaw))
+                nearest_index = self._nearest_path_index(x, y)
+                nearest_x, nearest_y, _ = self.active_path[nearest_index]
+                state_args["nearest_index"] = nearest_index
+                state_args["cross_track_error"] = math.hypot(nearest_x - x, nearest_y - y)
+            self._maybe_log_state(**state_args)
             return
 
         pose = self._current_pose(compensate=True)
         if pose is None:
             self._stop_if_needed("stop no_recent_odom")
+            self._maybe_log_state(pose=None, mode="no_recent_odom")
             return
 
         if self.completed or len(self.active_path) < 2:
             if self.stop_without_plan:
                 self._publish_cmd(0.0, 0.0)
+            self._maybe_log_state(pose=pose, mode="idle")
             return
 
         x, y, yaw, _, _ = pose
         goal_x, goal_y, _ = self.active_path[-1]
+        goal_yaw = self.active_path[-1][2]
         goal_distance = math.hypot(goal_x - x, goal_y - y)
+        goal_yaw_error = abs(normalize_angle(goal_yaw - yaw))
         if goal_distance <= self.goal_tolerance:
+            if self.align_goal_yaw and goal_yaw_error > self.goal_yaw_tolerance:
+                self._align_final_yaw(
+                    x, y, yaw, goal_x, goal_y, goal_yaw, goal_yaw_error
+                )
+                return
             self.completed = True
             self._publish_cmd(0.0, 0.0)
-            self._publish_status(f"goal_reached {self.active_goal_node}")
+            self._publish_status(
+                f"goal_reached {self.active_goal_node} "
+                f"goal_error={goal_distance:.3f} yaw_error={goal_yaw_error:.3f}"
+            )
             return
 
         nearest_index = self._nearest_path_index(x, y)
         self.current_path_index = nearest_index
         target_index = self._lookahead_index(x, y, nearest_index)
         target_x, target_y, target_path_yaw = self.active_path[target_index]
+        nearest_x, nearest_y, _ = self.active_path[nearest_index]
+        cross_track_error = math.hypot(nearest_x - x, nearest_y - y)
 
         dx = target_x - x
         dy = target_y - y
@@ -540,6 +612,51 @@ class TopologyPurePursuit(Node):
         angular = speed * curvature
         angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
         self._publish_cmd(speed, angular)
+        self._maybe_log_state(
+            pose=pose,
+            mode="tracking",
+            goal_distance=goal_distance,
+            goal_yaw_error=goal_yaw_error,
+            nearest_index=nearest_index,
+            target_index=target_index,
+            cross_track_error=cross_track_error,
+            reverse=reverse,
+            curvature=curvature,
+            lookahead=lookahead,
+        )
+
+    def _align_final_yaw(self, x, y, yaw, goal_x, goal_y, goal_yaw, goal_yaw_error):
+        reverse = math.cos(normalize_angle(goal_yaw - yaw)) < 0.0
+        target_x = goal_x - self.final_alignment_offset * math.cos(goal_yaw) if reverse else goal_x + self.final_alignment_offset * math.cos(goal_yaw)
+        target_y = goal_y - self.final_alignment_offset * math.sin(goal_yaw) if reverse else goal_y + self.final_alignment_offset * math.sin(goal_yaw)
+        control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw
+        dx = target_x - x
+        dy = target_y - y
+        local_x = math.cos(control_yaw) * dx + math.sin(control_yaw) * dy
+        local_y = -math.sin(control_yaw) * dx + math.cos(control_yaw) * dy
+        lookahead = max(0.05, math.hypot(local_x, local_y))
+        curvature = 2.0 * local_y / (lookahead * lookahead)
+        if self.min_turning_radius > 1e-3:
+            max_curvature = 1.0 / self.min_turning_radius
+            curvature = max(-max_curvature, min(max_curvature, curvature))
+        speed = -self.final_alignment_speed if reverse else self.final_alignment_speed
+        angular = speed * curvature
+        angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
+        self._publish_cmd(speed, angular)
+        self._maybe_log_state(
+            pose=(x, y, yaw, 0.0, 0.0),
+            mode="align_goal_yaw",
+            goal_distance=math.hypot(goal_x - x, goal_y - y),
+            goal_yaw_error=goal_yaw_error,
+            nearest_index=self.current_path_index,
+            target_index=len(self.active_path) - 1 if self.active_path else 0,
+            reverse=reverse,
+            curvature=curvature,
+            lookahead=lookahead,
+        )
+        self._publish_status(
+            f"aligning_goal_yaw error={goal_yaw_error:.3f} reverse={reverse}"
+        )
 
     def _nearest_path_index(self, x, y):
         start = max(0, self.current_path_index - 5)
@@ -583,6 +700,62 @@ class TopologyPurePursuit(Node):
             return fallback_yaw
         return math.atan2(ty - sy, tx - sx)
 
+    @staticmethod
+    def _path_length(path):
+        return sum(
+            math.hypot(b[0] - a[0], b[1] - a[1])
+            for a, b in zip(path, path[1:])
+        )
+
+    def _maybe_log_state(
+        self,
+        pose=None,
+        mode="idle",
+        goal_distance=None,
+        goal_yaw_error=None,
+        nearest_index=None,
+        target_index=None,
+        cross_track_error=None,
+        reverse=False,
+        curvature=None,
+        lookahead=None,
+    ):
+        if self.state_log_period <= 0.0:
+            return
+        now_ns = self.get_clock().now().nanoseconds
+        min_period_ns = int(self.state_log_period * 1_000_000_000)
+        if now_ns - self.last_state_log_ns < min_period_ns:
+            return
+        self.last_state_log_ns = now_ns
+
+        parts = [f"tracker_state mode={mode}"]
+        if pose is None:
+            parts.append("pose=unavailable")
+        else:
+            x, y, yaw, vx, wz = pose
+            parts.append(f"pose=({x:.3f},{y:.3f},{yaw:.3f})")
+            parts.append(f"odom_twist=({vx:.3f},{wz:.3f})")
+
+        parts.append(f"path_points={len(self.active_path)}")
+        parts.append(f"completed={self.completed}")
+        if goal_distance is not None:
+            parts.append(f"goal_distance={goal_distance:.3f}")
+        if goal_yaw_error is not None:
+            parts.append(f"yaw_error={goal_yaw_error:.3f}")
+        if nearest_index is not None:
+            parts.append(f"nearest_index={nearest_index}")
+        if target_index is not None:
+            parts.append(f"target_index={target_index}")
+        if cross_track_error is not None:
+            parts.append(f"cross_track={cross_track_error:.3f}")
+        if curvature is not None:
+            parts.append(f"curvature={curvature:.3f}")
+        if lookahead is not None:
+            parts.append(f"lookahead={lookahead:.3f}")
+        parts.append(f"reverse={reverse}")
+        parts.append(f"cmd=({self.last_cmd_linear:.3f},{self.last_cmd_angular:.3f})")
+        self.get_logger().info(" ".join(parts))
+
     def _target_speed(self, abs_curvature, goal_distance, reverse):
         base_speed = self.reverse_speed if reverse else self.linear_speed
         speed = base_speed
@@ -594,9 +767,11 @@ class TopologyPurePursuit(Node):
         return -speed if reverse else speed
 
     def _publish_cmd(self, linear, angular):
+        self.last_cmd_linear = float(linear)
+        self.last_cmd_angular = float(angular)
         cmd = Twist()
-        cmd.linear.x = float(linear)
-        cmd.angular.z = float(angular)
+        cmd.linear.x = self.last_cmd_linear
+        cmd.angular.z = self.last_cmd_angular
         self.cmd_pub.publish(cmd)
 
     def _stop_if_needed(self, reason):