Multi robot plan repair

[JY-HIM4U]

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:

1. pause executor
2. check cached symbolic plan
3. if goal already covered and no forbidden violation:
   • resume execution
4. 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]

@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.

[JY-HIM4U]

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