Planning domain adaptations for issue #228

.github/CODEOWNERS

@@ -1,7 +1,4 @@
 # Default owners for everything in the repo.
 # Unless a later match takes precedence, they will be requested for review
 # when someone opens a pull request.
-* @argenos @alex-mitrevski @PatrickNa @b-it-bots/pullpanda-junior @b-it-bots/pullpanda-senior
-
-mdr_perception @minhnh
-mdr_navigation @argenos
+* @argenos @alex-mitrevski @minhnh

mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch

@@ -1,12 +1,16 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="ontology_url" default="file://$(find mas_knowledge_base)/common/ontology/apartment.owl" />
+    <arg name="ontology_url" default="file://$(find mas_knowledge_base)/common/ontology/apartment_go_2019.owl" />
+    <arg name="ontology_base_url" default="" />
+    <arg name="ontology_entity_delimiter" default="/" />
     <arg name="ontology_class_prefix" default="apartment" />
     <arg name="retry_count_on_failure" default="0" />
     <arg name="timeout_s" default="120" />
 
     <node pkg="mdr_find_object_action" type="find_object_action" name="find_object_server" output="screen">
         <param name="ontology_url" value="$(arg ontology_url)" />
+        <param name="ontology_base_url" value="$(arg ontology_base_url)" />
+        <param name="ontology_entity_delimiter" value="$(arg ontology_entity_delimiter)" />
         <param name="ontology_class_prefix" value="$(arg ontology_class_prefix)" />
         <param name="retry_count_on_failure" value="$(arg retry_count_on_failure)" />
         <param name="timeout_s" value="$(arg timeout_s)" />

mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action

@@ -21,6 +21,8 @@ class FindObjectServer(object):
     '''
     def __init__(self):
         ontology_url = rospy.get_param('~ontology_url', '')
+        ontology_base_url = rospy.get_param('~ontology_base_url', '')
+        ontology_entity_delimiter = rospy.get_param('~ontology_entity_delimiter', '')
         ontology_class_prefix = rospy.get_param('~ontology_class_prefix', '')
         retry_count_on_failure = int(rospy.get_param('~retry_count_on_failure', 0))
         timeout_s = float(rospy.get_param('~timeout_s', 120.))
@@ -32,6 +34,8 @@ class FindObjectServer(object):
 
         rospy.loginfo('[find_object] Initialising state machine')
         self.action_sm = FindObjectSM(ontology_url=ontology_url,
+                                      ontology_base_url=ontology_base_url,
+                                      ontology_entity_delimiter=ontology_entity_delimiter,
                                       ontology_class_prefix=ontology_class_prefix,
                                       number_of_retries=retry_count_on_failure,
                                       timeout=timeout_s)

mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py

@@ -8,12 +8,16 @@
 
 class FindObjectSM(ActionSMBase):
     def __init__(self, ontology_url,
+                 ontology_base_url,
+                 ontology_entity_delimiter,
                  ontology_class_prefix,
                  number_of_retries=0,
                  timeout=120.,
                  max_recovery_attempts=1):
         super(FindObjectSM, self).__init__('FindObject', [], max_recovery_attempts)
         self.ontology_url = ontology_url
+        self.ontology_base_url = ontology_base_url
+        self.ontology_entity_delimiter = ontology_entity_delimiter
         self.ontology_class_prefix = ontology_class_prefix
         self.number_of_retries = number_of_retries
         self.timeout = timeout
@@ -23,8 +27,10 @@ def __init__(self, ontology_url,
     def init(self):
         try:
             rospy.loginfo('[find_object] Creating an interface client for ontology %s', self.ontology_url)
-            self.ontology_interface = DomesticOntologyInterface(self.ontology_url,
-                                                                self.ontology_class_prefix)
+            self.ontology_interface = DomesticOntologyInterface(ontology_file=self.ontology_url,
+                                                                base_url=self.ontology_base_url,
+                                                                entity_delimiter=self.ontology_entity_delimiter,
+                                                                class_prefix=self.ontology_class_prefix)
         except Exception as exc:
             rospy.logerr('[find_object] Could not create an ontology interface client: %s', exc)
 

mdr_planning/mdr_rosplan_interface/config/default_domain.pddl

@@ -1,118 +1,185 @@
 (define (domain default-domestic-domain)
 
-    (:requirements :strips :typing :fluents :disjunctive-preconditions :durative-actions)
+    (:requirements :strips :typing :equality)
 
     (:types
-        waypoint
-        object
-        category
-        robot
-        door
-        plane
-        person
+        Thing
+        Object - Thing
+        Location - Thing
+        Waypoint - Location
+        Furniture - Object
+        Category
+        Robot
+        Door
+        Plane
+        Person
+        NamedPose
+        GraspingStrategy
+        Context
     )
 
+    (:constants pick_from_plane pick_from_container place_on_plane place_in_container - Context)
+
     (:predicates
-        (robot_name ?bot - robot)
-        (object_category ?obj - object ?cat - category)
-        (robot_at ?bot - robot ?wp - waypoint)
-        (door_at ?door - door ?wp - waypoint)
-        (object_at ?obj - object ?wp - waypoint)
-        (plane_at ?plane - plane ?wp - waypoint)
-        (door_open ?door - door)
-        (belongs_to ?plane - plane ?obj - object)
-        (unexplored ?plane - plane)
-        (explored ?plane - plane)
-        (on ?obj - object ?plane - plane)
-        (in ?obj - object ?source - object)
-        (holding ?bot - robot ?obj - object)
-        (empty_gripper ?bot - robot)
-        (known ?person - person)
-        (unknown ?person - person)
+        (robotName ?Robot - Robot)
+        (objectCategory ?Object0 - Object ?Object1 - Object)
+        (robotAt ?Robot - Robot ?Waypoint - Waypoint)
+        (doorAt ?Door - Door ?Waypoint - Waypoint)
+        (objectAt ?Object - Object ?Waypoint - Waypoint)
+        (furnitureAt ?Furniture - Furniture ?Waypoint - Waypoint)
+        (planeAt ?Plane - Plane ?Waypoint - Waypoint)
+        (personAt ?Person - Person ?Waypoint - Waypoint)
+        (doorOpen ?Door - Door)
+        (belongsTo ?Plane - Plane ?Object - Object)
+        (unexplored ?Plane - Plane)
+        (explored ?Plane - Plane)
+        (on ?Object - Object ?Plane - Plane)
+        (in ?Object0 - Object ?Object1 - Object)
+        (in ?Object - Object ?Furniture - Furniture)
+        (holding ?Robot - Robot ?Object - Object)
+        (holding ?Person - Person ?Object - Object)
+        (emptyGripper ?Robot - Robot)
+        (known ?Person - Person)
+        (unknown ?Person - Person)
+
+        (canPlaceOn ?Object - Object ?Plane - Plane)
+        (defaultStoringLocation ?Object - Object ?Furniture - Furniture)
+        (likelyLocation ?Object - Object ?Furniture - Furniture)
+        (locatedAt ?Object - Object ?Location - Location)
+        (hasDoor ?Furniture - Furniture)
+        (above ?Object0 - Object ?Object1 - Object)
+        (below ?Object0 - Object ?Object1 - Object)
+        (onTopOf ?Object0 - Object ?Object1 - Object)
+        (inside ?Object0 - Object ?Object1 - Object)
+        (toTheLeftOf ?Object0 - Object ?Object1 - Object)
+        (toTheRightOf ?Object0 - Object ?Object1 - Object)
+        (isAtNamedPose ?Thing - Thing ?NamedPose - NamedPose)
+        (isAtLocation ?NamedPose - NamedPose ?Location - Location)
+        (preferredGraspingStrategy ?Object - Object ?GraspingStrategy - GraspingStrategy)
+    )
+
+    (:action MoveBase
+        :parameters (?Robot - Robot ?Location0 ?Location1 - Location)
+        :precondition (and
+            (robotAt ?Robot ?Location0)
+        )
+        :effect (and
+            (not (robotAt ?Robot ?Location0))
+            (robotAt ?Robot ?Location1)
+        )
+    )
+
+    (:action Open
+        :parameters (?Door - Door ?Robot - Robot ?Waypoint - Waypoint)
+        :precondition (and
+            (doorAt ?Door ?Waypoint)
+            (robotAt ?Robot ?Waypoint)
+        )
+        :effect (and
+            (doorOpen ?Door)
+        )
+    )
+
+    (:action PerceivePlane
+        :parameters (?Plane - Plane ?Robot - Robot ?Waypoint - Waypoint)
+        :precondition (and
+            (robotAt ?Robot ?Waypoint)
+            (planeAt ?Plane ?Waypoint)
+            (unexplored ?Plane)
+        )
+        :effect (and
+            (not (unexplored ?Plane))
+            (explored ?Plane)
+        )
     )
 
-    (:durative-action move_base
-        :parameters (?bot - robot ?from ?to - waypoint)
-        :duration ( = ?duration 10)
-        :condition (and
-            (at start (robot_at ?bot ?from))
+    (:action Pick
+        :parameters (?Object - Object ?Plane - Plane ?Robot - Robot ?Waypoint - Waypoint ?Context - Context)
+        :precondition (and
+            (= ?Context pick_from_plane)
+            (robotAt ?Robot ?Waypoint)
+            (planeAt ?Plane ?Waypoint)
+            (explored ?Plane)
+            (on ?Object ?Plane)
+            (emptyGripper ?Robot)
         )
         :effect (and
-            (at start (not (robot_at ?bot ?from)))
-            (at end (robot_at ?bot ?to))
+            (not (on ?Object ?Plane))
+            (not (emptyGripper ?Robot))
+            (holding ?Robot ?Object)
         )
     )
 
-    (:durative-action open_cupboard
-        :parameters (?cupboard - door ?bot - robot ?c - waypoint)
-        :duration ( = ?duration 10)
-        :condition (and
-            (at start (door_at ?cupboard ?c))
-            (at start (robot_at ?bot ?c))
+    (:action Pick
+        :parameters (?Object - Object ?Furniture - Furniture ?Robot - Robot ?Waypoint - Waypoint ?Context - Context)
+        :precondition (and
+            (= ?Context pick_from_container)
+            (robotAt ?Robot ?Waypoint)
+            (furnitureAt ?Furniture ?Waypoint)
+            (in ?Object ?Furniture)
+            (emptyGripper ?Robot)
         )
         :effect (and
-            (at end (door_open ?cupboard))
+            (not (in ?Object ?Furniture))
+            (not (emptyGripper ?Robot))
+            (holding ?Robot ?Object)
         )
     )
 
-    (:durative-action perceive_plane
-        :parameters (?plane - plane ?bot - robot ?wp - waypoint)
-        :duration ( = ?duration 10)
-        :condition (and
-            (at start (robot_at ?bot ?wp))
-            (at start (plane_at ?plane ?wp))
-            (at start (unexplored ?plane))
+    (:action Place
+        :parameters (?Object - Object ?Plane - Plane ?Robot - Robot ?Waypoint - Waypoint ?Context - Context)
+        :precondition (and
+            (= ?Context place_on_plane)
+            (robotAt ?Robot ?Waypoint)
+            (planeAt ?Plane ?Waypoint)
+            (holding ?Robot ?Object)
         )
         :effect (and
-            (at start (not (unexplored ?plane)))
-            (at end (explored ?plane))
+            (not (holding ?Robot ?Object))
+            (emptyGripper ?Robot)
+            (on ?Object ?Plane)
         )
     )
 
-    (:durative-action pickup
-        :parameters (?obj - object ?plane - plane ?bot - robot ?wp - waypoint)
-        :duration ( = ?duration 10)
-        :condition (and
-            (at start (robot_at ?bot ?wp))
-            (at start (plane_at ?plane ?wp))
-            (at start (explored ?plane))
-            (at start (on ?obj ?plane))
-            (at start (empty_gripper ?bot))
+    (:action Place
+        :parameters (?Object - Object ?Furniture - Furniture ?Robot - Robot ?Waypoint - Waypoint ?Context - Context)
+        :precondition (and
+            (= ?Context place_in_container)
+            (robotAt ?Robot ?Waypoint)
+            (furnitureAt ?Furniture ?Waypoint)
+            (holding ?Robot ?Object)
         )
         :effect (and
-            (at start (not (on ?obj ?plane)))
-            (at start (not (empty_gripper ?bot)))
-            (at end (holding ?bot ?obj))
+            (not (holding ?Robot ?Object))
+            (emptyGripper ?Robot)
+            (in ?Object ?Furniture)
         )
     )
 
-    (:durative-action place
-        :parameters (?obj - object ?plane - plane ?bot - robot ?wp - waypoint)
-        :duration ( = ?duration 10)
-        :condition (and
-            (at start (robot_at ?bot ?wp))
-            (at start (plane_at ?plane ?wp))
-            (at start (holding ?bot ?obj))
+    (:action Throw
+        :parameters (?Object0 ?Object1 - Object ?Robot - Robot ?Waypoint - Waypoint)
+        :precondition (and
+            (robotAt ?Robot ?Waypoint)
+            (objectAt ?Object1 ?Waypoint)
+            (holding ?Robot ?Object0)
         )
         :effect (and
-            (at start (not (holding ?bot ?obj)))
-            (at start (empty_gripper ?bot))
-            (at end (on ?obj ?plane))
+            (not (holding ?Robot ?Object0))
+            (emptyGripper ?Robot)
+            (in ?Object0 ?Object1)
         )
     )
 
-    (:durative-action throw
-        :parameters (?obj - object ?dest_obj - object ?bot - robot ?wp - waypoint)
-        :duration ( = ?duration 10)
-        :condition (and
-            (at start (robot_at ?bot ?wp))
-            (at start (object_at ?dest_obj ?wp))
-            (at start (holding ?bot ?obj))
+    (:action HandOver
+        :parameters (?Object - Object ?Robot - Robot ?Person - Person ?Waypoint - Waypoint)
+        :precondition (and
+            (robotAt ?Robot ?Waypoint)
+            (personAt ?Person ?Waypoint)
+            (holding ?Robot ?Object)
         )
         :effect (and
-            (at start (not (holding ?bot ?obj)))
-            (at start (empty_gripper ?bot))
-            (at end (in ?obj ?dest_obj))
+            (not (holding ?Robot ?Object))
+            (holding ?Person ?Object)
         )
     )
 )

mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch

@@ -2,15 +2,17 @@
 <launch>
     <arg name="knowledge_base_path" default="$(find mdr_knowledge_base)/common/mongoDB" />
     <arg name="planner_path" default="$(find rosplan_planning_system)/common/bin/" />
-    <arg name="data_path" default="" />
+    <arg name="data_path" default="$(find mdr_rosplan_interface)/config/" />
     <arg name="domain_path" default="$(find mdr_rosplan_interface)/config/default_domain.pddl" />
     <arg name="problem_path" default="$(find mdr_rosplan_interface)/config/problem.pddl" />
-    <arg name="strl_file_path" default="$(find mdr_rosplan_interface)/config/plan.pddl" />
-    <arg name="planner_command" default="$(find rosplan_planning_system)/common/bin/fast-downward/fast-downward.py --search-time-limit 10 --alias seq-sat-lama-2011 DOMAIN PROBLEM" />
+    <arg name="strl_file_path" default="$(find mdr_rosplan_interface)/config/plan/sas_plan" />
+    <arg name="planner_command" default="$(find lama_planner)/fast-downward/fast-downward.py --plan-file $(arg strl_file_path) --search-time-limit 10 --alias seq-sat-lama-2011 DOMAIN PROBLEM" />
     <arg name="action_dispatch_topic" default="/kcl_rosplan/action_dispatch" />
     <arg name="action_feedback_topic" default="/kcl_rosplan/action_feedback" />
 
     <param name="/ontology_url" value="file://$(find mas_knowledge_base)/common/ontology/apartment_go_2019.owl" />
+    <param name="/ontology_base_url" value="" />
+    <param name="/ontology_entity_delimiter" value="/" />
     <param name="/ontology_class_prefix" value="apartment" />
 
     <!-- data paths -->

mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py

@@ -15,9 +15,6 @@ class ActionClientBase(object):
 
     '''
     def __init__(self):
-        self.action_success_msg = 'action achieved'
-        self.action_failure_msg = 'action failed'
-
         # unique action ID
         self.action_id = -1
 
@@ -87,9 +84,9 @@ def send_action_feedback(self, success):
         msg = plan_dispatch_msgs.ActionFeedback()
         msg.action_id = self.action_id
         if success:
-            msg.status = self.action_success_msg
+            msg.status = plan_dispatch_msgs.ActionFeedback.ACTION_SUCCEEDED_TO_GOAL_STATE
         else:
-            msg.status = self.action_failure_msg
+            msg.status = plan_dispatch_msgs.ActionFeedback.ACTION_FAILED
 
         action_name_kvp = diag_msgs.KeyValue()
         action_name_kvp.key = 'action_name'