718 lines
28 KiB
Python
Executable File
718 lines
28 KiB
Python
Executable File
#!/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<points>\d+).*iterations=(?P<iterations>\d+)"
|
||
r".*planning_ms=(?P<planning_ms>[-+]?\d+(?:\.\d+)?)"
|
||
r".*goal_error=(?P<goal_error>[-+]?\d+(?:\.\d+)?)"
|
||
r".*yaw_error=(?P<yaw_error>[-+]?\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())
|