Look with PR2 head to closest legs

look_legs.py

#!/usr/bin/env python

import rospy
from people_msgs.msg import PositionMeasurementArray, PositionMeasurement

from pr2_controllers_msgs.msg import PointHeadActionGoal
import tf.transformations
from geometry_msgs.msg import PointStamped

"""
Node that looks at the closest legs.

Using http://wiki.ros.org/leg_detector as leg detector with a PR2.

Authors: Sammy Pfeiffer, Siddharth Agrawal 
"""

class LookLegs(object):
    def __init__(self):
        rospy.loginfo("Initalizing looklegs")
        self.tf_l = tf.TransformListener()
        self.head_pub = rospy.Publisher('/head_traj_controller/point_head_action/goal',
                                        PointHeadActionGoal, queue_size=1)
        self.last_legs = None
        self.legs_sub = rospy.Subscriber('/leg_tracker_measurements',
                                         PositionMeasurementArray,
                                         self.legs_cb,
                                         queue_size=1)
        self.point_pub = rospy.Publisher(
            '/legs_looking_at', PointStamped, queue_size=1)

        rospy.loginfo("initialized")
        rospy.sleep(3.0)

    def legs_cb(self, data):
        self.last_legs = data

    def transform_point(self, pointstamped, from_frame, to_frame):
        ps = PointStamped()
        # ps.header.stamp = #self.tf_l.getLatestCommonTime(from_frame,
        # to_frame)
        ps.header.frame_id = from_frame
        ps.point = pointstamped.point
        transform_ok = False
        while not transform_ok and not rospy.is_shutdown():
            try:
                target_ps = self.tf_l.transformPoint(to_frame, ps)
                transform_ok = True
            except tf.ExtrapolationException as e:
                rospy.logwarn(
                    "Exception on transforming point... trying again \n(" + str(e) + ")")
                rospy.sleep(0.2)
                ps.header.stamp = self.tf_l.getLatestCommonTime(
                    from_frame, to_frame)

        return target_ps

    def look_leg(self, x, y, z, frame='odom_combined'):
        # send goals to head from last pose to look at it
        phag = PointHeadActionGoal()
        phag.goal.pointing_axis.z = 1.0
        phag.goal.max_velocity = 1.0
        phag.goal.min_duration = rospy.Duration(0.15)
        phag.goal.pointing_frame = 'wide_stereo_optical_frame'
        phag.goal.target.header.frame_id = frame
        phag.goal.target.point.x = x
        phag.goal.target.point.y = y
        phag.goal.target.point.z = z
        self.head_pub.publish(phag)
        self.point_pub.publish(phag.goal.target)

    def do_work(self):
        closest = None
        for p in self.last_legs.people:
            ps = PointStamped()
            ps.header.frame_id = p.header.frame_id
            ps.point = p.pos

            ps_base_link = self.transform_point(
                ps, p.header.frame_id, 'base_link')
            rospy.loginfo("Transformed point:\n" + str(ps_base_link))
            if ps_base_link.point.x > 0.4 and ps_base_link.point.y < 0.9:
                if closest is None:
                    closest = ps_base_link
                else:
                    rospy.loginfo("people is further than 0.4")
                    if ps_base_link.point.x < closest.point.x:
                        closest = ps_base_link

        if closest is not None:
            self.look_leg(closest.point.x, closest.point.y,
                          closest.point.z, frame=closest.header.frame_id)

    def run(self):
        r = rospy.Rate(1)
        while not rospy.is_shutdown():
            if self.last_legs is not None:
                self.do_work()
            r.sleep()


if __name__ == '__main__':
    rospy.init_node('looklegs')
    ll = LookLegs()
    ll.run()