Multi robot plan repair by JY-HIM4U · Pull Request #25 · MIT-SPARK/Omniplanner

JY-HIM4U · 2026-05-12T00:35:12Z

Summary

Adds opt-in plan repair, plan interruption, and runtime constraint injection on top of the existing multi-robot PDDL pipeline.

Existing omniplanner_node behavior is unchanged.

All new functionality is isolated behind separate executables:

omniplanner_repair_node
goal_manager_node

New Capabilities

1. Plan Repair

When a new PDDL goal arrives:

check whether the currently executing symbolic plan already satisfies it
if yes:
- skip replanning
- resume execution directly

This avoids unnecessary planner calls during incremental task updates.

2. Plan Interruption / Preemption

When replanning is required:

planner publishes a new plan with a new plan_id
executor preempts the old plan
execution switches to the updated plan

3. Runtime Constraints

Supports dynamic planning constraints at runtime:

(forbidden-poi ...)
(forbidden-edge ...)

Constraints can be supplied:

per-goal message
persistently via a ROS topic

Reviewer Note

This branch includes the previously discussed multi-robot PDDL infrastructure as its base.

This PR focuses only on:

plan repair
runtime constraints
executor interruption / preemption support

The underlying multi-robot planning infrastructure is not the primary change proposed here.

File Changes

`omniplanner_msgs`

`msg/PddlGoalMsg.msg`

Added optional constraints string field
Allows JSON-encoded runtime constraints in a goal message
Backward compatible when unset

Example:

[
  ["forbidden-poi", "p123"],
  ["forbidden-edge", "p1", "p2"]
]

`dsg_pddl`

`domains/RegionObjectRearrangementDomain_MultiRobot_FD_Explore_Constraints.pddl`

Added constraint-aware multi-robot domain variant
Introduces:
- (forbidden-poi ?p)
- (forbidden-edge ?a ?b)
Prevents goto-poi from using forbidden targets / edges
Original domain remains unchanged

`dsg_pddl_grounding_multirobot.py`

Injects runtime constraints into generated PDDL :init
Example:

(forbidden-poi p123)
(forbidden-edge p1 p2)

No effect when constraints are empty

`dsg_pddl_planning.py`

Added optional Fast Downward overrides:

ADT4_FD_ALIAS
ADT4_FD_SEARCH
ADT4_FD_OVERALL_TIME_LIMIT

Defaults remain unchanged:

lazy_greedy([hff, hcea])

`omniplanner_ros`

`src/omniplanner_ros/last_pddl_plans.py`

New lightweight cache:

{robot_name -> PddlPlan}

Used to determine whether the current plan already satisfies a new goal

`src/omniplanner_ros/plan_utils.py`

New helper utilities for:

parsing visited place/object/POI predicates from goals
extracting visited POIs from plans
parsing forbidden constraints from JSON

`src/omniplanner_ros/omniplanner_repair_node.py`

Adds OmniPlannerRepairRos:

merges runtime constraints from:
- per-message constraints
- persistent ~/constraints topic
augments planning requests
publishes visited POIs for generated plans:

/<robot>/omniplanner_node/plan_visited_pois

`src/omniplanner_ros/goal_manager_node.py`

New interception node for:

/<robot>/commanded_pddl_goal

Behavior:

pause executor
check cached symbolic plan
if goal already covered and no forbidden violation:
- resume execution
otherwise:
- forward for replanning

`src/omniplanner_ros/pddl_planner_ros.py`

Added set_last_plan(...) calls after plan compilation
Stores latest symbolic plans in cache
Side-effect-free:
- no plan mutation
- no behavior change unless repair node is running

`setup.py`

Added console scripts:

omniplanner_repair_node
goal_manager_node

Existing executables remain unchanged:

omniplanner_node
pddl_plan_rviz_viz

GoldenZephyr · 2026-05-12T15:26:57Z

@JY-HIM4U It looks like the branch in the PR has several commits that have been merged in previously -- this makes the diff too hard to review. Can you clean up the history so that this PR has only the net new content being merged? That will make my review process much easier.

- omniplanner_msgs: optional 'constraints' field on PddlGoalMsg - dsg_pddl: constraint-aware multi-robot domain, runtime constraint injection in grounding, ADT4_FD_* env-var overrides for FD search - omniplanner_ros: plan-repair cache hooks in pddl_planner_ros, new opt-in executables (omniplanner_repair_node, goal_manager_node)

JY-HIM4U · 2026-05-12T20:55:11Z

I did it. Force-pushed multi_robot_plan_repair to drop the previous noisy commits

GoldenZephyr

Looks like a good start. I have made several comments. A couple of high-level things:

I think there are 2 or 3 distinct pieces to functionality. There is the addition of constraints to the planning problem, there is the omniplanner_repair_node which provides an interface for merging constraints that are fed into the planning problem, and there is the goal manager node which handles replanning. We should break this PR up more so that we can merge in each piece when it's ready (I think the order probably needs to be the order I listed, based on how those pieces depend on each other). The way to do this is have several PRs open (like master <- Feature1; Feature1 <- Feature2; Feature 2 <- Feature3).
I'm not sure I fully understand the kind of problems we eventually have in mind for the constraints and replanning. At a high level the motivation makes sense, but I don't have a good understanding of exactly where we hope to end up. For example, the current constraints can mostly be described within the goal already as (not (visited X)) . Is there a writeup somewhere of the class of problems, or a specific problem formulation of planning-with-constraints or plan-repair-with constraints, that we are trying to support?

GoldenZephyr · 2026-05-15T13:03:30Z

+)
+
+
+def solve_pddl(problem: GroundedPddlProblem):


We really don't want a full duplicate implementation of the solve_pddl function that does almost the same thing as the existing one. Any functionality that was missing with the existing version of solve_pddl should be added there as a refactor if necessary.

GoldenZephyr · 2026-05-15T13:04:02Z

    plan = solve_pddl(grounded_problem)

-    logger.warning(f"Made plan {plan}")
+    # logger.warning(f"Made plan {plan}")


I think we should keep this, although we can change from warning to info or debug

GoldenZephyr · 2026-05-15T13:06:07Z

    robot_states: dict,
    goal: PddlGoal,
    feedback: Any = None,
+    constraints: list = [],


The interface to ground_problem should not be touched. These constraints need to be included inside one of the other arguments (probably the goal)

GoldenZephyr · 2026-05-15T13:08:52Z

    robot_states: dict,
    goal: GotoPointsGoal,
    feedback: Any = None,
+    constraints: list = [],


We don't want to change the interface to ground_problem. These constraints should be included in the goal class (for any kind of domain where we support constraints)

(applies to all instances of ground_problem and make_plan)

GoldenZephyr · 2026-05-15T13:17:14Z

@@ -1,2 +1,3 @@
 string robot_id
 string pddl_goal
+string constraints  # JSON list of constraint facts, e.g. '[["forbidden-poi","p123"]]'. Empty = no change.


Two comments here -- 1) I think we should still have a simple pddl goal message that doesn't have the constraints. The right way of building the constraints message you want is probably making a ConstrainedPddlGoalMsg (or something like that) with two fields, a PddlGoalMsg and your constraints. 2) It's probably worth having the constraints field be a list of constraint messages, where each constraint message has a predicate and a list of symbol strings. The PDDL goal is currently a string because representing it with ROS message types would be quite a pain, but the list of constraints has a much simpler structure.

GoldenZephyr · 2026-05-15T13:23:32Z

+        (safe ?o - dsg_object)
+        (explored-region ?reg - region)
+
+        (forbidden-poi ?p - point-of-interest)


Do we actually need this in the planning domain vs. just excluding the place/edge when we construct the pddl problem instance?

GoldenZephyr · 2026-05-15T13:28:59Z

+    def __init__(self):
+        super().__init__("goal_manager")
+
+        self.declare_parameter("commanded_goal_topic", "/hilbert/commanded_pddl_goal")


Our convention is that all publishers and subscribers get set to a private topic name (for example ~/commanded_pddl_goal, and then connection to the correct topic is handled via remapping in roslaunch. So we don't want these parameters, and instead we'll subscribe to a private topic.

GoldenZephyr · 2026-05-15T13:33:43Z

+            f"[{robot_id}] updated plan cache: {len(pois)} visited POIs"
+        )
+
+    def commanded_goal_cb(self, msg: PddlGoalMsg):


I think it would be beneficial to add a small amount of abstraction here -- currently the implementation is extremely specific to replanning/repairing based on one particular planning domain (i.e., visiting and not visiting points). It's hard to get a sense of how this would generalize. In this function, I think we probably want to represent a flow like: 1) pause the robot 2) decide if we want to replan 3) replan if necessary 4) whatever extra logging is necessary. It's fine if the resulting implementation still only handles this particular planning problem, but structuring it that way would make it a little clearer what parts would need to change in the future to support more domains.

GoldenZephyr · 2026-05-15T14:05:22Z

+        except Exception as e:
+            self.get_logger().error(f"Bad constraints message: {e}")
+
+    def register_plugin(self, name, plugin):


This is too much duplicated code. We should figure out how to extend the existing omniplanner node callback a little more cleanly. If I understand correctly, there are two pieces of functionality you had to add: 1) merging the persistent and goal-dependent constraints, 2) publishing some information about the resulting plan.

As mentioned in some of the other comments, the constraints should end up inside plan_request. So as long as that's handled by callback, there's no special handling required inside plan_handler. I think plugin stores a reference to the parent node, so it should probably be able to merge the persistent and goal-dependent constraints without too much trouble.

To handle the extra publishing, for now I think we can just attach this extra callback to plugin itself; I think that will let us get rid of most of the duplicated code.

JY-HIM4U · 2026-05-18T21:24:47Z

Thank you so much for the detailed feedback. I will open three separate PR reflecting your comments.

For 2nd high-level comments, you are right that (not (visited-place X)) handles POI-level avoidance. When I talked with Jake, he gave examples like “do not use the bridge,” which are really edge-level constraints. I do not think the current not visited-place predicates are a good fit for that kind of constraint, though I am still unsure whether edge-level constraints are needed for the DCIST project.

JY-HIM4U force-pushed the multi_robot_plan_repair branch from 9f82781 to 9d8af10 Compare May 12, 2026 20:51

GoldenZephyr requested changes May 15, 2026

View reviewed changes

Conversation

JY-HIM4U commented May 12, 2026

Summary

New Capabilities

1. Plan Repair

2. Plan Interruption / Preemption

3. Runtime Constraints

Reviewer Note

File Changes

omniplanner_msgs

msg/PddlGoalMsg.msg

dsg_pddl

domains/RegionObjectRearrangementDomain_MultiRobot_FD_Explore_Constraints.pddl

dsg_pddl_grounding_multirobot.py

dsg_pddl_planning.py

omniplanner_ros

src/omniplanner_ros/last_pddl_plans.py

src/omniplanner_ros/plan_utils.py

src/omniplanner_ros/omniplanner_repair_node.py

src/omniplanner_ros/goal_manager_node.py

src/omniplanner_ros/pddl_planner_ros.py

setup.py

Uh oh!

GoldenZephyr commented May 12, 2026

Uh oh!

JY-HIM4U commented May 12, 2026

Uh oh!

GoldenZephyr left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

JY-HIM4U commented May 18, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

`omniplanner_msgs`

`msg/PddlGoalMsg.msg`

`dsg_pddl`

`domains/RegionObjectRearrangementDomain_MultiRobot_FD_Explore_Constraints.pddl`

`dsg_pddl_grounding_multirobot.py`

`dsg_pddl_planning.py`

`omniplanner_ros`

`src/omniplanner_ros/last_pddl_plans.py`

`src/omniplanner_ros/plan_utils.py`

`src/omniplanner_ros/omniplanner_repair_node.py`

`src/omniplanner_ros/goal_manager_node.py`

`src/omniplanner_ros/pddl_planner_ros.py`

`setup.py`