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)
def execute_cb(self, goal):
res = FinishRouteDescriptionResult()
res.result.append(
ActionResult(cond="described_route__%s__%s" %
(goal.shop_id, goal.waypoint),
truth_value=False))
res.result.append(
ActionResult(cond="finished_description__%s__%s" %
(goal.shop_id, goal.waypoint),
truth_value=True))
self._as.set_succeeded(res)
def execute_cb(self, goal):
self.stop_client.send_goal(TrackPersonGoal())
if goal.to != "none":
pose, _ = self.load({"waypoint_name": goal.to})
rospy.loginfo("Going from '%s' to '%s'" % (goal.from_, goal.to))
self.client.send_goal_and_wait(MoveBaseGoal(target_pose=pose))
res = MoveToWaypointResult()
res.result.append(ActionResult(cond="robot_at_waypoint__"+goal.to, truth_value=True))
res.result.append(ActionResult(cond="robot_at_waypoint__"+goal.from_, truth_value=False))
res.result.append(ActionResult(cond="robot_pose_unknown", truth_value=True))
self._as.set_succeeded(res)
def stop_cb(self, goal):
self.stop()
self._call_service("/naoqi_driver/tracker/stop_tracker", Empty,
EmptyRequest())
self._call_service("/naoqi_driver/tracker/unregister_all_targets",
Empty, EmptyRequest())
self.pub.publish(Twist())
res = TrackPersonResult()
res.result.append(
ActionResult(cond="tracking__" + goal.id, truth_value=False))
res.result.append(ActionResult(cond="no_tracking", truth_value=True))
self._ps.set_succeeded(res)
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)
def execute_cb(self, goal):
self.start_client.send_goal(TrackPersonGoal(id=goal.id, no_turn=True))
try:
msg = rospy.wait_for_message("/naoqi_driver_node/people_detected",
PersonDetectedArray,
timeout=5.)
except rospy.ROSException as e:
rospy.logwarn(e)
self._ps.set_aborted()
else:
int_id = int(goal.id.split("_")[1])
for p in msg.person_array:
if p.id == int_id:
try:
t = self.listener.getLatestCommonTime(
self.target_frame, msg.header.frame_id)
p_pose = PoseStamped(header=msg.header,
pose=p.person.position)
p_pose.header.stamp = t
bl_pose = self.listener.transformPose(
self.target_frame, p_pose)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException) as ex:
rospy.logwarn(ex)
else:
target_dist = p.person.distance - self.distance[
goal.to]
d = target_dist / p.person.distance
theta = math.atan2(bl_pose.pose.position.y,
bl_pose.pose.position.x)
target_pose = bl_pose
target_pose.pose.position.x *= d
target_pose.pose.position.y *= d
target_pose.pose.orientation.x = 0.
target_pose.pose.orientation.y = 0.
target_pose.pose.orientation.z = math.sin(theta / 2.)
target_pose.pose.orientation.w = math.cos(theta / 2.)
print target_pose
self.move_client.send_goal_and_wait(
MoveBaseGoal(target_pose=target_pose))
finally:
break
res = QualitativeMovePepperResult()
res.result.append(
ActionResult(cond="robot_at_home", truth_value=False))
res.result.append(
ActionResult(cond="robot_pose_unknown", truth_value=True))
self._ps.set_succeeded(res)
def start_cb(self, goal):
print goal
self.id = goal.id.split('_')[1]
if not goal.no_turn:
self.start()
req = SetTrackerTargetRequest()
req.target = req.PEOPLE
req.values.append(float(self.id))
self._call_service("/naoqi_driver/tracker/register_target",
SetTrackerTarget, req)
self._call_service("/naoqi_driver/tracker/set_mode", SetTrackerMode,
SetTrackerModeRequest(SetTrackerModeRequest.HEAD))
self._call_service("/naoqi_driver/tracker/track", StartTracker,
StartTrackerRequest(StartTrackerRequest.PEOPLE))
res = TrackPersonResult()
res.result.append(
ActionResult(cond="tracking__" + goal.id, truth_value=True))
res.result.append(ActionResult(cond="no_tracking", truth_value=False))
self._ps.set_succeeded(res)
def execute_cb(self, goal):
# self.pub.publish(self.texts[goal.text])
# rospy.sleep(3)
self.start_client.send_goal(TrackPersonGoal(id=goal.id))
self.client.send_goal(dialogueGoal(userID=int(goal.id.split("_")[1])))
res = SpeakResult()
res.result.append(
ActionResult(cond=self.predicate + "__" + goal.id + "__" +
goal.text,
truth_value=ActionResult.TRUE))
self._ps.set_succeeded(res)
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)
def execute_cb(self, goal):
self.stop_client.send_goal(TrackPersonGoal())
if self.__call_service("/naoqi_driver/localization/is_data_available",
LocalizationCheck,
LocalizationCheckRequest()).result:
while not rospy.is_shutdown(
) and not self._ps.is_preempt_requested():
rospy.loginfo("Robot returning home")
res = self.__call_service(
"/naoqi_driver/localization/go_to_home",
LocalizationTrigger, LocalizationTriggerRequest())
if res.result != 0:
rospy.logwarn(self.__get_error_message(res.result))
break
else:
rospy.logwarn("Cannot go home. Pepper has never been localised.")
res = GoHomeResult()
res.result.append(ActionResult(cond="robot_at_home", truth_value=True))
res.result.append(
ActionResult(cond="robot_pose_unknown", truth_value=False))
self._ps.set_succeeded(res)
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)
def start_cb(self, goal):
if goal.id != "":
self.id = goal.id.split('_')[1]
else:
self.id = rospy.wait_for_message(
"/naoqi_driver_node/people_detected",
PersonDetectedArray).person_array[0].id
if not goal.no_turn:
self.start()
req = SetTrackerTargetRequest()
req.target = req.PEOPLE
req.values.append(float(self.id))
self._call_service("/naoqi_driver/tracker/register_target",
SetTrackerTarget, req)
self._call_service("/naoqi_driver/tracker/set_mode", SetTrackerMode,
SetTrackerModeRequest(SetTrackerModeRequest.HEAD))
self._call_service("/naoqi_driver/tracker/track", StartTracker,
StartTrackerRequest(StartTrackerRequest.PEOPLE))
res = TrackPersonResult()
res.result.append(
ActionResult(cond="tracking__" + goal.id, truth_value=True))
res.result.append(ActionResult(cond="no_tracking", truth_value=False))
self._ps.set_succeeded(res)
def execute_cb(self, goal):
self.stop_client.send_goal(TrackPersonGoal())
shop_id = goal.shop_id.split('_')[1]
result = self.db[self.collection_name].find_one({
"shop_id":
shop_id,
"semantic_map_name":
self.semantic_map_name
})
rospy.loginfo("Waiting for current pose from odometry.")
o = rospy.wait_for_message("/naoqi_driver_node/odom", Odometry)
current_pose = PoseStamped()
current_pose.header = o.header
current_pose.pose = o.pose.pose
rospy.loginfo("Received pose.")
loc = PoseStamped()
loc.header.frame_id = "semantic_map"
loc.pose.position.x = float(result["loc_x"])
loc.pose.position.y = float(result["loc_y"])
target = self.transform_pose(DescribeRoute.BASE_LINK, loc)
t = TrackerPointAtRequest()
t.effector = t.EFFECTOR_RARM
t.frame = t.FRAME_TORSO
t.max_speed_fraction = 0.5
t.target.x = target.pose.position.x
t.target.y = target.pose.position.y
s1 = ServiceThread("/naoqi_driver/tracker/point_at", TrackerPointAt, t)
self.set_breathing(False)
s1.start()
s1.join(timeout=1.0)
target = self.transform_pose(DescribeRoute.BASE_LINK, loc)
t = TrackerLookAtRequest()
t.use_whole_body = False
t.frame = t.FRAME_TORSO
t.max_speed_fraction = 0.6
t.target.x = target.pose.position.x
t.target.y = target.pose.position.y
s2 = ServiceThread("/naoqi_driver/tracker/look_at", TrackerLookAt, t)
s2.start()
s1.join()
s2.join()
self.pub.publish(Twist())
current_pose = self.transform_pose(DescribeRoute.BASE_LINK,
current_pose)
current_pose.pose.position.x = 0.
current_pose.pose.position.y = 0.
current_pose.pose.position.z = 0.
self.pub.publish(Twist())
self.__call_service(
"/naoqi_driver/tts/say", Say,
SayRequest(result["directions"] % result["shopName"]))
self.set_breathing(True)
g = MoveBaseGoal()
g.target_pose = current_pose
self.move_client.send_goal_and_wait(g)
self.stand()
t = TrackPersonGoal()
t.id = "id_%s" % rospy.wait_for_message(
"/naoqi_driver_node/people_detected",
PersonDetectedArray).person_array[0].id
self.start_client.send_goal(t)
res = RouteDescriptionResult()
res.result.append(
ActionResult(cond="described_route__%s__%s" %
(goal.shop_id, goal.waypoint),
truth_value=True))
res.result.append(
ActionResult(cond="finished_description__%s__%s" %
(goal.shop_id, goal.waypoint),
truth_value=False))
self._as.set_succeeded(res)