diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index f6cd137f8..11aeb5e64 100644 --- a/.github/CODEOWNERS +++ b/.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 diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch index 9a02224b8..d7ecdff78 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch @@ -1,12 +1,16 @@ - + + + + + diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action index 048aa6ad1..b3880e13c 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action +++ b/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) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py index d4a67adbb..cc170c9bf 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py +++ b/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) diff --git a/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl b/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl index 5b5a3a549..41a07ffc8 100644 --- a/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl +++ b/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) ) ) ) diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch index ce1d5c695..b7e984653 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch @@ -2,15 +2,17 @@ - + - - + + + + diff --git a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py index 4f49656a4..9222aced5 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py +++ b/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'