# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
import rospy
from hri_api.entities import Person, World
from hri_api.query import Query
from nao_hri import Nao, Gesture
world = World()
robot = Nao()
people = Query(world).select_type(Person)
closest_person = people.sort_decreasing(lambda p: p.distance_to(robot)).take(1)
rate = rospy.Rate(2)
while not rospy.is_shutdown():
result = closest_person.execute()
if len(result) > 0:
break
rate.sleep()
ah0 = robot.gesture(Gesture.PointRarm, target=result[0].head)
ah1 = robot.say_to('Hello I can see you!', closest_person)
ah2 = robot.gesture(Gesture.WaveLarm)
robot.wait(ah1, ah2, ah0)
