#!/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()