def _handle_get_fluent_value(self, fluent):
"""
:fluent: task_planner/Fluent
:returns: QueryKBResponse
"""
fluent_value = self.kb_interface.get_fluent_value(
MessageConverter.fluent_ros_to_tuple(fluent))
fluent_value_ros = MessageConverter.fluent_value_obj_to_ros(
fluent_value)
return QueryKBResponse(fluent_value=fluent_value_ros)
def _handle_get_fluent_assertions(self):
"""
:returns: QueryKBResponse
"""
fluent_assertions = [MessageConverter.fluent_obj_to_ros(fluent) \
for fluent in self.kb_interface.get_fluent_assertions()]
return QueryKBResponse(fluent_assertions=fluent_assertions)
def _handle_facts_to_remove(self, facts):
"""Remove all predicates from kb
:facts: list of TaskPlannerPredicate
:returns: bool
"""
if not facts:
return True
return self.kb_interface.remove_facts(
[MessageConverter.predicate_ros_to_tuple(fact) for fact in facts])
def _handle_facts_to_add(self, facts):
"""Add all predicates to kb
:facts: list of TaskPlannerPredicate
:returns: bool
"""
if not facts:
return True
return self.kb_interface.insert_facts(
[MessageConverter.predicate_ros_to_tuple(fact) for fact in facts])
def _handle_goals_to_remove(self, goals):
"""Remove all goals from kb
:goals: list of TaskPlannerPredicate
:returns: bool
"""
if not goals:
return True
return self.kb_interface.remove_goals(
[MessageConverter.predicate_ros_to_tuple(goal) for goal in goals])
def _handle_goals_to_add(self, goals):
"""Add all goals to kb
:goals: list of TaskPlannerPredicate
:returns: bool
"""
if not goals:
return True
return self.kb_interface.insert_goals(
[MessageConverter.predicate_ros_to_tuple(goal) for goal in goals])
def _handle_fluents_to_add(self, fluents):
"""Add all fluents to kb
:fluents: list of TaskPlannerFluent
:returns: bool
"""
if not fluents:
return True
return self.kb_interface.insert_fluents([
MessageConverter.fluent_ros_to_tuple(fluent) for fluent in fluents
])
def _handle_get_predicate_assertions(self, predicate_name):
"""
:predicate_name: string
:returns: QueryKBResponse
"""
if predicate_name == '':
predicate_name = None
predicate_assertions = [MessageConverter.predicate_obj_to_ros(predicate) \
for predicate in self.kb_interface.get_predicate_assertions(predicate_name)]
return QueryKBResponse(predicate_assertions=predicate_assertions)
def _handle_fluents_to_remove(self, fluents):
"""Remove all fluents from kb
:fluents: list of TaskPlannerFluent
:returns: bool
"""
if not fluents:
return True
success = self.kb_interface.remove_fluents([
MessageConverter.fluent_ros_to_tuple(fluent) for fluent in fluents
])
return success
def get_safe_result(self, req):
"""Handle plan action call within try block.
:req: PlanGoal
:returns: PlanResult
"""
plan_found = False
actions = []
try:
plan_found, action_objs = self.planner.plan(
MessageConverter.task_request_ros_to_obj(req.task_request),
req.robot_name, [
MessageConverter.predicate_ros_to_obj(goal)
for goal in req.task_goals
])
actions = [
MessageConverter.action_obj_to_ros(action)
for action in action_objs
]
except Exception as e:
rospy.logerr(str(e))
return PlanResult(plan_found=plan_found, actions=actions)