#!/usr/bin/env python3 """Collect data needed to diagnose why the robot does not reach a goal. This node is read-only. It subscribes to planner/tracker topics, queries key parameters, samples odom/command state, and writes one JSON report. """ import json import math from pathlib import Path import re import time import rclpy from geometry_msgs.msg import PoseStamped, Twist from nav_msgs.msg import Odometry, Path as NavPath from rcl_interfaces.msg import ParameterType from rcl_interfaces.srv import GetParameters from rclpy.duration import Duration from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from rclpy.time import Time from std_msgs.msg import String from tf2_ros import Buffer, TransformException, TransformListener try: from ackermann_msgs.msg import AckermannDriveStamped except ImportError: # pragma: no cover - ackermann_msgs is a runtime dependency. AckermannDriveStamped = None PLAN_OK_RE = re.compile( r"plan_ok.*points=(?P\d+).*iterations=(?P\d+)" r".*planning_ms=(?P[-+]?\d+(?:\.\d+)?)" r".*goal_error=(?P[-+]?\d+(?:\.\d+)?)" r".*yaw_error=(?P[-+]?\d+(?:\.\d+)?)" ) def yaw_from_quaternion(q): return math.atan2( 2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z), ) def normalize_angle(angle): return math.atan2(math.sin(angle), math.cos(angle)) def finite_or_none(value): if value is None: return None if isinstance(value, float) and not math.isfinite(value): return None return value def endpoint_to_dict(endpoint): return { "node_name": getattr(endpoint, "node_name", ""), "node_namespace": getattr(endpoint, "node_namespace", ""), "topic_type": getattr(endpoint, "topic_type", ""), } def parameter_value_to_python_compatible(value): """Convert rcl_interfaces/msg/ParameterValue on ROS 2 Humble and later.""" if value.type == ParameterType.PARAMETER_BOOL: return bool(value.bool_value) if value.type == ParameterType.PARAMETER_INTEGER: return int(value.integer_value) if value.type == ParameterType.PARAMETER_DOUBLE: return float(value.double_value) if value.type == ParameterType.PARAMETER_STRING: return str(value.string_value) if value.type == ParameterType.PARAMETER_BYTE_ARRAY: return list(value.byte_array_value) if value.type == ParameterType.PARAMETER_BOOL_ARRAY: return list(value.bool_array_value) if value.type == ParameterType.PARAMETER_INTEGER_ARRAY: return list(value.integer_array_value) if value.type == ParameterType.PARAMETER_DOUBLE_ARRAY: return list(value.double_array_value) if value.type == ParameterType.PARAMETER_STRING_ARRAY: return list(value.string_array_value) return None class GoalReachDebugCollector(Node): def __init__(self): super().__init__("goal_reach_debug_collector") self.declare_parameter("duration", 30.0) self.declare_parameter("sample_rate", 10.0) self.declare_parameter("output_dir", "~/yiliao_ws/test_results") self.declare_parameter("test_name", "goal_reach_debug") self.declare_parameter("map_frame", "map") self.declare_parameter("odom_topic", "/odom") self.declare_parameter("goal_pose_topic", "/goal_pose") self.declare_parameter("plan_topic", "/plan") self.declare_parameter("cmd_vel_topic", "/planner_cmd_vel") self.declare_parameter("ackermann_cmd_topic", "/ackermann_cmd") self.declare_parameter("planner_status_topic", "/hybrid_astar_status") self.declare_parameter("tracker_status_topic", "/pure_pursuit_status") self.declare_parameter("planner_node", "/grid_astar_theta_planner") self.declare_parameter("tracker_node", "/topology_pure_pursuit") self.declare_parameter("tf_timeout", 0.10) self.declare_parameter("max_odom_age", 2.0) self.declare_parameter("include_plan_points", True) self.duration = max(1.0, float(self.get_parameter("duration").value)) self.sample_rate = max(1.0, float(self.get_parameter("sample_rate").value)) self.map_frame = str(self.get_parameter("map_frame").value) self.odom_topic = str(self.get_parameter("odom_topic").value) self.goal_pose_topic = str(self.get_parameter("goal_pose_topic").value) self.plan_topic = str(self.get_parameter("plan_topic").value) self.cmd_vel_topic = str(self.get_parameter("cmd_vel_topic").value) self.ackermann_cmd_topic = str(self.get_parameter("ackermann_cmd_topic").value) self.planner_status_topic = str( self.get_parameter("planner_status_topic").value ) self.tracker_status_topic = str( self.get_parameter("tracker_status_topic").value ) self.planner_node = str(self.get_parameter("planner_node").value) self.tracker_node = str(self.get_parameter("tracker_node").value) self.tf_timeout = float(self.get_parameter("tf_timeout").value) self.max_odom_age = float(self.get_parameter("max_odom_age").value) self.include_plan_points = bool( self.get_parameter("include_plan_points").value ) output_dir = Path(str(self.get_parameter("output_dir").value)).expanduser() output_dir.mkdir(parents=True, exist_ok=True) timestamp = time.strftime("%Y%m%d_%H%M%S") test_name = str(self.get_parameter("test_name").value) self.output_path = output_dir / f"{test_name}_{timestamp}.json" self.started_wall = time.time() self.started_mono = time.monotonic() self.finished = False self.latest_odom = None self.latest_goal = None self.latest_plan_points = [] self.latest_cmd = None self.latest_ackermann = None self.latest_planner_status = "" self.latest_tracker_status = "" self.last_plan_ok = None self.remote_parameters = {} self.samples = [] self.planner_status_events = [] self.tracker_status_events = [] self.plan_events = [] self.goal_events = [] self.min_distance_to_goal = None self.min_distance_to_plan_end = None self.max_abs_cmd_linear = 0.0 self.max_abs_cmd_angular = 0.0 self.max_abs_ackermann_speed = 0.0 self.max_abs_steering_angle = 0.0 self.tf_failures = 0 self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) goal_qos = QoSProfile( depth=10, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, ) self.subscriptions_keepalive = [ self.create_subscription(Odometry, self.odom_topic, self._odom_cb, 50), self.create_subscription( PoseStamped, self.goal_pose_topic, self._goal_cb, goal_qos ), self.create_subscription(NavPath, self.plan_topic, self._plan_cb, 10), self.create_subscription(Twist, self.cmd_vel_topic, self._cmd_cb, 50), self.create_subscription( String, self.planner_status_topic, self._planner_status_cb, 50 ), self.create_subscription( String, self.tracker_status_topic, self._tracker_status_cb, 50 ), ] if AckermannDriveStamped is not None: self.subscriptions_keepalive.append( self.create_subscription( AckermannDriveStamped, self.ackermann_cmd_topic, self._ackermann_cb, 50, ) ) self.sample_timer = self.create_timer(1.0 / self.sample_rate, self._sample) self.done_timer = self.create_timer(0.20, self._maybe_finish) self.get_logger().info( f"collecting goal reach debug data for {self.duration:.1f}s -> {self.output_path}" ) def elapsed(self): return max(0.0, time.monotonic() - self.started_mono) def _odom_cb(self, msg): self.latest_odom = msg def _goal_cb(self, msg): goal = { "elapsed_sec": self.elapsed(), "frame_id": msg.header.frame_id or self.map_frame, "x": msg.pose.position.x, "y": msg.pose.position.y, "yaw": yaw_from_quaternion(msg.pose.orientation), } self.latest_goal = goal self.goal_events.append(goal) def _plan_cb(self, msg): points = [ { "x": pose.pose.position.x, "y": pose.pose.position.y, "yaw": yaw_from_quaternion(pose.pose.orientation), } for pose in msg.poses ] self.latest_plan_points = points summary = self._plan_summary(points, msg.header.frame_id or self.map_frame) summary["elapsed_sec"] = self.elapsed() self.plan_events.append(summary) def _cmd_cb(self, msg): self.latest_cmd = { "linear_x": msg.linear.x, "angular_z": msg.angular.z, } self.max_abs_cmd_linear = max(self.max_abs_cmd_linear, abs(msg.linear.x)) self.max_abs_cmd_angular = max(self.max_abs_cmd_angular, abs(msg.angular.z)) def _ackermann_cb(self, msg): self.latest_ackermann = { "speed": msg.drive.speed, "steering_angle": msg.drive.steering_angle, } self.max_abs_ackermann_speed = max( self.max_abs_ackermann_speed, abs(msg.drive.speed) ) self.max_abs_steering_angle = max( self.max_abs_steering_angle, abs(msg.drive.steering_angle) ) def _planner_status_cb(self, msg): self.latest_planner_status = msg.data event = {"elapsed_sec": self.elapsed(), "data": msg.data} parsed = self._parse_plan_ok(msg.data) if parsed is not None: event["parsed_plan_ok"] = parsed self.last_plan_ok = parsed self.planner_status_events.append(event) def _tracker_status_cb(self, msg): self.latest_tracker_status = msg.data self.tracker_status_events.append( {"elapsed_sec": self.elapsed(), "data": msg.data} ) def _parse_plan_ok(self, text): match = PLAN_OK_RE.search(text) if not match: return None parsed = {} for key, value in match.groupdict().items(): parsed[key] = int(value) if key in {"points", "iterations"} else float(value) return parsed def _sample(self): pose = self._current_pose_map() pose_available = self._pose_available(pose) goal = self.latest_goal plan_end = self.latest_plan_points[-1] if self.latest_plan_points else None distance_to_goal = None yaw_error_to_goal = None if pose_available and goal is not None: distance_to_goal = math.hypot(goal["x"] - pose["x"], goal["y"] - pose["y"]) yaw_error_to_goal = abs(normalize_angle(goal["yaw"] - pose["yaw"])) self.min_distance_to_goal = self._min_or_value( self.min_distance_to_goal, distance_to_goal ) distance_to_plan_end = None yaw_error_to_plan_end = None if pose_available and plan_end is not None: distance_to_plan_end = math.hypot( plan_end["x"] - pose["x"], plan_end["y"] - pose["y"] ) yaw_error_to_plan_end = abs(normalize_angle(plan_end["yaw"] - pose["yaw"])) self.min_distance_to_plan_end = self._min_or_value( self.min_distance_to_plan_end, distance_to_plan_end ) plan_end_to_goal = None plan_end_yaw_to_goal = None if plan_end is not None and goal is not None: plan_end_to_goal = math.hypot(plan_end["x"] - goal["x"], plan_end["y"] - goal["y"]) plan_end_yaw_to_goal = abs(normalize_angle(plan_end["yaw"] - goal["yaw"])) self.samples.append({ "elapsed_sec": self.elapsed(), "pose_map": pose, "goal": goal, "plan_end": plan_end, "distance_to_goal": finite_or_none(distance_to_goal), "yaw_error_to_goal": finite_or_none(yaw_error_to_goal), "distance_to_plan_end": finite_or_none(distance_to_plan_end), "yaw_error_to_plan_end": finite_or_none(yaw_error_to_plan_end), "plan_end_to_goal": finite_or_none(plan_end_to_goal), "plan_end_yaw_to_goal": finite_or_none(plan_end_yaw_to_goal), "cmd_vel": self.latest_cmd, "ackermann_cmd": self.latest_ackermann, "planner_status": self.latest_planner_status, "tracker_status": self.latest_tracker_status, }) def _min_or_value(self, old, new): if new is None: return old if old is None: return new return min(old, new) def _pose_available(self, pose): return ( isinstance(pose, dict) and pose.get("available") is True and "x" in pose and "y" in pose and "yaw" in pose ) def _current_pose_map(self): if self.latest_odom is None: return None msg = self.latest_odom stamp = Time.from_msg(msg.header.stamp) if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0: age = 0.0 else: age = (self.get_clock().now() - stamp).nanoseconds * 1e-9 if age < 0.0: age = 0.0 if age > self.max_odom_age: return { "available": False, "reason": "odom_too_old", "age_sec": age, } source_frame = msg.header.frame_id or self.map_frame x = msg.pose.pose.position.x y = msg.pose.pose.position.y yaw = yaw_from_quaternion(msg.pose.pose.orientation) transformed = self._transform_pose_2d(x, y, yaw, source_frame) if transformed is None: return { "available": False, "reason": "tf_unavailable", "source_frame": source_frame, } x, y, yaw = transformed return { "available": True, "frame_id": self.map_frame, "source_frame": source_frame, "x": x, "y": y, "yaw": yaw, "odom_age_sec": age, "odom_linear_x": msg.twist.twist.linear.x, "odom_angular_z": msg.twist.twist.angular.z, } def _transform_pose_2d(self, x, y, yaw, source_frame): if source_frame == self.map_frame: return x, y, yaw try: transform = self.tf_buffer.lookup_transform( self.map_frame, source_frame, Time(), timeout=Duration(seconds=self.tf_timeout), ) except TransformException: self.tf_failures += 1 return None t = transform.transform.translation transform_yaw = yaw_from_quaternion(transform.transform.rotation) cos_yaw = math.cos(transform_yaw) sin_yaw = math.sin(transform_yaw) target_x = t.x + cos_yaw * x - sin_yaw * y target_y = t.y + sin_yaw * x + cos_yaw * y target_yaw = normalize_angle(transform_yaw + yaw) return target_x, target_y, target_yaw def _plan_summary(self, points, frame_id): if not points: return { "frame_id": frame_id, "points": 0, "length": 0.0, "start": None, "end": None, } length = 0.0 reverse_segments = 0 for a, b in zip(points, points[1:]): length += math.hypot(b["x"] - a["x"], b["y"] - a["y"]) heading = math.atan2(b["y"] - a["y"], b["x"] - a["x"]) if math.cos(normalize_angle(a["yaw"] - heading)) < 0.0: reverse_segments += 1 return { "frame_id": frame_id, "points": len(points), "length": length, "start": points[0], "end": points[-1], "reverse_segment_fraction": ( reverse_segments / max(1, len(points) - 1) ), } def _maybe_finish(self): if self.elapsed() >= self.duration: self.finished = True def collect_remote_parameters(self): parameter_targets = { "planner": ( self.planner_node, [ "goal_xy_tolerance", "goal_yaw_tolerance", "require_goal_yaw", "resolution", "yaw_bins", "primitive_length", "min_turning_radius", "heuristic_weight", "goal_yaw_heuristic_weight", "planning_timeout_ms", "max_iterations", ], ), "tracker": ( self.tracker_node, [ "goal_tolerance", "goal_yaw_tolerance", "align_goal_yaw", "publish_cmd_vel", "cmd_vel_topic", "lookahead_distance", "linear_speed", "min_linear_speed", "reverse_speed", "slowdown_distance", "curvature_slowdown_gain", "min_turning_radius", "max_angular_speed", ], ), } for label, (node_name, names) in parameter_targets.items(): normalized_node_name = node_name if node_name.startswith("/") else f"/{node_name}" service_name = f"{normalized_node_name}/get_parameters" client = self.create_client(GetParameters, service_name) if not client.wait_for_service(timeout_sec=1.0): self.remote_parameters[label] = { "node": node_name, "service": service_name, "error": "parameter_service_unavailable", } continue request = GetParameters.Request() request.names = list(names) future = client.call_async(request) rclpy.spin_until_future_complete(self, future, timeout_sec=2.0) if not future.done() or future.result() is None: self.remote_parameters[label] = { "node": node_name, "service": service_name, "error": "parameter_query_timeout", } continue response = future.result() self.remote_parameters[label] = { "node": node_name, "service": service_name, "values": { name: parameter_value_to_python_compatible(value) for name, value in zip(names, response.values) }, } def write_report(self): report = { "metadata": { "started_wall_time": self.started_wall, "duration_sec": self.elapsed(), "output_path": str(self.output_path), }, "topics": { "odom": self.odom_topic, "goal_pose": self.goal_pose_topic, "plan": self.plan_topic, "cmd_vel": self.cmd_vel_topic, "ackermann_cmd": self.ackermann_cmd_topic, "planner_status": self.planner_status_topic, "tracker_status": self.tracker_status_topic, }, "topic_endpoints": self._topic_endpoints_report(), "remote_parameters": self.remote_parameters, "latest": { "goal": self.latest_goal, "plan": self._plan_summary(self.latest_plan_points, self.map_frame), "pose_map": self._current_pose_map(), "cmd_vel": self.latest_cmd, "ackermann_cmd": self.latest_ackermann, "planner_status": self.latest_planner_status, "tracker_status": self.latest_tracker_status, "last_plan_ok": self.last_plan_ok, }, "metrics": self._metrics(), "diagnostic_hints": self._diagnostic_hints(), "events": { "goals": self.goal_events, "plans": self.plan_events, "planner_status": self.planner_status_events, "tracker_status": self.tracker_status_events, }, "samples": self.samples, } if self.include_plan_points: report["latest"]["plan_points"] = self.latest_plan_points self.output_path.write_text( json.dumps(report, indent=2, ensure_ascii=False), encoding="utf-8", ) self.get_logger().info(f"wrote goal reach debug report: {self.output_path}") for hint in report["diagnostic_hints"]: self.get_logger().warn(hint) def _topic_endpoints_report(self): topics = [ self.odom_topic, self.goal_pose_topic, self.plan_topic, self.cmd_vel_topic, self.ackermann_cmd_topic, self.planner_status_topic, self.tracker_status_topic, ] report = {} for topic in topics: publishers = self.get_publishers_info_by_topic(topic) subscribers = self.get_subscriptions_info_by_topic(topic) report[topic] = { "publisher_count": len(publishers), "subscriber_count": len(subscribers), "publishers": [endpoint_to_dict(endpoint) for endpoint in publishers], "subscribers": [endpoint_to_dict(endpoint) for endpoint in subscribers], } return report def _metrics(self): final = self.samples[-1] if self.samples else {} planner_texts = [event["data"] for event in self.planner_status_events] tracker_texts = [event["data"] for event in self.tracker_status_events] return { "sample_count": len(self.samples), "goal_count": len(self.goal_events), "plan_count": len(self.plan_events), "planner_status_count": len(self.planner_status_events), "tracker_status_count": len(self.tracker_status_events), "min_distance_to_goal": finite_or_none(self.min_distance_to_goal), "min_distance_to_plan_end": finite_or_none(self.min_distance_to_plan_end), "final_distance_to_goal": final.get("distance_to_goal"), "final_distance_to_plan_end": final.get("distance_to_plan_end"), "final_plan_end_to_goal": final.get("plan_end_to_goal"), "final_plan_end_yaw_to_goal": final.get("plan_end_yaw_to_goal"), "max_abs_cmd_linear": self.max_abs_cmd_linear, "max_abs_cmd_angular": self.max_abs_cmd_angular, "max_abs_ackermann_speed": self.max_abs_ackermann_speed, "max_abs_steering_angle": self.max_abs_steering_angle, "tf_failures": self.tf_failures, "saw_plan_ok": any("plan_ok" in text for text in planner_texts), "saw_plan_failed": any("plan_failed" in text for text in planner_texts), "saw_goal_reached": any("goal_reached" in text for text in tracker_texts), "saw_align_goal_yaw": any("aligning_goal_yaw" in text for text in tracker_texts), "saw_no_recent_odom": any("no_recent_odom" in text for text in tracker_texts + planner_texts), } def _diagnostic_hints(self): hints = [] params = self.remote_parameters planner = params.get("planner", {}).get("values", {}) tracker = params.get("tracker", {}).get("values", {}) metrics = self._metrics() if not self.latest_goal: hints.append("没有采集到 /goal_pose；请先运行采集脚本，再发送目标。") if not self.latest_plan_points: hints.append("没有采集到 /plan；planner 可能未规划成功或采集脚本启动太晚。") if self.latest_odom is None: hints.append(f"没有采集到 {self.odom_topic}；tracker 无法闭环到点。") if metrics["tf_failures"] > 0: hints.append( f"TF 查询失败 {metrics['tf_failures']} 次；需要检查 map->odom 和 odom frame。" ) planner_xy = planner.get("goal_xy_tolerance") tracker_xy = tracker.get("goal_tolerance") if isinstance(planner_xy, (int, float)) and isinstance(tracker_xy, (int, float)): if planner_xy > tracker_xy: hints.append( "planner 的 goal_xy_tolerance 大于 tracker 的 goal_tolerance；" "planner 可能发布一个离原始目标较远的路径终点。" ) planner_yaw = planner.get("goal_yaw_tolerance") tracker_yaw = tracker.get("goal_yaw_tolerance") planner_requires_yaw = bool(planner.get("require_goal_yaw")) tracker_aligns_yaw = bool(tracker.get("align_goal_yaw")) if ( planner_requires_yaw and tracker_aligns_yaw and isinstance(planner_yaw, (int, float)) and isinstance(tracker_yaw, (int, float)) ): if planner_yaw > tracker_yaw: hints.append( "planner 的 goal_yaw_tolerance 大于 tracker 的 goal_yaw_tolerance；" "planner 认为合格的终点 yaw，tracker 可能继续调整。" ) min_speed = tracker.get("min_linear_speed") if isinstance(min_speed, (int, float)) and isinstance(tracker_xy, (int, float)): if min_speed > tracker_xy: hints.append( "tracker 的 min_linear_speed 数值大于 goal_tolerance；" "接近终点时可能仍以较高速度越过目标。" ) plan_goal_error = metrics.get("final_plan_end_to_goal") if isinstance(plan_goal_error, (int, float)) and isinstance(tracker_xy, (int, float)): if plan_goal_error > tracker_xy: hints.append( "最新 /plan 的终点到 /goal_pose 的距离大于 tracker 到点半径；" "tracker 跟随 /plan 时不会到达你原始输入的目标点。" ) if metrics["saw_align_goal_yaw"] and not metrics["saw_goal_reached"]: hints.append( "采集期间出现 aligning_goal_yaw 但没有 goal_reached；" "终点 yaw 对齐可能把车辆带离目标圆。" ) if ( not metrics["saw_goal_reached"] and metrics["max_abs_cmd_linear"] <= 1e-6 and metrics["max_abs_cmd_angular"] <= 1e-6 ): hints.append( "采集期间没有观察到速度命令；如果正在做实车到点测试，" "请确认 launch 使用 enable_motion:=true，且采集的是 tracker 实际输出话题。" ) ack_info = self._topic_endpoints_report().get(self.ackermann_cmd_topic, {}) if ack_info.get("publisher_count", 0) == 0: hints.append( f"{self.ackermann_cmd_topic} 没有发布者；阿克曼桥可能未启动，" "通常是 enable_motion:=false。" ) if not hints: hints.append("未发现明显配置矛盾；请把 JSON 报告发给我继续分析。") return hints def main(args=None): rclpy.init(args=args) node = GoalReachDebugCollector() try: node.collect_remote_parameters() while rclpy.ok() and not node.finished: rclpy.spin_once(node, timeout_sec=0.10) node.write_report() except KeyboardInterrupt: node.get_logger().warn("interrupted; writing partial report") node.write_report() finally: node.destroy_node() if rclpy.ok(): rclpy.shutdown() return 0 if __name__ == "__main__": raise SystemExit(main())