forked from zbw/yiliao2026
805 lines
31 KiB
Python
Executable File
805 lines
31 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
"""Lightweight topology graph planner with Pure Pursuit tracking."""
|
|
|
|
import heapq
|
|
import math
|
|
from pathlib import Path
|
|
|
|
import rclpy
|
|
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 Bool, String
|
|
import yaml
|
|
|
|
from rclpy.duration import Duration
|
|
from tf2_ros import Buffer, TransformException, TransformListener
|
|
|
|
|
|
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 make_pose(frame_id, stamp, x, y, yaw):
|
|
pose = PoseStamped()
|
|
pose.header.frame_id = frame_id
|
|
pose.header.stamp = stamp
|
|
pose.pose.position.x = float(x)
|
|
pose.pose.position.y = float(y)
|
|
pose.pose.position.z = 0.0
|
|
pose.pose.orientation.z = math.sin(yaw * 0.5)
|
|
pose.pose.orientation.w = math.cos(yaw * 0.5)
|
|
return pose
|
|
|
|
|
|
class TopologyPurePursuit(Node):
|
|
def __init__(self):
|
|
super().__init__("topology_pure_pursuit")
|
|
|
|
self.declare_parameter("graph_file", "")
|
|
self.declare_parameter("map_frame", "map")
|
|
self.declare_parameter("odom_topic", "/odom")
|
|
self.declare_parameter("goal_pose_topic", "/goal_pose")
|
|
self.declare_parameter("goal_node_topic", "/topology_goal")
|
|
self.declare_parameter("external_plan_topic", "/plan")
|
|
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)
|
|
self.declare_parameter("autoplan_default_goal", False)
|
|
self.declare_parameter("control_rate", 20.0)
|
|
self.declare_parameter("path_resolution", 0.10)
|
|
self.declare_parameter("nearest_node_max_distance", 2.0)
|
|
self.declare_parameter("lookahead_distance", 0.55)
|
|
self.declare_parameter("goal_tolerance", 0.25)
|
|
self.declare_parameter("linear_speed", 0.30)
|
|
self.declare_parameter("min_linear_speed", 0.08)
|
|
self.declare_parameter("reverse_speed", 0.12)
|
|
self.declare_parameter("slowdown_distance", 0.80)
|
|
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(
|
|
self.get_parameter("enable_topology_planning").value
|
|
)
|
|
self.accept_external_plan = bool(
|
|
self.get_parameter("accept_external_plan").value
|
|
)
|
|
self.publish_cmd_vel = bool(self.get_parameter("publish_cmd_vel").value)
|
|
self.autoplan_default_goal = bool(
|
|
self.get_parameter("autoplan_default_goal").value
|
|
)
|
|
self.path_resolution = float(self.get_parameter("path_resolution").value)
|
|
self.nearest_node_max_distance = float(
|
|
self.get_parameter("nearest_node_max_distance").value
|
|
)
|
|
self.lookahead_distance = float(
|
|
self.get_parameter("lookahead_distance").value
|
|
)
|
|
self.goal_tolerance = float(self.get_parameter("goal_tolerance").value)
|
|
self.linear_speed = float(self.get_parameter("linear_speed").value)
|
|
self.min_linear_speed = float(self.get_parameter("min_linear_speed").value)
|
|
self.reverse_speed = float(self.get_parameter("reverse_speed").value)
|
|
self.slowdown_distance = float(self.get_parameter("slowdown_distance").value)
|
|
self.curvature_slowdown_gain = float(
|
|
self.get_parameter("curvature_slowdown_gain").value
|
|
)
|
|
self.max_angular_speed = float(
|
|
self.get_parameter("max_angular_speed").value
|
|
)
|
|
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
|
|
)
|
|
self.max_latency_compensation = float(
|
|
self.get_parameter("max_latency_compensation").value
|
|
)
|
|
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 = {}
|
|
self.default_start_node = ""
|
|
self.default_goal_node = ""
|
|
if self.enable_topology_planning:
|
|
self._load_graph(str(self.get_parameter("graph_file").value).strip())
|
|
elif not self.accept_external_plan:
|
|
raise RuntimeError(
|
|
"enable_topology_planning=false requires accept_external_plan=true"
|
|
)
|
|
|
|
self.tf_buffer = Buffer()
|
|
self.tf_listener = TransformListener(self.tf_buffer, self)
|
|
|
|
self.latest_odom = None
|
|
self.active_path = []
|
|
self.active_node_path = []
|
|
self.active_goal_node = ""
|
|
self.current_path_index = 0
|
|
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
|
|
)
|
|
self.cmd_pub = self.create_publisher(
|
|
Twist, self.get_parameter("cmd_vel_topic").value, 10
|
|
)
|
|
self.status_pub = self.create_publisher(
|
|
String, self.get_parameter("status_topic").value, 10
|
|
)
|
|
|
|
self.odom_sub = self.create_subscription(
|
|
Odometry,
|
|
self.get_parameter("odom_topic").value,
|
|
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
|
|
if self.enable_topology_planning:
|
|
self.goal_pose_sub = self.create_subscription(
|
|
PoseStamped,
|
|
self.get_parameter("goal_pose_topic").value,
|
|
self._goal_pose_callback,
|
|
10,
|
|
)
|
|
self.goal_node_sub = self.create_subscription(
|
|
String,
|
|
self.get_parameter("goal_node_topic").value,
|
|
self._goal_node_callback,
|
|
10,
|
|
)
|
|
if self.accept_external_plan:
|
|
self.external_plan_sub = self.create_subscription(
|
|
NavPath,
|
|
self.get_parameter("external_plan_topic").value,
|
|
self._external_plan_callback,
|
|
10,
|
|
)
|
|
|
|
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)} "
|
|
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):
|
|
if not graph_file_value:
|
|
raise RuntimeError("graph_file parameter is empty")
|
|
graph_file = Path(graph_file_value)
|
|
if not graph_file.exists():
|
|
raise RuntimeError(f"graph file does not exist: {graph_file}")
|
|
|
|
with graph_file.open("r", encoding="utf-8") as stream:
|
|
data = yaml.safe_load(stream) or {}
|
|
|
|
self.default_start_node = str(data.get("default_start", ""))
|
|
self.default_goal_node = str(data.get("default_goal", ""))
|
|
|
|
raw_nodes = data.get("nodes", {})
|
|
for name, value in raw_nodes.items():
|
|
if isinstance(value, dict):
|
|
x = value["x"]
|
|
y = value["y"]
|
|
else:
|
|
x = value[0]
|
|
y = value[1]
|
|
self.nodes[str(name)] = (float(x), float(y))
|
|
|
|
self.edges = {name: [] for name in self.nodes}
|
|
for edge in data.get("edges", []):
|
|
if isinstance(edge, dict):
|
|
start = str(edge["from"])
|
|
goal = str(edge["to"])
|
|
cost = float(edge.get("cost", self._distance_nodes(start, goal)))
|
|
bidirectional = bool(edge.get("bidirectional", True))
|
|
else:
|
|
start = str(edge[0])
|
|
goal = str(edge[1])
|
|
cost = self._distance_nodes(start, goal)
|
|
bidirectional = True
|
|
self._add_edge(start, goal, cost)
|
|
if bidirectional:
|
|
self._add_edge(goal, start, cost)
|
|
|
|
if not self.nodes:
|
|
raise RuntimeError(f"graph contains no nodes: {graph_file}")
|
|
|
|
def _distance_nodes(self, start, goal):
|
|
if start not in self.nodes or goal not in self.nodes:
|
|
raise RuntimeError(f"edge references unknown node: {start}->{goal}")
|
|
ax, ay = self.nodes[start]
|
|
bx, by = self.nodes[goal]
|
|
return math.hypot(bx - ax, by - ay)
|
|
|
|
def _add_edge(self, start, goal, cost):
|
|
if start not in self.nodes or goal not in self.nodes:
|
|
raise RuntimeError(f"edge references unknown node: {start}->{goal}")
|
|
self.edges.setdefault(start, []).append((goal, float(cost)))
|
|
|
|
def _odom_callback(self, msg):
|
|
self.latest_odom = msg
|
|
if (
|
|
self.enable_topology_planning
|
|
and self.autoplan_default_goal
|
|
and not self.tried_default_goal
|
|
and self.default_goal_node
|
|
):
|
|
self.tried_default_goal = True
|
|
self.plan_to_node(self.default_goal_node)
|
|
|
|
def _goal_pose_callback(self, msg):
|
|
if not self.enable_topology_planning:
|
|
return
|
|
goal_node, distance = self._nearest_node(
|
|
msg.pose.position.x, msg.pose.position.y
|
|
)
|
|
if not goal_node or distance > self.nearest_node_max_distance:
|
|
self._publish_status(
|
|
f"goal_pose_rejected nearest={goal_node} distance={distance:.2f}"
|
|
)
|
|
return
|
|
self.plan_to_node(goal_node)
|
|
|
|
def _goal_node_callback(self, msg):
|
|
if not self.enable_topology_planning:
|
|
return
|
|
self.plan_to_node(msg.data.strip())
|
|
|
|
def _external_plan_callback(self, msg):
|
|
if not self.accept_external_plan:
|
|
return
|
|
frame_id = msg.header.frame_id or self.map_frame
|
|
if frame_id != self.map_frame:
|
|
self._publish_status(f"external_plan_rejected frame={frame_id}")
|
|
return
|
|
if len(msg.poses) < 2:
|
|
self._publish_status("external_plan_rejected too_short")
|
|
if self.stop_without_plan:
|
|
self.active_path = []
|
|
self.completed = True
|
|
return
|
|
|
|
self.active_path = [
|
|
(
|
|
pose.pose.position.x,
|
|
pose.pose.position.y,
|
|
yaw_from_quaternion(pose.pose.orientation),
|
|
)
|
|
for pose in msg.poses
|
|
]
|
|
self.active_node_path = []
|
|
self.active_goal_node = "external_plan"
|
|
self.current_path_index = 0
|
|
self.completed = False
|
|
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:
|
|
self._publish_status(f"goal_node_rejected unknown={goal_node}")
|
|
return False
|
|
|
|
pose = self._current_pose(compensate=False)
|
|
if pose is None:
|
|
self._publish_status("plan_rejected no_odom")
|
|
return False
|
|
|
|
if self.default_start_node and self.default_start_node in self.nodes:
|
|
start_hint = self.default_start_node
|
|
else:
|
|
start_hint = ""
|
|
start_node, start_distance = self._nearest_node(pose[0], pose[1])
|
|
if start_hint and start_distance > self.nearest_node_max_distance:
|
|
start_node = start_hint
|
|
elif not start_node or start_distance > self.nearest_node_max_distance:
|
|
self._publish_status(
|
|
f"plan_rejected no_near_start nearest={start_node} distance={start_distance:.2f}"
|
|
)
|
|
return False
|
|
|
|
node_path = self._dijkstra(start_node, goal_node)
|
|
if not node_path:
|
|
self._publish_status(f"plan_failed {start_node}->{goal_node}")
|
|
return False
|
|
|
|
dense_path = self._densify_path(node_path, pose)
|
|
if len(dense_path) < 2:
|
|
self._publish_status(f"plan_failed short_path {start_node}->{goal_node}")
|
|
return False
|
|
|
|
self.active_path = dense_path
|
|
self.active_node_path = node_path
|
|
self.active_goal_node = goal_node
|
|
self.current_path_index = 0
|
|
self.completed = False
|
|
self._publish_path()
|
|
self._publish_status(
|
|
f"plan_ok {'->'.join(node_path)} points={len(dense_path)}"
|
|
)
|
|
return True
|
|
|
|
def _nearest_node(self, x, y):
|
|
best_name = ""
|
|
best_dist = float("inf")
|
|
for name, (nx, ny) in self.nodes.items():
|
|
distance = math.hypot(nx - x, ny - y)
|
|
if distance < best_dist:
|
|
best_name = name
|
|
best_dist = distance
|
|
return best_name, best_dist
|
|
|
|
def _dijkstra(self, start, goal):
|
|
queue = [(0.0, start)]
|
|
previous = {}
|
|
cost_so_far = {start: 0.0}
|
|
|
|
while queue:
|
|
cost, current = heapq.heappop(queue)
|
|
if current == goal:
|
|
break
|
|
if cost > cost_so_far[current]:
|
|
continue
|
|
for neighbor, edge_cost in self.edges.get(current, []):
|
|
new_cost = cost + edge_cost
|
|
if new_cost < cost_so_far.get(neighbor, float("inf")):
|
|
cost_so_far[neighbor] = new_cost
|
|
previous[neighbor] = current
|
|
heapq.heappush(queue, (new_cost, neighbor))
|
|
|
|
if goal not in cost_so_far:
|
|
return []
|
|
|
|
path = [goal]
|
|
while path[-1] != start:
|
|
path.append(previous[path[-1]])
|
|
path.reverse()
|
|
return path
|
|
|
|
def _densify_path(self, node_path, pose):
|
|
points = [(pose[0], pose[1], pose[2])]
|
|
for index, node_name in enumerate(node_path):
|
|
target = self.nodes[node_name]
|
|
if index == 0 and math.hypot(target[0] - pose[0], target[1] - pose[1]) < 0.05:
|
|
points[-1] = (target[0], target[1], points[-1][2])
|
|
continue
|
|
self._append_segment(points, target)
|
|
return self._with_segment_yaws(points)
|
|
|
|
def _append_segment(self, points, target):
|
|
sx, sy, _ = points[-1]
|
|
tx, ty = target
|
|
length = math.hypot(tx - sx, ty - sy)
|
|
if length < 1e-6:
|
|
return
|
|
steps = max(1, int(math.ceil(length / max(0.02, self.path_resolution))))
|
|
for step in range(1, steps + 1):
|
|
ratio = step / steps
|
|
yaw = math.atan2(ty - sy, tx - sx)
|
|
points.append((sx + (tx - sx) * ratio, sy + (ty - sy) * ratio, yaw))
|
|
|
|
def _with_segment_yaws(self, points):
|
|
if len(points) <= 1:
|
|
return points
|
|
|
|
updated = []
|
|
for index, (x, y, yaw) in enumerate(points):
|
|
if index + 1 < len(points):
|
|
nx, ny, _ = points[index + 1]
|
|
if math.hypot(nx - x, ny - y) > 1e-6:
|
|
yaw = math.atan2(ny - y, nx - x)
|
|
elif updated:
|
|
yaw = updated[-1][2]
|
|
updated.append((x, y, yaw))
|
|
return updated
|
|
|
|
def _publish_path(self):
|
|
path_msg = NavPath()
|
|
path_msg.header.frame_id = self.map_frame
|
|
path_msg.header.stamp = self.get_clock().now().to_msg()
|
|
|
|
for x, y, yaw in self.active_path:
|
|
path_msg.poses.append(make_pose(self.map_frame, path_msg.header.stamp, x, y, yaw))
|
|
|
|
self.plan_pub.publish(path_msg)
|
|
|
|
def _current_pose(self, compensate=True):
|
|
if self.latest_odom is None:
|
|
return None
|
|
|
|
msg = self.latest_odom
|
|
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)
|
|
vx = msg.twist.twist.linear.x
|
|
wz = msg.twist.twist.angular.z
|
|
|
|
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 None
|
|
|
|
if compensate and self.latency_compensation:
|
|
horizon = min(age, self.max_latency_compensation)
|
|
if abs(wz) < 1e-4:
|
|
x += vx * math.cos(yaw) * horizon
|
|
y += vx * math.sin(yaw) * horizon
|
|
else:
|
|
radius = vx / wz
|
|
new_yaw = yaw + wz * horizon
|
|
x += radius * (math.sin(new_yaw) - math.sin(yaw))
|
|
y -= radius * (math.cos(new_yaw) - math.cos(yaw))
|
|
yaw = normalize_angle(new_yaw)
|
|
|
|
if source_frame != self.map_frame:
|
|
transformed = self._transform_pose_2d(x, y, yaw, source_frame)
|
|
if transformed is None:
|
|
return None
|
|
x, y, yaw = transformed
|
|
|
|
return x, y, yaw, vx, wz
|
|
|
|
def _transform_pose_2d(self, x, y, yaw, source_frame):
|
|
try:
|
|
transform = self.tf_buffer.lookup_transform(
|
|
self.map_frame,
|
|
source_frame,
|
|
Time(),
|
|
timeout=Duration(seconds=self.tf_timeout),
|
|
)
|
|
except TransformException as exc:
|
|
now_ns = self.get_clock().now().nanoseconds
|
|
if now_ns - self.last_tf_warn_ns > 2_000_000_000:
|
|
self.last_tf_warn_ns = now_ns
|
|
self.get_logger().warn(
|
|
f"TF unavailable {source_frame}->{self.map_frame}: {exc}"
|
|
)
|
|
return None
|
|
|
|
t = transform.transform.translation
|
|
q = transform.transform.rotation
|
|
transform_yaw = yaw_from_quaternion(q)
|
|
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 _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} "
|
|
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
|
|
path_heading = self._segment_heading(target_index, x, y)
|
|
reverse = math.cos(normalize_angle(target_path_yaw - path_heading)) < 0.0
|
|
control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw
|
|
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._target_speed(abs(curvature), goal_distance, reverse)
|
|
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)
|
|
best_index = start
|
|
best_dist = float("inf")
|
|
for index in range(start, len(self.active_path)):
|
|
px, py, _ = self.active_path[index]
|
|
dist = (px - x) * (px - x) + (py - y) * (py - y)
|
|
if dist < best_dist:
|
|
best_index = index
|
|
best_dist = dist
|
|
return best_index
|
|
|
|
def _lookahead_index(self, x, y, start_index):
|
|
target_index = len(self.active_path) - 1
|
|
for index in range(start_index, len(self.active_path)):
|
|
px, py, _ = self.active_path[index]
|
|
if math.hypot(px - x, py - y) >= self.lookahead_distance:
|
|
target_index = index
|
|
break
|
|
return target_index
|
|
|
|
def _segment_heading(self, index, current_x=None, current_y=None):
|
|
if len(self.active_path) < 2:
|
|
return 0.0
|
|
if current_x is not None and current_y is not None:
|
|
tx, ty, target_yaw = self.active_path[index]
|
|
if math.hypot(tx - current_x, ty - current_y) > 1e-6:
|
|
return math.atan2(ty - current_y, tx - current_x)
|
|
return target_yaw
|
|
|
|
if index <= 0:
|
|
start_index = 0
|
|
end_index = 1
|
|
else:
|
|
start_index = index - 1
|
|
end_index = index
|
|
sx, sy, fallback_yaw = self.active_path[start_index]
|
|
tx, ty, _ = self.active_path[end_index]
|
|
if math.hypot(tx - sx, ty - sy) <= 1e-6:
|
|
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
|
|
speed *= max(0.35, 1.0 - self.curvature_slowdown_gain * abs_curvature)
|
|
if goal_distance < self.slowdown_distance:
|
|
ratio = max(0.25, goal_distance / max(0.05, self.slowdown_distance))
|
|
speed *= ratio
|
|
speed = max(self.min_linear_speed, min(base_speed, speed))
|
|
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 = self.last_cmd_linear
|
|
cmd.angular.z = self.last_cmd_angular
|
|
self.cmd_pub.publish(cmd)
|
|
|
|
def _stop_if_needed(self, reason):
|
|
self._publish_cmd(0.0, 0.0)
|
|
self._publish_status(reason)
|
|
|
|
def _publish_status(self, text):
|
|
msg = String()
|
|
msg.data = text
|
|
self.status_pub.publish(msg)
|
|
self.get_logger().info(text)
|
|
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
node = None
|
|
try:
|
|
node = TopologyPurePursuit()
|
|
rclpy.spin(node)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
if node is not None:
|
|
node._publish_cmd(0.0, 0.0)
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|