ros for multiplanner jaeyoun local

JY-HIM4U · JY-HIM4U · commit 6deb31b8479a · 2025-10-14T11:28:00.000-04:00
diff --git a/omniplanner/examples/pddl_example.py b/omniplanner/examples/pddl_example.py
@@ -1,12 +1,10 @@
 import logging
-from importlib.resources import as_file, files
 
-import dsg_pddl.domains
 import numpy as np
 import spark_dsg
 from dsg_pddl.pddl_grounding import PddlDomain, PddlGoal
 from ruamel.yaml import YAML
-from utils import DummyRobotPlanningAdaptor
+from utils import DummyRobotPlanningAdaptor, load_omniplanner_pddl_domain
 
 from omniplanner.compile_plan import collect_plans
 from omniplanner.omniplanner import PlanRequest, full_planning_pipeline
@@ -22,7 +20,7 @@
 # goal = PddlGoal(robot_id="euclid", pddl_goal="(and (visited-object o0) (visited-object o1))")
 # goal = PddlGoal(robot_id="euclid", pddl_goal="(object-in-place o1 p0)")
 G = spark_dsg.DynamicSceneGraph.load(
-    "/home/ubuntu/lxc_datashare/west_point_fused_map_wregions_labelspace.json"
+    "/home/jaeyoun-choi/colcon_ws/assets/west_point_fused_map_wregions_labelspace.json"
 )
 
 
@@ -43,10 +41,7 @@
 )
 
 # Load the PDDL domain you want to use
-with as_file(files(dsg_pddl.domains).joinpath("GotoObjectDomain.pddl")) as path:
-    print(f"Loading domain {path}")
-    with open(str(path), "r") as fo:
-        domain = PddlDomain(fo.read())
+domain = PddlDomain(load_omniplanner_pddl_domain("GotoObjectDomain.pddl"))
 
 
 # Build the plan request
@@ -62,7 +57,7 @@
 print("Plan from planning domain:")
 print(plan)
 
-compiled_plan = compile_plan(adaptors, plan)
+compiled_plan = compile_plan(adaptors, "map", plan)
 print(compiled_plan)
 
 collected_plans = collect_plans(compiled_plan)
@@ -77,12 +72,7 @@
 goal = PddlGoal(robot_id="euclid", pddl_goal="(and (object-in-place o94 p2157))")
 
 # Load the PDDL domain you want to use
-with as_file(
-    files(dsg_pddl.domains).joinpath("ObjectRearrangementDomain.pddl")
-) as path:
-    print(f"Loading domain {path}")
-    with open(str(path), "r") as fo:
-        domain = PddlDomain(fo.read())
+domain = PddlDomain(load_omniplanner_pddl_domain("ObjectRearrangementDomain.pddl"))
 
 
 # Build the plan request
@@ -98,7 +88,7 @@
 print("Plan from planning domain:")
 print(plan)
 
-collected_plan = collect_plans(compile_plan(adaptors, plan))
+collected_plan = collect_plans(compile_plan(adaptors, "map", plan))
 print("collected plan: ", collected_plan)
 
 
@@ -117,12 +107,9 @@
 )
 
 # Load the PDDL domain you want to use
-with as_file(
-    files(dsg_pddl.domains).joinpath("RegionObjectRearrangementDomain.pddl")
-) as path:
-    print(f"Loading domain {path}")
-    with open(str(path), "r") as fo:
-        domain = PddlDomain(fo.read())
+domain = PddlDomain(
+    load_omniplanner_pddl_domain("RegionObjectRearrangementDomain.pddl")
+)
 
 
 # Build the plan request
@@ -139,5 +126,5 @@
 print(plan)
 
 
-collected_plan = collect_plans(compile_plan(adaptors, plan))
+collected_plan = collect_plans(compile_plan(adaptors, "map", plan))
 print("collected plan: ", collected_plan)
diff --git a/omniplanner/examples/pddl_example_multirobot_FD_ominiplanner.py b/omniplanner/examples/pddl_example_multirobot_FD_ominiplanner.py
@@ -4,19 +4,25 @@
 Follows the exact format you wanted using existing infrastructure
 """
 
-import sys
+import logging
 
 import numpy as np
 import spark_dsg
 from dsg_pddl.pddl_grounding import MultiRobotPddlDomain, PddlGoal
+from utils import DummyRobotPlanningAdaptor, load_omniplanner_pddl_domain
 
+from omniplanner.compile_plan import collect_plans
 from omniplanner.omniplanner import PlanRequest, full_planning_pipeline
+from omniplanner_ros.pddl_planner_ros import compile_plan
 
-multi_robot_path = (
-    "/home/jaeyoun-choi/colcon_ws/src/awesome_dcist_t4/omniplanner/omniplanner/src"
-)
-if multi_robot_path not in sys.path:
-    sys.path.insert(0, multi_robot_path)
+logging.basicConfig(level=logging.DEBUG, force=True)
+
+
+# multi_robot_path = (
+#    "/home/ubuntu/lxc_datashare/heracles_evaluation_dsg_data/"
+# )
+# if multi_robot_path not in sys.path:
+#    sys.path.insert(0, multi_robot_path)
 
 
 def extract_plan_from_wrapper(plan):
@@ -215,77 +221,87 @@ def print_places_in_region(G, region_symbol: str):
         print(f"  Error listing places for {region_symbol}: {e}")
 
 
-def main():
-    """Main test function following your exact format"""
-    print("Multi-Robot Fast Downward Test with Real Scene Graph")
-    print("=" * 80)
-
-    # Configuration
-    # scene_graph_path = "./src/awesome_dcist_t4/omniplanner/omniplanner/examples/scenegraph/west_point_fused_map_wregions_labelspace.json"
-    scene_graph_path = "/home/jaeyoun-choi/colcon_ws/assets/b45_clip_final_connected_rooms_and_labelspace.json"
-    # scene_graph_path = "/home/jaeyoun-choi/colcon_ws/assets/west_point_fused_map_wregions_labelspace.json"
-    # robot_ids = ["robot1","robot2"]
-    robot_ids = ["robot1", "robot2", "robot3"]
-
-    print(f"Scene graph: {scene_graph_path}")
-    print(f"Robots: {robot_ids}")
-
-    # Load scene graph
-    print(f"Loading scene graph from: {scene_graph_path}")
-    G = spark_dsg.DynamicSceneGraph.load(scene_graph_path)
-    print(f"✓ Scene graph loaded: {G.num_nodes()} total nodes")
-
-    # Examine scene graph coordinates
-    examine_scene_graph_coordinates(G)
-    # Print existing region kinds
-
-    print_region_kinds(G)
-    # Print region names
-    print_region_names(G)
-    # Print places included in specific regions of interest
-    # print_places_in_region(G, "r68")
-    print_places_in_region(G, "r3")
-    print_places_in_region(G, "r4")
-
-    # Create robot poses (following your format)
-    robot_poses = {
-        "robot1": np.array([-15.0, -15.1]),
-        "robot2": np.array([-15.0, 0.1]),
-        "robot3": np.array([0.0, 6.0]),
-    }
-
-    print("=========================================")
-    print("==== PDDL region Domain (Multi-Robot) ====")
-    print("=========================================")
-    print("")
-
-    # goal_string ="(and (safe o2)(safe o3))"
-    goal_string = "(and (explored-region r1)(explored-region r2)(visited-object o2)(visited-object o9)(visited-place p22543)(visited-place p6255))"
-    goal_string = "(and (in-region robot1 r2) (visited-object o79)(visited-object o285)(visited-object o43)(safe o79)(explored-region r2)(visited-object o2)(visited-object o9)(visited-place p22543)(visited-place p6255))"
-    # goal_string ="(and (visited-poi o27))"
-    # goal_string ="(and (object-in-place o5 p91) (object-in-place o85 p118) )"
-    # goal_string ="(and (object-in-place o5 p91) (object-in-place o94 p2157))"
-    # goal_string ="(and (safe o2))"
-    # goal_string ="(and (object-in-place o5 p91) (object-in-place o85 p118) (object-in-place o94 p2157))"
-    goal = PddlGoal(robot_id="robot1", pddl_goal=goal_string)
-
-    # Load the multi-robot domain
-    domain_path = "./src/awesome_dcist_t4/omniplanner/omniplanner/src/dsg_pddl/domains/RegionObjectRearrangementDomain_MultiRobot_FD_Explore.pddl"
-    with open(domain_path, "r") as fo:
-        domain = MultiRobotPddlDomain(fo.read())
-
-    # print(f"Loading domain {domain_path}")
-    # print(f"Domain name: {domain.domain_name}")
-    req = PlanRequest(
-        domain=domain,
-        goal=goal,
-        robot_states=robot_poses,
+"""Main test function following your exact format"""
+print("Multi-Robot Fast Downward Test with Real Scene Graph")
+print("=" * 80)
+
+# Configuration
+# scene_graph_path = "./src/awesome_dcist_t4/omniplanner/omniplanner/examples/scenegraph/west_point_fused_map_wregions_labelspace.json"
+scene_graph_path = "/home/jaeyoun-choi/colcon_ws/assets/b45_clip_final_connected_rooms_and_labelspace.json"
+# scene_graph_path = "/home/jaeyoun-choi/colcon_ws/assets/west_point_fused_map_wregions_labelspace.json"
+# robot_ids = ["robot1","robot2"]
+robot_ids = ["robot1", "robot2", "robot3"]
+
+print(f"Scene graph: {scene_graph_path}")
+print(f"Robots: {robot_ids}")
+
+# Load scene graph
+print(f"Loading scene graph from: {scene_graph_path}")
+G = spark_dsg.DynamicSceneGraph.load(scene_graph_path)
+print(f"✓ Scene graph loaded: {G.num_nodes()} total nodes")
+
+# Examine scene graph coordinates
+examine_scene_graph_coordinates(G)
+# Print existing region kinds
+
+print_region_kinds(G)
+# Print region names
+print_region_names(G)
+# Print places included in specific regions of interest
+# print_places_in_region(G, "r68")
+print_places_in_region(G, "r3")
+print_places_in_region(G, "r4")
+
+# Create robot poses (following your format)
+robot_poses = {
+    "ROBOT1": np.array([-15.0, -15.1]),
+    "ROBOT2": np.array([-15.0, 0.1]),
+    "ROBOT3": np.array([0.0, 6.0]),
+}
+
+print("=========================================")
+print("==== PDDL region Domain (Multi-Robot) ====")
+print("=========================================")
+print("")
+
+# goal_string ="(and (safe o2)(safe o3))"
+goal_string = "(and (explored-region r1)(explored-region r2)(visited-object o2)(visited-object o9)(visited-place p22543)(visited-place p6255))"
+goal_string = "(and (visited-object o79)(visited-object o285)(visited-object o43)(safe o79)(explored-region r2)(visited-object o2)(visited-object o9)(visited-place p22543)(visited-place p6255))"
+# goal_string ="(and (visited-poi o27))"
+# goal_string ="(and (object-in-place o5 p91) (object-in-place o85 p118) )"
+# goal_string ="(and (object-in-place o5 p91) (object-in-place o94 p2157))"
+# goal_string ="(and (safe o2))"
+# goal_string ="(and (object-in-place o5 p91) (object-in-place o85 p118) (object-in-place o94 p2157))"
+goal = PddlGoal(robot_id="robot1", pddl_goal=goal_string)
+
+# Load the multi-robot domain
+domain = MultiRobotPddlDomain(
+    load_omniplanner_pddl_domain(
+        "RegionObjectRearrangementDomain_MultiRobot_FD_Explore.pddl"
     )
-    # Automatically generate a multi-robot PDDL problem from DSG (first pass to discover objects)
-    # pddl_start_time = time.time()
-    full_planning_pipeline(req, G)
-    # actual_plan = extract_plan_from_wrapper(plan)
+)
 
 
-if __name__ == "__main__":
-    main()
+# print(f"Loading domain {domain_path}")
+# print(f"Domain name: {domain.domain_name}")
+req = PlanRequest(
+    domain=domain,
+    goal=goal,
+    robot_states=robot_poses,
+)
+# Automatically generate a multi-robot PDDL problem from DSG (first pass to discover objects)
+# pddl_start_time = time.time()
+plan = full_planning_pipeline(req, G)
+print("Symbolic Plan:")
+for a in plan.value.value.symbolic_actions:
+    print(a)
+
+adaptor1 = DummyRobotPlanningAdaptor("euclid", "spot", "map", "euclid/body")
+adaptor2 = DummyRobotPlanningAdaptor("hamilton", "spot", "map", "hamilton/body")
+adaptor3 = DummyRobotPlanningAdaptor("gauss", "husky", "map", "husky/body")
+adaptors = {"ROBOT1": adaptor1, "ROBOT2": adaptor2, "ROBOT3": adaptor3}
+
+compiled_plans = compile_plan(adaptors, "map", plan)
+
+collected_plan = collect_plans(compile_plan(adaptors, "map", plan))
+print("collected plan: ", collected_plan)
diff --git a/omniplanner/examples/utils.py b/omniplanner/examples/utils.py
@@ -1,9 +1,19 @@
 from dataclasses import dataclass
+from importlib.resources import as_file, files
 
+import dsg_pddl.domains
 import numpy as np
 import spark_dsg
 
 
+def load_omniplanner_pddl_domain(domain_name):
+    with as_file(files(dsg_pddl.domains).joinpath(domain_name)) as path:
+        print(f"Loading domain {path}")
+        with open(str(path), "r") as fo:
+            domain = fo.read()
+    return domain
+
+
 @dataclass
 class DummyRobotPlanningAdaptor:
     name: str
diff --git a/omniplanner/src/dsg_pddl/dsg_pddl_grounding_Multirobot_Better.py b/omniplanner/src/dsg_pddl/dsg_pddl_grounding_Multirobot_Better.py
@@ -27,7 +27,7 @@
     PddlSymbol,
 )
 from dsg_pddl.pddl_utils import lisp_string_to_ast
-from omniplanner.omniplanner import RobotWrapper
+from omniplanner.omniplanner import MultiRobotWrapper
 from omniplanner.tsp import LayerPlanner
 
 logger = logging.getLogger(__name__)
@@ -366,9 +366,10 @@ def ground_problem(
     robot_states: dict,
     goal: PddlGoal,
     feedback: Any = None,
-) -> RobotWrapper[GroundedPddlProblem]:
+) -> MultiRobotWrapper[GroundedPddlProblem]:
     logger.info(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(
@@ -377,14 +378,25 @@ def ground_problem(
 
         case "region-object-rearrangement-domain-multirobot-fd":
             pddl_problem, symbols = generate_multirobot_region_pddl(
-                dsg, goal.pddl_goal, robot_states
+                dsg, goal.pddl_goal, pddl_compliant_robot_states
             )
         case _:
             raise NotImplementedError(
                 f"I don't know how to ground a domain of type {domain.domain_name}!"
             )
 
     symbol_dict = {s.symbol: s for s in symbols}
-    return RobotWrapper(
-        goal.robot_id, GroundedPddlProblem(domain, pddl_problem, symbol_dict)
+
+    # TODO: We don't actually want to rely on the robot_states, because
+    # robot_states may contain information about robots we actually don't care
+    # about for planning purposes.  Instead, we probably want this information
+    # passed as part of the goal.
+
+    names = list(robot_states.keys())
+    wrapper = MultiRobotWrapper(
+        names, GroundedPddlProblem(domain, pddl_problem, symbol_dict)
     )
+    for outer_name in robot_states.keys():
+        inner_name = outer_name.lower()
+        wrapper.set_name_remap(outer_name, inner_name)
+    return wrapper
diff --git a/omniplanner/src/dsg_pddl/dsg_pddl_planning.py b/omniplanner/src/dsg_pddl/dsg_pddl_planning.py
@@ -9,7 +9,7 @@
 import spark_dsg
 from plum import dispatch
 
-from dsg_pddl.dsg_pddl_grounding import GroundedPddlProblem, PddlSymbol
+from dsg_pddl.dsg_pddl_grounding import GroundedPddlProblem, PddlDomain, PddlSymbol
 from dsg_pddl.pddl_utils import lisp_string_to_ast
 from omniplanner.tsp import LayerPlanner
 
@@ -18,6 +18,7 @@
 
 @dataclass
 class PddlPlan:
+    domain: PddlDomain
     symbolic_actions: List[tuple]
     parameterized_actions: List
     symbols: Dict[str, PddlSymbol]
@@ -108,4 +109,6 @@ def make_plan(grounded_problem: GroundedPddlProblem, map_context: Any) -> PddlPl
                     f"Plan contains {p[0]} action, but I don't know how to parameterize!"
                 )
 
-    return PddlPlan(plan, parameterized_plan, grounded_problem.symbols)
+    return PddlPlan(
+        grounded_problem.domain, plan, parameterized_plan, grounded_problem.symbols
+    )
diff --git a/omniplanner/src/dsg_pddl/pddl_grounding.py b/omniplanner/src/dsg_pddl/pddl_grounding.py
@@ -130,19 +130,8 @@ def to_string(self):
         return ast_to_string(self.domain_ast)
 
 
-class MultiRobotPddlDomain:
-    def __init__(self, domain_str):
-        self.domain_ast = lisp_string_to_ast(domain_str)
-        self.domain_name = get_domain_name(self.domain_ast)
-        self.requirements = get_domain_requirements(self.domain_ast)
-        self.get_domain_types = get_domain_types(self.domain_ast)
-        self.predicates = get_domain_predicates(self.domain_ast)
-        self.functions = get_functions(self.domain_ast)
-        self.derived = get_derived(self.domain_ast)
-        self.actions = get_actions(self.domain_ast)
-
-    def to_string(self):
-        return ast_to_string(self.domain_ast)
+class MultiRobotPddlDomain(PddlDomain):
+    pass
 
 
 def get_domain_name(ast):
diff --git a/omniplanner/src/omniplanner/compile_plan.py b/omniplanner/src/omniplanner/compile_plan.py
diff --git a/omniplanner/src/omniplanner/omniplanner.py b/omniplanner/src/omniplanner/omniplanner.py
diff --git a/omniplanner_ros/src/omniplanner_ros/pddl_planner_ros.py b/omniplanner_ros/src/omniplanner_ros/pddl_planner_ros.py
