def __init__(self):
self.problem_generation_proxy = None
self.planner_proxy = None
self.plan_parsing_proxy = None
self.plan_dispatch_proxy = None
self.action_dispatch_topic = rospy.get_param('/planner/action_dispatch_topic',
'/kcl_rosplan/action_dispatch')
self.action_feedback_topic = rospy.get_param('/planner/action_feedback_topic',
'/kcl_rosplan/action_feedback')
self.kb_interface = DomesticKBInterface()
problem_generation_srv = rospy.get_param('/planner/problem_generation_srv',
'/rosplan_problem_interface/problem_generation_server')
try:
rospy.wait_for_service(problem_generation_srv, 5.)
self.problem_generation_proxy = rospy.ServiceProxy(problem_generation_srv, Empty)
except (rospy.ServiceException, rospy.ROSException):
rospy.logerr('The service %s does not appear to exist.', problem_generation_srv)
planner_srv = rospy.get_param('/planner/planner_srv',
'/rosplan_planner_interface/planning_server')
try:
rospy.wait_for_service(planner_srv, 5.)
self.planner_proxy = rospy.ServiceProxy(planner_srv, Empty)
except (rospy.ServiceException, rospy.ROSException):
rospy.logerr('The service %s does not appear to exist.', planner_srv)
plan_parsing_srv = rospy.get_param('/planner/plan_parsing_srv',
'/rosplan_parsing_interface/parse_plan')
try:
rospy.wait_for_service(plan_parsing_srv, 5.)
self.plan_parsing_proxy = rospy.ServiceProxy(plan_parsing_srv, Empty)
except (rospy.ServiceException, rospy.ROSException):
rospy.logerr('The service %s does not appear to exist.', plan_parsing_srv)
plan_dispatch_srv = rospy.get_param('/planner/plan_dispatch_srv',
'/rosplan_plan_dispatcher/dispatch_plan')
try:
rospy.wait_for_service(plan_dispatch_srv, 5.)
self.plan_dispatch_proxy = rospy.ServiceProxy(plan_dispatch_srv,
plan_dispatch_srvs.DispatchService)
except (rospy.ServiceException, rospy.ROSException):
rospy.logerr('The service %s does not appear to exist.', plan_dispatch_srv)
self.current_action = ''
self.executing = False
self.action_dispatch_sub = rospy.Subscriber(self.action_dispatch_topic,
plan_dispatch_msgs.ActionDispatch,
self.get_dispatched_action)
self.action_feedback_sub = rospy.Subscriber(self.action_feedback_topic,
plan_dispatch_msgs.ActionFeedback,
self.get_action_feedback)
def __init__(self):
self.action_success_msg = 'action achieved'
self.action_failure_msg = 'action failed'
# unique action ID
self.action_id = -1
# name of the robot on which the client is spawned
self.robot_name = None
# name of the action (converted to lowercase)
self.action_name = rospy.get_param('~action_name', '')
self.action_name = self.action_name.lower()
# name of the action server
self.action_server_name = rospy.get_param('~server_name', '')
# timeout for action calls
self.action_timeout = rospy.get_param('~action_timeout', 15.)
# knowledge base interface instance
self.kb_interface = DomesticKBInterface()
# subscriber for dispatched actions
rospy.Subscriber('action_dispatch_topic',
plan_dispatch_msgs.ActionDispatch, self.call_action)
# action feedback publisher
self.feedback_pub = rospy.Publisher('action_feedback_topic',
plan_dispatch_msgs.ActionFeedback,
queue_size=1)
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)
except Exception as exc:
rospy.logerr(
'[find_object] Could not create an ontology interface client: %s',
exc)
try:
rospy.loginfo(
'[find_object] Creating a knowledge base interface client')
self.kb_interface = DomesticKBInterface()
except Exception as exc:
rospy.logerr(
'[find_object] Could not create a knowledge base interface client: %s',
exc)
return FTSMTransitions.INITIALISED
def __init__(self,
action_name,
outcomes,
input_keys=list(),
output_keys=list(),
save_sm_state=False):
smach.State.__init__(self,
outcomes=outcomes,
input_keys=input_keys,
output_keys=output_keys)
self.sm_id = ''
self.state_name = ''
self.action_name = action_name
self.save_sm_state = save_sm_state
self.retry_count = 0
self.executing = False
self.succeeded = False
self.say_pub = rospy.Publisher('/say',
String,
latch=True,
queue_size=1)
self.ontology_url = rospy.get_param('/ontology_url', '')
self.ontology_class_prefix = rospy.get_param('/ontology_class_prefix',
'')
self.action_dispatch_pub = rospy.Publisher(
'/kcl_rosplan/action_dispatch',
plan_dispatch_msgs.ActionDispatch,
queue_size=1)
rospy.Subscriber('/kcl_rosplan/action_feedback',
plan_dispatch_msgs.ActionFeedback,
self.get_action_feedback)
self.kb_interface = DomesticKBInterface()
self.ontology_interface = DomesticOntologyInterface(
self.ontology_url, self.ontology_class_prefix)
self.robot_name = self.kb_interface.robot_name
class ScenarioStateBase(smach.State):
def __init__(self,
action_name,
outcomes,
input_keys=list(),
output_keys=list(),
save_sm_state=False):
smach.State.__init__(self,
outcomes=outcomes,
input_keys=input_keys,
output_keys=output_keys)
self.sm_id = ''
self.state_name = ''
self.action_name = action_name
self.save_sm_state = save_sm_state
self.retry_count = 0
self.executing = False
self.succeeded = False
self.say_pub = rospy.Publisher('/say',
String,
latch=True,
queue_size=1)
self.ontology_url = rospy.get_param('/ontology_url', '')
self.ontology_class_prefix = rospy.get_param('/ontology_class_prefix',
'')
self.action_dispatch_pub = rospy.Publisher(
'/kcl_rosplan/action_dispatch',
plan_dispatch_msgs.ActionDispatch,
queue_size=1)
rospy.Subscriber('/kcl_rosplan/action_feedback',
plan_dispatch_msgs.ActionFeedback,
self.get_action_feedback)
self.kb_interface = DomesticKBInterface()
self.ontology_interface = DomesticOntologyInterface(
self.ontology_url, self.ontology_class_prefix)
self.robot_name = self.kb_interface.robot_name
def execute(self, userdata):
pass
def save_current_state(self):
execution_state_msg = ExecutionState()
execution_state_msg.stamp = rospy.Time.now()
execution_state_msg.state_machine = self.sm_id
execution_state_msg.state = self.state_name
self.kb_interface.insert_obj_instance('current_state',
execution_state_msg)
def get_dispatch_msg(self):
pass
def get_action_feedback(self, msg):
if msg.information and msg.information[0].key == 'action_name' and \
msg.information[0].value == self.action_name:
self.executing = False
self.succeeded = msg.status == 'action achieved'
def say(self, sentence):
say_msg = String()
say_msg.data = sentence
self.say_pub.publish(say_msg)
class FindObjectSM(ActionSMBase):
def __init__(self,
ontology_url,
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_class_prefix = ontology_class_prefix
self.number_of_retries = number_of_retries
self.timeout = timeout
self.ontology_interface = None
self.kb_interface = None
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)
except Exception as exc:
rospy.logerr(
'[find_object] Could not create an ontology interface client: %s',
exc)
try:
rospy.loginfo(
'[find_object] Creating a knowledge base interface client')
self.kb_interface = DomesticKBInterface()
except Exception as exc:
rospy.logerr(
'[find_object] Could not create a knowledge base interface client: %s',
exc)
return FTSMTransitions.INITIALISED
def running(self):
obj_name = None
obj_location = None
relation = None
if self.goal.goal_type == FindObjectGoal.NAMED_OBJECT:
obj_name = self.goal.object_name
location, predicate = self.kb_interface.get_object_location(
obj_name)
if location:
rospy.loginfo('[find_object] Found %s %s %s', obj_name,
predicate, location)
# TODO: verify that the object is still at that location
obj_location = location
relation = predicate
self.result = self.set_result(True, obj_location, relation)
return FTSMTransitions.DONE
rospy.loginfo(
'[find_object] %s not found in the knowledge base; querying the ontology',
obj_name)
location = self.ontology_interface.get_default_storing_location(
obj_name=obj_name)
if location:
predicate = 'in'
rospy.loginfo('[find_object] %s is usually %s %s', obj_name,
predicate, location)
# TODO: check if the object is at the default location
obj_location = location
relation = predicate
self.result = self.set_result(True, obj_location, relation)
return FTSMTransitions.DONE
rospy.loginfo('[find_object] %s not found', obj_name)
obj_location = None
relation = None
self.result = self.set_result(True, obj_location, relation)
return FTSMTransitions.DONE
elif self.goal.goal_type == FindObjectGoal.OBJECT_CATEGORY:
obj_category = self.goal.object_name
category_objects = self.kb_interface.get_category_objects(
obj_category)
if category_objects:
for obj_name in category_objects:
location, predicate = self.kb_interface.get_object_location(
obj_name)
if location:
rospy.loginfo('[find_object] Found %s %s %s', obj_name,
predicate, location)
# TODO: verify that the object is still at that location
obj_location = location
relation = predicate
self.result = self.set_result(True, obj_location,
relation)
return FTSMTransitions.DONE
else:
rospy.loginfo(
'[find_object] The location of %s is unknown',
obj_name)
else:
rospy.loginfo(
'[find_object] No object of category %s was found in the knowledge base; querying the ontology',
obj_category)
location = self.ontology_interface.get_default_storing_location(
obj_category=obj_category)
if location:
predicate = 'in'
rospy.loginfo('[find_object] Objects of %s are usually %s %s',
obj_category, predicate, location)
# TODO: check if an object of the desired category is at the default location
obj_location = location
relation = predicate
self.result = self.set_result(True, obj_location, relation)
return FTSMTransitions.DONE
rospy.logerr(
'[find_object] Object of category %s could not be found',
obj_category)
self.result = self.set_result(False, obj_location, relation)
return FTSMTransitions.DONE
def set_result(self, success, obj_location, relation):
result = FindObjectResult()
result.success = success
result.object_location = obj_location
result.relation = relation
return result
class PlannerInterface(object):
'''An interface providing functionalities for interacting with a planner.
The interface expects four planner services to be exposed, whose names should be
made available to the parameter server as follows:
* /planner/problem_generation_srv: Name of a service that generates a planner problem
* /planner/planner_srv: Name of a service that invokes a planner
* /planner/plan_parsing_srv: Name of a service that parses a generated plan
* /planner/plan_dispatch_srv: Name of a service that invokes a plan dispatcher
@author Alex Mitrevski
@contact [email protected]
'''
def __init__(self):
self.problem_generation_proxy = None
self.planner_proxy = None
self.plan_parsing_proxy = None
self.plan_dispatch_proxy = None
self.action_dispatch_topic = rospy.get_param('/planner/action_dispatch_topic',
'/kcl_rosplan/action_dispatch')
self.action_feedback_topic = rospy.get_param('/planner/action_feedback_topic',
'/kcl_rosplan/action_feedback')
self.kb_interface = DomesticKBInterface()
problem_generation_srv = rospy.get_param('/planner/problem_generation_srv',
'/rosplan_problem_interface/problem_generation_server')
try:
rospy.wait_for_service(problem_generation_srv, 5.)
self.problem_generation_proxy = rospy.ServiceProxy(problem_generation_srv, Empty)
except (rospy.ServiceException, rospy.ROSException):
rospy.logerr('The service %s does not appear to exist.', problem_generation_srv)
planner_srv = rospy.get_param('/planner/planner_srv',
'/rosplan_planner_interface/planning_server')
try:
rospy.wait_for_service(planner_srv, 5.)
self.planner_proxy = rospy.ServiceProxy(planner_srv, Empty)
except (rospy.ServiceException, rospy.ROSException):
rospy.logerr('The service %s does not appear to exist.', planner_srv)
plan_parsing_srv = rospy.get_param('/planner/plan_parsing_srv',
'/rosplan_parsing_interface/parse_plan')
try:
rospy.wait_for_service(plan_parsing_srv, 5.)
self.plan_parsing_proxy = rospy.ServiceProxy(plan_parsing_srv, Empty)
except (rospy.ServiceException, rospy.ROSException):
rospy.logerr('The service %s does not appear to exist.', plan_parsing_srv)
plan_dispatch_srv = rospy.get_param('/planner/plan_dispatch_srv',
'/rosplan_plan_dispatcher/dispatch_plan')
try:
rospy.wait_for_service(plan_dispatch_srv, 5.)
self.plan_dispatch_proxy = rospy.ServiceProxy(plan_dispatch_srv,
plan_dispatch_srvs.DispatchService)
except (rospy.ServiceException, rospy.ROSException):
rospy.logerr('The service %s does not appear to exist.', plan_dispatch_srv)
self.current_action = ''
self.executing = False
self.action_dispatch_sub = rospy.Subscriber(self.action_dispatch_topic,
plan_dispatch_msgs.ActionDispatch,
self.get_dispatched_action)
self.action_feedback_sub = rospy.Subscriber(self.action_feedback_topic,
plan_dispatch_msgs.ActionFeedback,
self.get_action_feedback)
def add_plan_goals(self, goals):
'''Registers the ground predicates in the given list as planning goals.
Keyword arguments:
@param goal_list -- a list in which each entry is a tuple of the form
(predicate, [(variable, value), ...]), where
"predicate" is the predicate name and the
(variable, value) tuples are the variable values
'''
self.kb_interface.insert_goals(goals)
def remove_plan_goals(self, goals):
'''Removes the ground predicates in the given list from the list of planning goals.
Keyword arguments:
@param goal_list -- a list in which each entry is a tuple of the form
(predicate, [(variable, value), ...]), where
"predicate" is the predicate name and the
(variable, value) tuples are the variable values
'''
self.kb_interface.remove_goals(goals)
def plan(self):
'''Generates a problem file from the current state of the knowledge base
and invokes the planner.
'''
rospy.loginfo('[planner_interface] Generating a problem file')
try:
self.problem_generation_proxy()
except rospy.ServiceException as exc:
rospy.logerr('An error occured while generating a problem file: ' + str(exc))
return False
rospy.loginfo('[planner_interface] Invoking the planner')
try:
self.planner_proxy()
except rospy.ServiceException as exc:
rospy.logerr('An error occured while planning: ' + str(exc))
return False
return True
def start_plan_dispatch(self):
'''Parses a previously generated plan and starts dispatching the plan.
'''
rospy.loginfo('[planner_interface] Parsing the plan')
try:
self.plan_parsing_proxy()
except rospy.ServiceException as exc:
rospy.logerr('An error occured while parsing the plan: ' + str(exc))
return False
rospy.loginfo('[planner_interface] Invoking the plan dispatcher')
try:
self.plan_dispatch_proxy()
except rospy.ServiceException as exc:
rospy.logerr('An error occured while dispatching the plan: ' + str(exc))
return False
self.executing = True
return True
def get_current_action(self):
return self.current_action
def get_dispatched_action(self, msg):
self.current_action = msg.name
def get_action_feedback(self, msg):
if msg.information and msg.information[0].key == 'action_name' and \
msg.information[0].value == self.current_action:
self.executing = msg.status != 'action achieved'