forked from zbw/yiliao2026
添加了新的planner，未实际实现过，需进行测试
This commit is contained in:
629
src/planner/scripts/topology_pure_pursuit_node.py
Normal file
629
src/planner/scripts/topology_pure_pursuit_node.py
Normal file
@@ -0,0 +1,629 @@
|
||||
#!/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 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("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("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.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.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.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.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.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)} graph_edges={sum(len(v) for v in self.edges.values())}"
|
||||
)
|
||||
|
||||
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
|
||||
self._publish_status(f"external_plan_ok points={len(self.active_path)}")
|
||||
|
||||
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:
|
||||
return
|
||||
|
||||
pose = self._current_pose(compensate=True)
|
||||
if pose is None:
|
||||
self._stop_if_needed("stop 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)
|
||||
return
|
||||
|
||||
x, y, yaw, _, _ = pose
|
||||
goal_x, goal_y, _ = self.active_path[-1]
|
||||
goal_distance = math.hypot(goal_x - x, goal_y - y)
|
||||
if goal_distance <= self.goal_tolerance:
|
||||
self.completed = True
|
||||
self._publish_cmd(0.0, 0.0)
|
||||
self._publish_status(f"goal_reached {self.active_goal_node}")
|
||||
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]
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
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):
|
||||
cmd = Twist()
|
||||
cmd.linear.x = float(linear)
|
||||
cmd.angular.z = float(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()
|
||||
Reference in New Issue
Block a user