class FindInteractant(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=FindInteractantAction,
execute_cb=self.execute_cb,
auto_start=False
)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person", TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Finding interactant '%s'" % (goal.interactant_id,))
self.start_client.send_goal(TrackPersonGoal(id=goal.id, interactant_id=goal.interactant_id, no_turn=True))
res = FindInteractantResult()
res.result.append(ActionResult(cond="found_interactant__"+goal.interactant_id+"__"+goal.id, truth_value=True))
res.result.append(ActionResult(cond="free_interactant_id__"+goal.interactant_id, truth_value=False))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation',
GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value))
result = self.client.get_state()
if result == GoalStatus.PREEMPTED:
return
if result == GoalStatus.SUCCEEDED:
self._ps.set_succeeded()
else:
self._ps.set_aborted()
def preempt_cb(self):
self.client.cancel_all_goals()
self._ps.set_preempted()
class TerminateInteraction(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=TerminateInteractionAction,
execute_cb=self.execute_cb,
auto_start=False
)
rospy.loginfo("Creating tracker client")
self.stop_client = SimpleActionClient("/stop_tracking_person", TrackPersonAction)
self.stop_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Disengaging human '%s'" % (goal.id,))
self.stop_client.send_goal(TrackPersonGoal())
res = TerminateInteractionResult()
res.result.append(ActionResult(cond="engaged__"+goal.interactant_id+"__"+goal.text, truth_value=False))
res.result.append(ActionResult(cond="said__"+goal.id+"__"+goal.text, truth_value=False))
res.result.append(ActionResult(cond="found_interactant__"+goal.interactant_id+"__"+goal.id, truth_value=False))
res.result.append(ActionResult(cond="human_exists__"+goal.id, truth_value=False))
res.result.append(ActionResult(cond="free_interactant_id__"+goal.interactant_id, truth_value=True))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation', GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value))
result = self.client.get_state()
if result == GoalStatus.PREEMPTED:
return
if result == GoalStatus.SUCCEEDED:
self._ps.set_succeeded()
else:
self._ps.set_aborted()
def preempt_cb(self):
self.client.cancel_all_goals()
self._ps.set_preempted()
class CheckHumanExistance(PNPSimplePluginServer):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.predicate = rospy.get_param("~predicate", "human_exists")
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=CheckHumanExistsAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("checking existance of '%s'" % (goal.id, ))
res = CheckHumanExistsResult()
try:
msg = rospy.wait_for_message("/naoqi_driver_node/people_detected",
PersonDetectedArray,
timeout=5.)
except rospy.ROSException as e:
rospy.logwarn(e)
res.result.append(
ActionResult(cond=self.predicate + "__" + goal.id,
truth_value=ActionResult.FALSE))
res.result.append(
ActionResult(cond="found_interactant__" + goal.interactant_id +
"__" + goal.id,
truth_value=True))
res.result.append(
ActionResult(cond="free_interactant_id__" +
goal.interactant_id,
truth_value=False))
else:
found = ActionResult.FALSE
int_id = int(goal.id.split("_")[1])
for p in msg.person_array:
if p.id == int_id:
found = ActionResult.TRUE
break
res.result.append(
ActionResult(cond=self.predicate + "__" + goal.id,
truth_value=found))
finally:
if self._ps.is_preempt_requested():
self._ps.set_preempted()
elif res.result[-1].truth_value:
self._ps.set_succeeded(res)
else:
print "##### ABORTED #####"
self._ps.set_aborted(res)
class EngageHuman(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=EngageHumanAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Engaging human '%s'" % (goal.id, ))
res = EngageHumanResult()
# Has to be done in the end of the dialogue so the controller is blocked till it is over.
res.result.append(
ActionResult(cond="engaged__" + goal.interactant_id + "__" +
goal.text,
truth_value=True))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)
class FindInteractant(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=FindInteractantAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Finding interactant '%s'" % (goal.interactant_id, ))
res = FindInteractantResult()
res.result.append(
ActionResult(cond="found_interactant__" + goal.interactant_id +
"__" + goal.id,
truth_value=True))
res.result.append(
ActionResult(cond="free_interactant_id__" + goal.interactant_id,
truth_value=False))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)