From 9d8af109e6ce8e53c4c1f0660d8f6d4e66213472 Mon Sep 17 00:00:00 2001 From: Jaeyoun Date: Tue, 12 May 2026 14:19:15 -0400 Subject: [PATCH] Plan repair, runtime constraints, and FD env-var overrides - 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) --- ...ain_MultiRobot_FD_Explore_Constraints.pddl | 104 +++++++++ .../src/dsg_pddl/dsg_pddl_grounding.py | 1 + .../dsg_pddl/dsg_pddl_grounding_multirobot.py | 30 ++- omniplanner/src/dsg_pddl/dsg_pddl_planning.py | 91 +++++++- omniplanner/src/omniplanner/goto_points.py | 2 + .../src/omniplanner/language_planner.py | 1 + omniplanner/src/omniplanner/omniplanner.py | 3 + omniplanner/src/omniplanner/tsp.py | 7 +- omniplanner_msgs/msg/PddlGoalMsg.msg | 1 + omniplanner_ros/setup.py | 6 +- .../src/omniplanner_ros/goal_manager_node.py | 125 +++++++++++ .../src/omniplanner_ros/last_pddl_plans.py | 15 ++ .../omniplanner_repair_node.py | 209 ++++++++++++++++++ .../src/omniplanner_ros/pddl_planner_ros.py | 9 + .../src/omniplanner_ros/plan_utils.py | 50 +++++ 15 files changed, 640 insertions(+), 14 deletions(-) create mode 100644 omniplanner/src/dsg_pddl/domains/RegionObjectRearrangementDomain_MultiRobot_FD_Explore_Constraints.pddl create mode 100644 omniplanner_ros/src/omniplanner_ros/goal_manager_node.py create mode 100644 omniplanner_ros/src/omniplanner_ros/last_pddl_plans.py create mode 100644 omniplanner_ros/src/omniplanner_ros/omniplanner_repair_node.py create mode 100644 omniplanner_ros/src/omniplanner_ros/plan_utils.py diff --git a/omniplanner/src/dsg_pddl/domains/RegionObjectRearrangementDomain_MultiRobot_FD_Explore_Constraints.pddl b/omniplanner/src/dsg_pddl/domains/RegionObjectRearrangementDomain_MultiRobot_FD_Explore_Constraints.pddl new file mode 100644 index 0000000..790d623 --- /dev/null +++ b/omniplanner/src/dsg_pddl/domains/RegionObjectRearrangementDomain_MultiRobot_FD_Explore_Constraints.pddl @@ -0,0 +1,104 @@ +(define (domain region-object-rearrangement-domain-multirobot-fd-constraints) + (:requirements :derived-predicates :typing :adl) + (:types + region point-of-interest - object + place dsg_object - point-of-interest + robot - object + ) + + (:predicates + (at-poi ?r - robot ?p - point-of-interest) + (connected ?s - point-of-interest ?t - point-of-interest) + (suspicious ?o - dsg_object) + (at-object ?r - robot ?o - dsg_object) + (at-place ?r - robot ?p - place) + (in-region ?r - robot ?reg - region) + + (holding ?r - robot ?o - dsg_object) + (hand-full ?r - robot) + (object-in-place ?o - dsg_object ?p - place) + (place-in-region ?p - place ?reg - region) + + (visited-poi ?p - point-of-interest) + (visited-place ?p - place) + (visited-object ?o - dsg_object) + (visited-region ?reg - region) + + (safe ?o - dsg_object) + (explored-region ?reg - region) + + (forbidden-poi ?p - point-of-interest) + (forbidden-edge ?s - point-of-interest ?t - point-of-interest) + ) + + (:functions + (distance ?s - point-of-interest ?t - point-of-interest) + (total-cost) + ) + + (:derived (at-object ?r - robot ?o - dsg_object) + (at-poi ?r ?o)) + + (:derived (at-place ?r - robot ?p - place) + (at-poi ?r ?p)) + + (:derived (visited-place ?p - place) + (visited-poi ?p)) + + (:derived (visited-object ?o - dsg_object) + (visited-poi ?o)) + + (:derived (safe ?o - dsg_object) + (not (suspicious ?o))) + + (:derived (in-region ?r - robot ?reg - region) + (exists (?p - place) (and (at-place ?r ?p) (place-in-region ?p ?reg)))) + + (:derived (visited-region ?reg - region) + (exists (?p - place) (and (visited-place ?p) (place-in-region ?p ?reg)))) + + (:derived (explored-region ?reg - region) + (forall (?p - place) + (or (not (place-in-region ?p ?reg)) + (visited-place ?p)))) + + (:action goto-poi + :parameters (?r - robot ?s - point-of-interest ?t - point-of-interest) + :precondition (and (at-poi ?r ?s) + (or (connected ?s ?t) (connected ?t ?s)) + (not (forbidden-poi ?t)) + (not (forbidden-edge ?s ?t)) + (not (forbidden-edge ?t ?s))) + :effect (and (not (at-poi ?r ?s)) + (at-poi ?r ?t) + (visited-poi ?t) + (increase (total-cost) (distance ?s ?t)) + ) + ) + + (:action inspect + :parameters (?r - robot ?o - dsg_object) + :precondition (at-object ?r ?o) + :effect (and (not (suspicious ?o)) + (increase (total-cost) 1) + ) + ) + + (:action pick-object + :parameters (?r - robot ?o - dsg_object ?p - place) + :precondition (and (not (hand-full ?r)) + (safe ?o) + (at-object ?r ?o) + (object-in-place ?o ?p)) + :effect (and (holding ?r ?o) + (hand-full ?r) + (not (object-in-place ?o ?p)))) + + (:action place-object + :parameters (?r - robot ?o - dsg_object ?p - place) + :precondition (and (holding ?r ?o) (at-place ?r ?p)) + :effect (and (not (holding ?r ?o)) + (not (hand-full ?r)) + (object-in-place ?o ?p))) + +) diff --git a/omniplanner/src/dsg_pddl/dsg_pddl_grounding.py b/omniplanner/src/dsg_pddl/dsg_pddl_grounding.py index 698793e..5a1a11f 100644 --- a/omniplanner/src/dsg_pddl/dsg_pddl_grounding.py +++ b/omniplanner/src/dsg_pddl/dsg_pddl_grounding.py @@ -480,6 +480,7 @@ def ground_problem( robot_states: dict, goal: PddlGoal, feedback: Any = None, + constraints: list = [], ) -> RobotWrapper[GroundedPddlProblem]: logger.info(f"Grounding PDDL Problem {domain.domain_name}") diff --git a/omniplanner/src/dsg_pddl/dsg_pddl_grounding_multirobot.py b/omniplanner/src/dsg_pddl/dsg_pddl_grounding_multirobot.py index e625012..c214187 100644 --- a/omniplanner/src/dsg_pddl/dsg_pddl_grounding_multirobot.py +++ b/omniplanner/src/dsg_pddl/dsg_pddl_grounding_multirobot.py @@ -64,7 +64,7 @@ def generate_dense_region_symbol_connectivity_multirobot(G, symbols, robot_state 10, layer_planner, ) - start_connection_threshold = 50 + start_connection_threshold = 20 for robot_id in robot_states.keys(): start_symbol_key = f"pstart{robot_id}" if start_symbol_key in symbol_lookup: @@ -146,8 +146,10 @@ def generate_multirobot_region_pddl( G: spark_dsg.DynamicSceneGraph, raw_pddl_goal_string: str, robot_states: np.ndarray, + constraints: List[tuple] = None, + domain_name: str = "region-object-rearrangement-domain-multirobot-fd", ) -> Tuple[str, List[PddlSymbol]]: - """Generate a multi-robot PDDL problem for domain region-object-rearrangement-domain-multirobot-fd.""" + """Generate a multi-robot PDDL problem for the given multi-robot domain.""" # Collect all places/objects/regions and positions symbols = extract_all_symbols(G) normalize_symbols(symbols) @@ -194,12 +196,17 @@ def generate_multirobot_region_pddl( object_symbols = [s for s in symbols_of_interest if s.layer == "object"] init_facts_tuples += [("suspicious", o.symbol) for o in object_symbols] + # Inject runtime constraints (forbidden-poi, forbidden-edge, etc.) + if constraints: + init_facts_tuples += list(constraints) + logger.info(f"Injected {len(constraints)} constraint facts into PDDL init") + # Build objects dict using shared generator and adding robots pddl_objects = generate_objects(symbols_of_interest) pddl_objects["robot"] = robot_ids problem = PddlProblem( name="multi-robot-problem", - domain="region-object-rearrangement-domain-multirobot-fd", + domain=domain_name, objects=pddl_objects, initial_facts=tuple(init_facts_tuples), goal=goal_pddl, @@ -233,19 +240,22 @@ def ground_problem( robot_states: dict, goal: PddlGoal, feedback: Any = None, + constraints: list = [], ) -> MultiRobotWrapper[GroundedPddlProblem]: logger.warning(f"Grounding PDDL Problem {domain.domain_name}") pddl_compliant_robot_states = {k.lower(): v for k, v in robot_states.items()} match domain.domain_name: - # case "goto-object-domain-multirobot-fd": - # pddl_problem, symbols = generate_multirobot_inspection_pddl( - # dsg, goal.pddl_goal, robot_states - # ) - - case "region-object-rearrangement-domain-multirobot-fd": + case ( + "region-object-rearrangement-domain-multirobot-fd" + | "region-object-rearrangement-domain-multirobot-fd-constraints" + ): pddl_problem, symbols = generate_multirobot_region_pddl( - dsg, goal.pddl_goal, pddl_compliant_robot_states + dsg, + goal.pddl_goal, + pddl_compliant_robot_states, + constraints, + domain_name=domain.domain_name, ) # logger.warning(f"!!!!!!!!!!!!!!pddl_problem: {pddl_problem}") case _: diff --git a/omniplanner/src/dsg_pddl/dsg_pddl_planning.py b/omniplanner/src/dsg_pddl/dsg_pddl_planning.py index e3907a8..dbd860c 100644 --- a/omniplanner/src/dsg_pddl/dsg_pddl_planning.py +++ b/omniplanner/src/dsg_pddl/dsg_pddl_planning.py @@ -1,4 +1,7 @@ import logging +import os +import subprocess +import tempfile from dataclasses import dataclass from typing import Any, Dict, List @@ -7,7 +10,7 @@ from plum import dispatch from dsg_pddl.dsg_pddl_grounding import GroundedPddlProblem, PddlDomain, PddlSymbol -from dsg_pddl.pddl_planning import solve_pddl +from dsg_pddl.pddl_utils import lisp_string_to_ast from omniplanner.tsp import LayerPlanner logger = logging.getLogger(__name__) @@ -21,6 +24,90 @@ class PddlPlan: symbols: Dict[str, PddlSymbol] +DEFAULT_FD_SEARCH = ( + "let(hff, ff(), let(hcea, cea(), lazy_greedy([hff, hcea], preferred=[hff, hcea])))" +) + + +def solve_pddl(problem: GroundedPddlProblem): + """Use fast-downward to solve the given pddl problem. + + The Fast Downward search can be overridden via environment variables: + ADT4_FD_ALIAS — use a Fast Downward alias (e.g. "seq-sat-lama-2011"). + Takes precedence over ADT4_FD_SEARCH when set. + ADT4_FD_SEARCH — raw value passed after `--search`. + If neither is set, the historical lazy-greedy(ff+cea) search is used. + """ + with tempfile.TemporaryDirectory() as tmpdirname: + problem_fn = os.path.join(tmpdirname, "problem.pddl") + domain_fn = os.path.join(tmpdirname, "domain.pddl") + plan_fn = os.path.join(tmpdirname, "plan.txt") + + with open(problem_fn, "w") as fo: + fo.write(problem.problem_str) + + with open(domain_fn, "w") as fo: + fo.write(problem.domain.to_string()) + + alias = os.getenv("ADT4_FD_ALIAS", "").strip() + search = os.getenv("ADT4_FD_SEARCH", "").strip() + time_limit = os.getenv("ADT4_FD_OVERALL_TIME_LIMIT", "").strip() + + command = ["fast-downward"] + command += ["--plan-file", plan_fn] + if time_limit: + command += ["--overall-time-limit", time_limit] + if alias: + command += ["--alias", alias] + command += [domain_fn, problem_fn] + else: + command += [domain_fn, problem_fn] + command += ["--search", search or DEFAULT_FD_SEARCH] + + logger.warning(f"Calling: {command}") + return_code = subprocess.run(command) + logger.warning(f"Return code: {return_code}") + + # Anytime aliases (e.g. seq-sat-lama-2011) write plan.txt.1, plan.txt.2, + # ... and do NOT write plan.txt. Pick the highest-numbered (best) one. + import glob + + numbered = sorted( + glob.glob(plan_fn + ".*"), + key=lambda p: int(p.rsplit(".", 1)[-1]) + if p.rsplit(".", 1)[-1].isdigit() + else -1, + ) + if numbered and numbered[-1].rsplit(".", 1)[-1].isdigit(): + best_plan = numbered[-1] + elif os.path.exists(plan_fn): + best_plan = plan_fn + else: + best_plan = None + + if best_plan is not None: + logger.warning(f"Reading plan from {best_plan}") + with open(best_plan, "r") as fo: + lines = fo.readlines() + else: + output_dir = os.getenv("ADT4_OUTPUT_DIR", "") + debug_fn = os.path.join(output_dir, "pddl_problem_debugging.pddl") + logger.warning( + f"Planning failed. Please see {debug_fn} for the failed problem file." + ) + with open(debug_fn, "w") as fo: + fo.write(problem.problem_str) + raise Exception( + f"Planning failed, please see {debug_fn} for failed problem file." + ) + + plan = [lisp_string_to_ast(line) for line in lines[:-1]] + plan = [ + ("goto-poi",) + p[1:] if p and p[0] == "goto-poi-revisit" else p for p in plan + ] + return plan + + def drop_index(t, k): return t[:k] + t[k + 1 :] @@ -76,7 +163,7 @@ def parameterize_place_object_multirobot(layer_planner, symbols, action, last_po def make_plan(grounded_problem: GroundedPddlProblem, map_context: Any) -> PddlPlan: plan = solve_pddl(grounded_problem) - logger.warning(f"Made plan {plan}") + # logger.warning(f"Made plan {plan}") parameterized_plan = [] diff --git a/omniplanner/src/omniplanner/goto_points.py b/omniplanner/src/omniplanner/goto_points.py index bb08ba0..e74eb02 100644 --- a/omniplanner/src/omniplanner/goto_points.py +++ b/omniplanner/src/omniplanner/goto_points.py @@ -49,6 +49,7 @@ def ground_problem( robot_states: dict, goal: GotoPointsGoal, feedback: Any = None, + constraints: list = [], ) -> RobotWrapper[GroundedGotoPointsProblem]: start = robot_states[goal.robot_id] @@ -78,6 +79,7 @@ def ground_problem( start: np.ndarray, goal: list, feedback: Any = None, + constraints: list = [], ) -> GroundedGotoPointsProblem: point_sequence = map_context[goal] return GroundedGotoPointsProblem(start, point_sequence) diff --git a/omniplanner/src/omniplanner/language_planner.py b/omniplanner/src/omniplanner/language_planner.py index 7197b7d..7040bf6 100644 --- a/omniplanner/src/omniplanner/language_planner.py +++ b/omniplanner/src/omniplanner/language_planner.py @@ -29,6 +29,7 @@ def ground_problem( robot_states: dict, goal: LanguageGoal, feedback: Any = None, + constraints: list = [], ): if domain.domain_type == "goto_points": language_grounded_goal = GotoPointsGoal( diff --git a/omniplanner/src/omniplanner/omniplanner.py b/omniplanner/src/omniplanner/omniplanner.py index c697c4b..760fcf3 100644 --- a/omniplanner/src/omniplanner/omniplanner.py +++ b/omniplanner/src/omniplanner/omniplanner.py @@ -43,6 +43,7 @@ class PlanRequest: domain: PlanningDomain goal: PlanningGoal robot_states: dict + constraints: list = field(default_factory=list) # PDDL constraint facts @dataclass @@ -248,6 +249,7 @@ def ground_problem( intial_state: Any, goal: PlanningGoal, feedback: Any = None, + constraints: list = [], ) -> GroundedProblem: raise DispatchException(ground_problem, domain, map_context, goal, feedback) @@ -272,6 +274,7 @@ def full_planning_pipeline(plan_request: PlanRequest, map_context: Any, feedback plan_request.robot_states, plan_request.goal, feedback, + plan_request.constraints, ) logger.debug("Grounded Problem") diff --git a/omniplanner/src/omniplanner/tsp.py b/omniplanner/src/omniplanner/tsp.py index 4e57970..41edf06 100644 --- a/omniplanner/src/omniplanner/tsp.py +++ b/omniplanner/src/omniplanner/tsp.py @@ -150,7 +150,12 @@ class TspGoal: @overload @dispatch def ground_problem( - domain: TspDomain, dsg: Any, robot_states: dict, goal: TspGoal, feedback: Any = None + domain: TspDomain, + dsg: Any, + robot_states: dict, + goal: TspGoal, + feedback: Any = None, + constraints: list = [], ) -> RobotWrapper[GroundedTspProblem]: logger.info("Grounding TSP Problem") diff --git a/omniplanner_msgs/msg/PddlGoalMsg.msg b/omniplanner_msgs/msg/PddlGoalMsg.msg index 7f8d7a0..ad49037 100644 --- a/omniplanner_msgs/msg/PddlGoalMsg.msg +++ b/omniplanner_msgs/msg/PddlGoalMsg.msg @@ -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. diff --git a/omniplanner_ros/setup.py b/omniplanner_ros/setup.py index de03f8e..c827e86 100644 --- a/omniplanner_ros/setup.py +++ b/omniplanner_ros/setup.py @@ -43,6 +43,10 @@ def get_share_info(top_level, pattern, dest=None): license="TODO: License declaration", tests_require=["pytest"], entry_points={ - "console_scripts": ["omniplanner_node = omniplanner_ros.omniplanner_node:main"], + "console_scripts": [ + "omniplanner_node = omniplanner_ros.omniplanner_node:main", + "omniplanner_repair_node = omniplanner_ros.omniplanner_repair_node:main", + "goal_manager_node = omniplanner_ros.goal_manager_node:main", + ], }, ) diff --git a/omniplanner_ros/src/omniplanner_ros/goal_manager_node.py b/omniplanner_ros/src/omniplanner_ros/goal_manager_node.py new file mode 100644 index 0000000..b63b32d --- /dev/null +++ b/omniplanner_ros/src/omniplanner_ros/goal_manager_node.py @@ -0,0 +1,125 @@ +"""Goal manager node for plan repair: skip replanning when current plan already achieves the new goal.""" + +import json + +import rclpy +from omniplanner_msgs.msg import PddlGoalMsg +from rclpy.node import Node +from std_msgs.msg import Bool, String + +from omniplanner_ros.plan_utils import ( + extract_forbidden_pois, + extract_visited_places, + plan_visits_all, +) + + +class GoalManager(Node): + def __init__(self): + super().__init__("goal_manager") + + self.declare_parameter("commanded_goal_topic", "/hilbert/commanded_pddl_goal") + self.declare_parameter( + "omniplanner_goal_topic", + "/hilbert/omniplanner_node/multi_robot_pddl/pddl_goal", + ) + self.declare_parameter("robot_names", ["hilbert"]) + + commanded_topic = self.get_parameter("commanded_goal_topic").value + omniplanner_topic = self.get_parameter("omniplanner_goal_topic").value + robot_names = self.get_parameter("robot_names").value + + self.current_goal_formula = {} # robot_id -> PDDL string + self.stop_publishers = {} # robot_id -> Publisher(Bool) for executor stop topic + self.plan_visited_pois = {} # robot_id -> Set[str] of POIs in current plan + + self.pub = self.create_publisher(PddlGoalMsg, omniplanner_topic, 10) + self.sub = self.create_subscription( + PddlGoalMsg, + commanded_topic, + self.commanded_goal_cb, + 10, + ) + + # Subscribe to visited-POI updates published by omniplanner_node + for rn in robot_names: + topic = f"/{rn}/omniplanner_node/plan_visited_pois" + self.create_subscription( + String, + topic, + lambda msg, robot=rn: self._visited_pois_cb(robot, msg), + 10, + ) + self.get_logger().info(f"Subscribing to {topic} for plan-repair cache") + + def _visited_pois_cb(self, robot_id: str, msg: String): + pois = set(json.loads(msg.data)) + self.plan_visited_pois[robot_id] = pois + self.get_logger().info( + f"[{robot_id}] updated plan cache: {len(pois)} visited POIs" + ) + + def commanded_goal_cb(self, msg: PddlGoalMsg): + robot_id = msg.robot_id + requested_pois = extract_visited_places(msg.pddl_goal) + + # First, always request the executor to pause the current plan so the + # robot visibly pauses before we decide whether to replan. + if robot_id not in self.stop_publishers: + pause_topic = f"/{robot_id}/spot_executor_node/pause" + self.stop_publishers[robot_id] = self.create_publisher( + Bool, pause_topic, 10 + ) + stop_msg = Bool() + stop_msg.data = True + self.stop_publishers[robot_id].publish(stop_msg) + + # Plan-repair logic: if the current plan already visits all requested + # POIs and doesn't violate any constraints, resume without replanning. + cached_pois = self.plan_visited_pois.get(robot_id, set()) + self.get_logger().info(f"[{robot_id}] msg.constraints raw: '{msg.constraints}'") + forbidden = extract_forbidden_pois(msg.constraints) + if cached_pois and plan_visits_all(cached_pois, requested_pois, forbidden): + resume_topic = f"/{robot_id}/spot_executor_node/resume" + resume_pub = self.create_publisher(Bool, resume_topic, 10) + resume_msg = Bool() + resume_msg.data = True + resume_pub.publish(resume_msg) + self.get_logger().info( + f"[{robot_id}] current plan already visits all requested POIs " + f"{requested_pois} without forbidden {forbidden}, resuming" + ) + return + + # Not satisfied: forward to omniplanner for replanning. + self.current_goal_formula[robot_id] = msg.pddl_goal + + out = PddlGoalMsg() + out.robot_id = robot_id + out.pddl_goal = msg.pddl_goal + out.constraints = msg.constraints # forward constraints to omniplanner + self.pub.publish(out) + + # Log why we're replanning + if forbidden and forbidden & cached_pois: + reason = f"plan visits forbidden POIs {forbidden & cached_pois}" + elif not requested_pois.issubset(cached_pois): + reason = f"plan missing POIs {requested_pois - cached_pois}" + else: + reason = "no cached plan" + self.get_logger().info( + f"[{robot_id}] replanning: {reason}. requested={requested_pois} forbidden={forbidden} " + f"(cached: {cached_pois}), sent goal to omniplanner" + ) + + +def main(args=None): + rclpy.init(args=args) + node = GoalManager() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/omniplanner_ros/src/omniplanner_ros/last_pddl_plans.py b/omniplanner_ros/src/omniplanner_ros/last_pddl_plans.py new file mode 100644 index 0000000..d817526 --- /dev/null +++ b/omniplanner_ros/src/omniplanner_ros/last_pddl_plans.py @@ -0,0 +1,15 @@ +"""Symbolic plan cache for plan-repair: record latest PDDL plan per robot.""" + +from typing import Dict, Optional + +from dsg_pddl.dsg_pddl_planning import PddlPlan + +_last_plans: Dict[str, PddlPlan] = {} + + +def set_last_plan(robot_name: str, plan: PddlPlan) -> None: + _last_plans[robot_name] = plan + + +def get_last_plan(robot_name: str) -> Optional[PddlPlan]: + return _last_plans.get(robot_name) diff --git a/omniplanner_ros/src/omniplanner_ros/omniplanner_repair_node.py b/omniplanner_ros/src/omniplanner_ros/omniplanner_repair_node.py new file mode 100644 index 0000000..51de259 --- /dev/null +++ b/omniplanner_ros/src/omniplanner_ros/omniplanner_repair_node.py @@ -0,0 +1,209 @@ +"""Omniplanner node with execution-time plan repair and runtime constraints. + +Extends OmniPlannerRos with: +- Runtime constraints (forbidden-poi, forbidden-edge) via ~/constraints topic + and per-message constraints in PddlGoalMsg.constraints +- Visited POI publishing for goal_manager viability checks +- Better error handling with traceback logging +""" + +import json +import logging +import threading +import time +import traceback + +import rclpy +from omniplanner.compile_plan import collect_plans, compile_plan +from omniplanner.omniplanner import full_planning_pipeline +from rclpy.executors import MultiThreadedExecutor +from robot_executor_interface_ros.action_descriptions_ros import to_msg, to_viz_msg +from std_msgs.msg import String + +from omniplanner_ros.last_pddl_plans import get_last_plan +from omniplanner_ros.omniplanner_node import OmniPlannerRos +from omniplanner_ros.plan_utils import extract_visited_pois_from_plan + +logger = logging.getLogger(__name__) + + +class OmniPlannerRepairRos(OmniPlannerRos): + """OmniPlannerRos with plan repair, constraints, and POI publishing.""" + + def __init__(self): + # Add constraints state BEFORE super().__init__() because + # register_plugin is called during super().__init__() + self._repair_pre_init() + super().__init__() + self._repair_post_init() + + def _repair_pre_init(self): + """Initialize repair state before parent __init__ (which calls register_plugin).""" + self.constraints_lock = threading.Lock() + self.active_constraints = [] + + def _repair_post_init(self): + """Set up repair-specific subscriptions and publishers after parent __init__.""" + # Subscribe to constraints topic + self.create_subscription( + String, + "~/constraints", + self._constraints_callback, + 10, + ) + self.get_logger().info("Listening for constraints on ~/constraints") + + # Add visited POI publishers to each robot adaptor + for name, adaptor in self.robot_adaptors.items(): + adaptor.visited_pois_pub = self.create_publisher( + String, f"/{name}/omniplanner_node/plan_visited_pois", 1 + ) + + def _constraints_callback(self, msg: String): + """Accept a JSON-encoded list of constraint facts. + + Each fact is a list like ["forbidden-poi", "p123"] or + ["forbidden-edge", "p1", "p2"]. Replaces all active constraints. + Send [] to clear. + """ + try: + raw = json.loads(msg.data) + constraints = [tuple(c) for c in raw] + with self.constraints_lock: + self.active_constraints = constraints + self.get_logger().info(f"Updated constraints: {constraints}") + except Exception as e: + self.get_logger().error(f"Bad constraints message: {e}") + + def register_plugin(self, name, plugin): + """Override to add constraints injection, POI publishing, and error handling.""" + self.get_logger().info(f"Registering subscription plugin {name}") + msg_type, topic, callback = plugin.get_plan_callback() + self.feedback.plugin_feedback_collectors[name] = plugin.get_plugin_feedback( + self + ) + + def plan_handler(msg): + self.get_logger().info(f"Handling plan for plugin {name}") + + if self.dsg_last is None: + self.get_logger().error("Got plan request, but no DSG!") + return + + with self.current_planner_lock and self.plan_time_start_lock: + self.current_planner = name + self.plan_time_start = time.time() + + try: + # Log the received message + goal_str = getattr(msg, "pddl_goal", str(msg)) + constraints_str = getattr(msg, "constraints", "") + self.get_logger().info( + f"=== Received goal: {goal_str}" + + (f" | constraints: {constraints_str}" if constraints_str else "") + + " ===" + ) + + robot_poses = self.get_robot_poses(self.dsg_frame) + self.get_logger().info(f"Planning with robot poses {robot_poses}") + + plan_request = callback(msg, robot_poses) + + # Merge constraints: persistent (from ~/constraints topic) + # + per-message (from goal msg .constraints field) + with self.constraints_lock: + combined = list(self.active_constraints) + msg_constraints = getattr(msg, "constraints", "") + if msg_constraints: + try: + extra = [tuple(c) for c in json.loads(msg_constraints)] + combined.extend(extra) + with self.constraints_lock: + for c in extra: + if c not in self.active_constraints: + self.active_constraints.append(c) + self.get_logger().info( + f"Added {len(extra)} constraints from goal message" + ) + except Exception as e: + self.get_logger().error(f"Bad constraints in goal msg: {e}") + plan_request.constraints = combined + + with self.dsg_lock: + plans = full_planning_pipeline( + plan_request, self.dsg_last, self.feedback + ) + + compiled_plans = compile_plan( + self.robot_adaptors, self.dsg_frame, plans + ) + plan_dict = collect_plans(compiled_plans) + for robot_name, compiled_plan in plan_dict.items(): + self.robot_adaptors[robot_name].publish_plan(to_msg(compiled_plan)) + self.compiled_plan_viz_pub.publish( + to_viz_msg(compiled_plan, robot_name) + ) + + # Log the symbolic plan clearly + for robot_name in plan_dict: + cached = get_last_plan(robot_name) + if cached is not None: + self.get_logger().info( + f"=== Symbolic plan for {robot_name} ===" + ) + for i, action in enumerate(cached.symbolic_actions): + self.get_logger().info(f" [{i}] {action}") + + # Publish visited POIs per robot for goal_manager viability check + for robot_name in plan_dict: + cached = get_last_plan(robot_name) + if cached is not None and hasattr( + self.robot_adaptors[robot_name], "visited_pois_pub" + ): + pois = extract_visited_pois_from_plan(cached) + poi_msg = String() + poi_msg.data = json.dumps(sorted(pois)) + self.robot_adaptors[robot_name].visited_pois_pub.publish( + poi_msg + ) + + self.get_logger().info("Published Plan") + except Exception as e: + self.get_logger().error( + f"Planning failed for plugin {name}: {e}\n{traceback.format_exc()}" + ) + finally: + with self.current_planner_lock and self.plan_time_start_lock: + self.current_planner = None + self.plan_time_start = None + + resolved_topic_name = name + "/" + topic + self.get_logger().info( + f"Registering subscription for {resolved_topic_name} (type {str(msg_type)})" + ) + self.create_subscription( + msg_type, + f"~/{resolved_topic_name}", + plan_handler, + 1, + ) + + +def main(args=None): + rclpy.init(args=args) + try: + node = OmniPlannerRepairRos() + executor = MultiThreadedExecutor() + executor.add_node(node) + + try: + executor.spin() + finally: + executor.shutdown() + node.destroy_node() + finally: + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/omniplanner_ros/src/omniplanner_ros/pddl_planner_ros.py b/omniplanner_ros/src/omniplanner_ros/pddl_planner_ros.py index 7ce46c4..b3761c1 100644 --- a/omniplanner_ros/src/omniplanner_ros/pddl_planner_ros.py +++ b/omniplanner_ros/src/omniplanner_ros/pddl_planner_ros.py @@ -33,6 +33,7 @@ Place, ) +from omniplanner_ros.last_pddl_plans import set_last_plan from omniplanner_ros.omniplanner_node import PhoenixPlanningAdaptor logger = logging.getLogger(__name__) @@ -127,6 +128,10 @@ def add_to_plan(robot_name, symbolic_action, parameterized_action): f"Multi-robot plan has action {sym_action} that either doesn't to specify a robot, or specifies a robot other than one that is allowed by the planning problem!" ) + # Cache for plan repair + for rn, p in plan_per_robot.items(): + set_last_plan(rn, p) + logger.info("Plans before multi-robot projection: ") for v in plan_per_robot.values(): logger.info(v.symbolic_actions) @@ -148,6 +153,10 @@ def compile_pddl_plan( ): plan = contextualized_plan.value context = contextualized_plan.context + + # Cache for plan repair + set_last_plan(robot_name, plan) + actions = [] for symbolic_action, parameters in zip( plan.symbolic_actions, plan.parameterized_actions diff --git a/omniplanner_ros/src/omniplanner_ros/plan_utils.py b/omniplanner_ros/src/omniplanner_ros/plan_utils.py new file mode 100644 index 0000000..a2ce175 --- /dev/null +++ b/omniplanner_ros/src/omniplanner_ros/plan_utils.py @@ -0,0 +1,50 @@ +"""Helpers for plan-repair: extract visited goals and check plan satisfaction.""" + +import re +from typing import Set + +from dsg_pddl.dsg_pddl_planning import PddlPlan + +# Match visited-place, visited-object, and visited-poi goal predicates. +VISITED_RE = re.compile(r"\(visited-(?:place|object|poi)\s+([^\s\)]+)\)") + + +def extract_visited_places(pddl_goal: str) -> Set[str]: + """Extract all point-of-interest IDs from visited-place/visited-object goals.""" + return set(VISITED_RE.findall(pddl_goal)) + + +def extract_visited_pois_from_plan(plan: PddlPlan) -> Set[str]: + """Extract all POI IDs visited by goto-poi actions in the plan.""" + pois: Set[str] = set() + for sym in plan.symbolic_actions: + if len(sym) >= 2 and sym[0] == "goto-poi": + pois.add(sym[-1]) + return pois + + +def extract_forbidden_pois(constraints_json: str) -> Set[str]: + """Extract forbidden POI IDs from a JSON constraints string.""" + import json + + forbidden: Set[str] = set() + if not constraints_json: + return forbidden + try: + for c in json.loads(constraints_json): + if len(c) >= 2 and c[0] == "forbidden-poi": + forbidden.add(c[1]) + except Exception: + pass + return forbidden + + +def plan_visits_all( + visited_pois: Set[str], requested: Set[str], forbidden: Set[str] = set() +) -> bool: + """Check whether the plan covers all requested targets without visiting forbidden POIs.""" + if not requested: + return False + if forbidden and forbidden & visited_pois: + return False # plan visits a forbidden POI → must replan + return requested.issubset(visited_pois)