mrpt_tps_astar_planner

Overview

This package provides a ROS 2 node that uses the PTG-based A* planner from mrpt_path_planning to compute collision-free waypoint sequences for a non-holonomic robot, respecting its real shape, orientation, and kinematic constraints.

Planning is performed on a SE(2) lattice using Parameterized Trajectory Generator (PTG) families that encode the robot’s motion primitives. The result is published as a mrpt_msgs/WaypointSequence (and a nav_msgs/Path for visualization) and/or returned as the response of a ROS 2 service call.

The node supports concurrent service requests: each executor thread owns a lazily-initialized planner instance, so multiple clients can request plans simultaneously without blocking each other.

How to cite

Configuration

Key configuration files passed as ROS 2 parameters:

Demos

See the path-planner-sandbox/ subdirectory for standalone test scripts and sample maps.

Node: mrpt_tps_astar_planner_node

Working rationale

1. Obstacle data are maintained from subscribed gridmaps and/or point-cloud topics (updated asynchronously, protected by a mutex).
2. On each planning request (topic goal or service call) the node snapshots the current obstacle data, builds cost evaluators, and runs the A* planner.
3. The planned path is interpolated at a fixed time step and converted to a WaypointSequence.

At startup the PTG lookup tables are built in the background, which can take tens of seconds. The planning services and the navigate_goal action are only offered once they are ready, so clients see them as unavailable (and can wait for them) instead of getting a failed plan. Topic goals received meanwhile are rejected with a warning in the log (PTGs initialized marks when the node is ready).

The A* implementation is an anytime algorithm: it improves the solution while time allows, then returns the best found. An optional refinement pass (astar_skip_refine: false) further smooths the result.

Navigation goals: anytime and continuous planning

Goals from <topic_goal_sub> (e.g. clicked in RViz/Foxglove) or the ~/navigate_goal action are handled by a goal manager running in a background thread, so the robot starts moving soon and reacts to obstacles in advance:

1. Anytime: if the search has not reached the goal after anytime_first_path_time, the best partial path so far (the tree node closest to the goal, if it gets at least anytime_min_progress closer) is published, and the robot starts following it while the search goes on.
2. Continuous: every replan_period, the search root moves to a node stitch_lookahead_time ahead of the robot along its path. The path up to it is committed (it never changes), and the search grows the same tree from there with the latest obstacles until it reaches the goal. Path updates keep the committed part, so the follower switches to them without slowing down.
3. Each cycle, the rest of the path is checked against the current obstacles (robot footprint). If it is blocked, the path is cut short before the obstacle (the follower stops there if needed) and a new search starts from scratch: from the end of the committed part, or from the robot if the obstacle is before it (cutting the path there, so the robot does not drive past it meanwhile). Follower failures (Blocked, OffPathExceeded, MissedGoal, or staying Blocked for 5 s) restart it from the robot pose. A search that ends with no path to follow (no motion possible from the robot pose, or no way with the current obstacles, maybe transient) is retried every 2 s. Obstacle points that blocked a path of the goal are kept in its searches for blocked_obstacles_memory, so it does not plan through them again as soon as they are out of sight.
4. New searches from the robot (a new goal while driving, or a restart) start from where it will be after start_lookahead_time on the path it is following (at its current speed, from TF), and keep that path up to there, so a fast robot is not already past the start of the new path when it is published, and the follower switches to it without stopping.
5. The goal ends when the follower (topic_follower_status_sub) reaches it after following a full path. A new goal preempts the current one. Goals whose footprint hits the current obstacles are rejected at once. A goal

File truncated at 100 lines see the full file

Changelog for package mrpt_tps_astar_planner_node

2.6.1 (2026-10-02)

2.6.0 (2026-10-01)

• Merge pull request #176 from mrpt-ros-pkg/docs/tps-astar-planner-params
• Enable backward motion in the default PTG set
• mrpt_tps_astar_planner: document the weighted-A* and Reeds-Shepp planner options
• Merge pull request #175 from mrpt-ros-pkg/mrpt3
• Merge ros2 into mrpt3
• Declare missing MRPT 3.x package.xml dependencies
• mrpt_tps_astar_planner: link the split-off mrpt_path_planning_viz lib
• mrpt_tps_astar_planner: link against the actual mpp::mrpt_path_planning target
• Merge pull request #174 from mrpt-ros-pkg/fix/astar-planner-mpp-compat
• fix(tps_astar_planner): properly SFINAE-guard windowTitle and ptgStepIndex
• fix: sfinae to set window title on mrpt_path_planning newer versions
• tps_astar_planner: fix clang-format violation blocking CI
• tps_astar_planner_node: densify solution path before GUI render, titled window
• update to use new mpp lib structure
• Merge pull request #169 from mrpt-ros-pkg/feat/astar-footprint-aware-costmap
• feat(tps_astar_planner): footprint-aware clearance costmap (back-compatible)
• Merge pull request #168 from mrpt-ros-pkg/fix/tps-astar-concurrent-planning-race
• fix(tps_astar_planner): serialize planning to avoid concurrent PTG corruption
• Merge pull request #163 from mrpt-ros-pkg/feature/ccache-support
• build: enable ccache in all packages when available
• Port to mrpt3: fix yaml API, mrpt::opengl→viz, CImage, asCvMatRef
• Port to MRPT 3.x: rename packages and targets
• Contributors: Jose Luis Blanco-Claraco

2.5.0 (2026-05-11)

• Merge pull request #161 from mrpt-ros-pkg/fix-threads Fix threads
• fix: safer multithreading access
• Merge pull request #159 from mrpt-ros-pkg/bump-cmake bump min req cmake version to 3.22
• bump min req cmake version to 3.22
• fix: remove deadcode on debug 3D window
• Contributors: Jose Luis Blanco-Claraco

2.4.0 (2026-04-19)

• Merge pull request #158 from mrpt-ros-pkg/feat/parallel-planner Implement parallel A* planner; misc bug and style fixes

• Fix QoS type usage

• astar planner: made reentrant for serving multiple service calls in multithread

• Improve readme

• fix(tps_astar_planner): fix crashes, null deref, and minor log bugs

  - Replace ASSERT_(robot_pose_ok) with graceful error+return in both callback_goal and srv_make_plan_to, so a temporary TF outage does not abort the node or throw inside a service callback.

  - Add null guards before using e.grid_obstacles / e.obstacle_points in do_path_plan() and init_3d_debug() — both can be nullptr if the respective topic callback has not fired yet.

  - Call init_3d_debug() from do_path_plan() when gui_mrpt_ is true, so the 3D window is actually opened (was dead code).

  - Protect pub_costmaps_ resize and lazy publisher creation with pub_costmaps_cs_ mutex.

  - Fix copy-paste error: srv_make_plan_from_to catch block logged wrong function name.

  • Fix log format string: "topic_wp_seq_pub%s" -> "topic_wp_seq_pub: %s".

  - Launch file: lowercase 'false' for problem_world_bbox_ignore_obstacles and astar_skip_refine boolean

File truncated at 100 lines see the full file