Recursive Tree Planner (RTP) Node

Maintained by Nova

Overview

This node (rtp_node.py) implements a Recursive Tree Planner (RTP) to generate paths based on cost values. It uses recursion to generate branches (or path segments), assessing the cost for each segment, and eventually selects and publishes the path with the least cost. This node also holds temporary controller logic.

---

In:

• /grid/steering_cost OccupancyGrid
• /grid/speed_cost OccupancyGrid
• /gnss/odometry Odometry
• /guardian/mode Mode
• /clock Clock

Out:

• /planning/path Path
• /planning/barrier_marker Marker
• /planning/target_speed VehicleSpeed
• /vehicle/control VehicleControl
• /node_statuses DiagnosticStatus

---

Constants and Parameters:

• N_BRANCHES: Number of branches generated in each iteration.
• STEP_LEN: Length of each path segment.
• DEPTH: Depth of recursion.
• WHEEL_BASE: Vehicle’s wheelbase.
• MAX_TURN_ANGLE: Maximum steering angle of the vehicle.
• COST_CUTOFF: Cost value above which a segment is considered nonviable.

---

getBarrierIndex(self, path: CostedPath, map: np.ndarray, width_meters=1.8)

• Checks a path for collision points.
• Returns the index of the first pose that would collide with an obstacle.

getSegment(self, inital_pose: np.ndarray, steering_angle, segment_length: float, res: float, costmap)

• Determines the cost of a path segment based on the provided steering angle.

generatePaths(self, depth: int, path: CostedPath, steering_angle, segment_length, res, num_branches, results: list, result_costs, costmap)

• Recursively generates path branches based on steering angles.
• Computes segments based on cost.

startGeneration(self, costmap: np.ndarray, depth=7, segment_length=9.0, branches=7)

• Commences the path generation process using a recursive approach.

costMapCb(self, msg: OccupancyGrid)

• Generates paths based on the incoming cost map data.
• Determines the best path based on cost.
• Publishes the least-cost path.