#!/usr/bin/python

from k2_client.msg._BodyArray import BodyArray
from k2_client.msg._JointPositionAndState import JointPositionAndState
from tinker_vision_msgs.msg._TrackPeople import TrackPeople
from tinker_vision_msgs.msg._TrackPerson import TrackPerson
from sensor_msgs.msg import Image
from geometry_msgs.msg import Point
import rospy
import numpy
import cv2
import sys
from cv_bridge import CvBridge, CvBridgeError
from threading import Lock
from numpy import array

def average_position(body):
    sum_x = 0.
    sum_y = 0.
    sum_z = 0.
    for joint in body.jointPositions:
        sum_x += joint.position.x
        sum_y += joint.position.y
        sum_z += joint.position.z
    point = Point(
            sum_x / len(body.jointPositions),
            sum_y / len(body.jointPositions),
            sum_z / len(body.jointPositions)
            )
    

class PersonInfoNode:
    def __init__(self):
        #self._people_pub = rospy.Publisher('track_people', TrackPeople, queue_size=10)
        self._image_pub = rospy.Publisher('~/people_track_img', Image, queue_size=10)
        self._new_body = False
        self._new_image = False
        self._body_lock = Lock()
        self._img_lock = Lock()
        self.bridge = CvBridge()
        self.bodies = None
        self.img = None


    def body_handler(self, body_array):
        with self._body_lock:
            self.bodies = body_array.bodies
            self._new_body = True

    def image_handler(self, data):
        with self._img_lock:
            try:
                self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
            except CvBridgeError as e:
                rospy.logwarn(e)
            self._new_image = True

    def pub_msgs(self):
        with self._body_lock:
            with self._img_lock:
                if not self._new_body or not self._new_image:
                    return
                self._new_body = False
                self._new_image = False
                track_people = TrackPeople()
                show_img = array(self.img, copy=True)
                rows, cols, _ = self.img.shape
                for body in self.bodies:
                    person = TrackPerson()
                    person.trackingId = body.trackingId
                    person.pos = average_position(body)
                    person.header = body.header
                    rospy.loginfo("Bounding box")
                    from_x = max(min(body.fromX, body.toX), 0)
                    to_x = min(max(body.fromX, body.toX), cols)
                    from_y = max(min(body.fromY, body.toY), 0)
                    to_y = min(max(body.fromY, body.toY), rows)
                    print(from_x, from_y)
                    print(to_x, to_y)
                    person.image = self.bridge.cv2_to_imgmsg(
                            self.img[from_y:to_y, from_x:to_x], 'bgr8')
                    track_people.people.append(person)
                    cv2.rectangle(show_img, 
                            (body.fromX, body.fromY), 
                            (body.toX, body.toY),
                            (255, 0, 0), 3)
                self._image_pub.publish(
                        self.bridge.cv2_to_imgmsg(show_img, 'bgr8'))
                #self._people_pub.publish(track_people)


def main(argv):
    person_info_node = PersonInfoNode()
    rospy.Subscriber('/head/kinect2/bodyArray', BodyArray, person_info_node.body_handler)
    rospy.Subscriber('/head/kinect2/rgb/image_color', Image, person_info_node.image_handler)
    rate = rospy.Rate(5)
    while not rospy.is_shutdown():
        person_info_node.pub_msgs()
        rate.sleep()

if __name__ == '__main__':
    rospy.init_node('generate_person_info', anonymous=False)
    main(sys.argv)

a4132cffb69d1d3a07dd0faa5bb457192b1a4272
<mask token>


class PersonInfoNode:

    def __init__(self):
        self._image_pub = rospy.Publisher('~/people_track_img', Image,
            queue_size=10)
        self._new_body = False
        self._new_image = False
        self._body_lock = Lock()
        self._img_lock = Lock()
        self.bridge = CvBridge()
        self.bodies = None
        self.img = None
    <mask token>
    <mask token>
    <mask token>


<mask token>

<mask token>


class PersonInfoNode:

    def __init__(self):
        self._image_pub = rospy.Publisher('~/people_track_img', Image,
            queue_size=10)
        self._new_body = False
        self._new_image = False
        self._body_lock = Lock()
        self._img_lock = Lock()
        self.bridge = CvBridge()
        self.bodies = None
        self.img = None

    def body_handler(self, body_array):
        with self._body_lock:
            self.bodies = body_array.bodies
            self._new_body = True

    def image_handler(self, data):
        with self._img_lock:
            try:
                self.img = self.bridge.imgmsg_to_cv2(data, 'bgr8')
            except CvBridgeError as e:
                rospy.logwarn(e)
            self._new_image = True

    def pub_msgs(self):
        with self._body_lock:
            with self._img_lock:
                if not self._new_body or not self._new_image:
                    return
                self._new_body = False
                self._new_image = False
                track_people = TrackPeople()
                show_img = array(self.img, copy=True)
                rows, cols, _ = self.img.shape
                for body in self.bodies:
                    person = TrackPerson()
                    person.trackingId = body.trackingId
                    person.pos = average_position(body)
                    person.header = body.header
                    rospy.loginfo('Bounding box')
                    from_x = max(min(body.fromX, body.toX), 0)
                    to_x = min(max(body.fromX, body.toX), cols)
                    from_y = max(min(body.fromY, body.toY), 0)
                    to_y = min(max(body.fromY, body.toY), rows)
                    print(from_x, from_y)
                    print(to_x, to_y)
                    person.image = self.bridge.cv2_to_imgmsg(self.img[
                        from_y:to_y, from_x:to_x], 'bgr8')
                    track_people.people.append(person)
                    cv2.rectangle(show_img, (body.fromX, body.fromY), (body
                        .toX, body.toY), (255, 0, 0), 3)
                self._image_pub.publish(self.bridge.cv2_to_imgmsg(show_img,
                    'bgr8'))


<mask token>

<mask token>


def average_position(body):
    sum_x = 0.0
    sum_y = 0.0
    sum_z = 0.0
    for joint in body.jointPositions:
        sum_x += joint.position.x
        sum_y += joint.position.y
        sum_z += joint.position.z
    point = Point(sum_x / len(body.jointPositions), sum_y / len(body.
        jointPositions), sum_z / len(body.jointPositions))


class PersonInfoNode:

    def __init__(self):
        self._image_pub = rospy.Publisher('~/people_track_img', Image,
            queue_size=10)
        self._new_body = False
        self._new_image = False
        self._body_lock = Lock()
        self._img_lock = Lock()
        self.bridge = CvBridge()
        self.bodies = None
        self.img = None

    def body_handler(self, body_array):
        with self._body_lock:
            self.bodies = body_array.bodies
            self._new_body = True

    def image_handler(self, data):
        with self._img_lock:
            try:
                self.img = self.bridge.imgmsg_to_cv2(data, 'bgr8')
            except CvBridgeError as e:
                rospy.logwarn(e)
            self._new_image = True

    def pub_msgs(self):
        with self._body_lock:
            with self._img_lock:
                if not self._new_body or not self._new_image:
                    return
                self._new_body = False
                self._new_image = False
                track_people = TrackPeople()
                show_img = array(self.img, copy=True)
                rows, cols, _ = self.img.shape
                for body in self.bodies:
                    person = TrackPerson()
                    person.trackingId = body.trackingId
                    person.pos = average_position(body)
                    person.header = body.header
                    rospy.loginfo('Bounding box')
                    from_x = max(min(body.fromX, body.toX), 0)
                    to_x = min(max(body.fromX, body.toX), cols)
                    from_y = max(min(body.fromY, body.toY), 0)
                    to_y = min(max(body.fromY, body.toY), rows)
                    print(from_x, from_y)
                    print(to_x, to_y)
                    person.image = self.bridge.cv2_to_imgmsg(self.img[
                        from_y:to_y, from_x:to_x], 'bgr8')
                    track_people.people.append(person)
                    cv2.rectangle(show_img, (body.fromX, body.fromY), (body
                        .toX, body.toY), (255, 0, 0), 3)
                self._image_pub.publish(self.bridge.cv2_to_imgmsg(show_img,
                    'bgr8'))


def main(argv):
    person_info_node = PersonInfoNode()
    rospy.Subscriber('/head/kinect2/bodyArray', BodyArray, person_info_node
        .body_handler)
    rospy.Subscriber('/head/kinect2/rgb/image_color', Image,
        person_info_node.image_handler)
    rate = rospy.Rate(5)
    while not rospy.is_shutdown():
        person_info_node.pub_msgs()
        rate.sleep()


<mask token>

<mask token>


def average_position(body):
    sum_x = 0.0
    sum_y = 0.0
    sum_z = 0.0
    for joint in body.jointPositions:
        sum_x += joint.position.x
        sum_y += joint.position.y
        sum_z += joint.position.z
    point = Point(sum_x / len(body.jointPositions), sum_y / len(body.
        jointPositions), sum_z / len(body.jointPositions))


class PersonInfoNode:

    def __init__(self):
        self._image_pub = rospy.Publisher('~/people_track_img', Image,
            queue_size=10)
        self._new_body = False
        self._new_image = False
        self._body_lock = Lock()
        self._img_lock = Lock()
        self.bridge = CvBridge()
        self.bodies = None
        self.img = None

    def body_handler(self, body_array):
        with self._body_lock:
            self.bodies = body_array.bodies
            self._new_body = True

    def image_handler(self, data):
        with self._img_lock:
            try:
                self.img = self.bridge.imgmsg_to_cv2(data, 'bgr8')
            except CvBridgeError as e:
                rospy.logwarn(e)
            self._new_image = True

    def pub_msgs(self):
        with self._body_lock:
            with self._img_lock:
                if not self._new_body or not self._new_image:
                    return
                self._new_body = False
                self._new_image = False
                track_people = TrackPeople()
                show_img = array(self.img, copy=True)
                rows, cols, _ = self.img.shape
                for body in self.bodies:
                    person = TrackPerson()
                    person.trackingId = body.trackingId
                    person.pos = average_position(body)
                    person.header = body.header
                    rospy.loginfo('Bounding box')
                    from_x = max(min(body.fromX, body.toX), 0)
                    to_x = min(max(body.fromX, body.toX), cols)
                    from_y = max(min(body.fromY, body.toY), 0)
                    to_y = min(max(body.fromY, body.toY), rows)
                    print(from_x, from_y)
                    print(to_x, to_y)
                    person.image = self.bridge.cv2_to_imgmsg(self.img[
                        from_y:to_y, from_x:to_x], 'bgr8')
                    track_people.people.append(person)
                    cv2.rectangle(show_img, (body.fromX, body.fromY), (body
                        .toX, body.toY), (255, 0, 0), 3)
                self._image_pub.publish(self.bridge.cv2_to_imgmsg(show_img,
                    'bgr8'))


def main(argv):
    person_info_node = PersonInfoNode()
    rospy.Subscriber('/head/kinect2/bodyArray', BodyArray, person_info_node
        .body_handler)
    rospy.Subscriber('/head/kinect2/rgb/image_color', Image,
        person_info_node.image_handler)
    rate = rospy.Rate(5)
    while not rospy.is_shutdown():
        person_info_node.pub_msgs()
        rate.sleep()


if __name__ == '__main__':
    rospy.init_node('generate_person_info', anonymous=False)
    main(sys.argv)

#!/usr/bin/python

from k2_client.msg._BodyArray import BodyArray
from k2_client.msg._JointPositionAndState import JointPositionAndState
from tinker_vision_msgs.msg._TrackPeople import TrackPeople
from tinker_vision_msgs.msg._TrackPerson import TrackPerson
from sensor_msgs.msg import Image
from geometry_msgs.msg import Point
import rospy
import numpy
import cv2
import sys
from cv_bridge import CvBridge, CvBridgeError
from threading import Lock
from numpy import array

def average_position(body):
    sum_x = 0.
    sum_y = 0.
    sum_z = 0.
    for joint in body.jointPositions:
        sum_x += joint.position.x
        sum_y += joint.position.y
        sum_z += joint.position.z
    point = Point(
            sum_x / len(body.jointPositions),
            sum_y / len(body.jointPositions),
            sum_z / len(body.jointPositions)
            )
    

class PersonInfoNode:
    def __init__(self):
        #self._people_pub = rospy.Publisher('track_people', TrackPeople, queue_size=10)
        self._image_pub = rospy.Publisher('~/people_track_img', Image, queue_size=10)
        self._new_body = False
        self._new_image = False
        self._body_lock = Lock()
        self._img_lock = Lock()
        self.bridge = CvBridge()
        self.bodies = None
        self.img = None


    def body_handler(self, body_array):
        with self._body_lock:
            self.bodies = body_array.bodies
            self._new_body = True

    def image_handler(self, data):
        with self._img_lock:
            try:
                self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
            except CvBridgeError as e:
                rospy.logwarn(e)
            self._new_image = True

    def pub_msgs(self):
        with self._body_lock:
            with self._img_lock:
                if not self._new_body or not self._new_image:
                    return
                self._new_body = False
                self._new_image = False
                track_people = TrackPeople()
                show_img = array(self.img, copy=True)
                rows, cols, _ = self.img.shape
                for body in self.bodies:
                    person = TrackPerson()
                    person.trackingId = body.trackingId
                    person.pos = average_position(body)
                    person.header = body.header
                    rospy.loginfo("Bounding box")
                    from_x = max(min(body.fromX, body.toX), 0)
                    to_x = min(max(body.fromX, body.toX), cols)
                    from_y = max(min(body.fromY, body.toY), 0)
                    to_y = min(max(body.fromY, body.toY), rows)
                    print(from_x, from_y)
                    print(to_x, to_y)
                    person.image = self.bridge.cv2_to_imgmsg(
                            self.img[from_y:to_y, from_x:to_x], 'bgr8')
                    track_people.people.append(person)
                    cv2.rectangle(show_img, 
                            (body.fromX, body.fromY), 
                            (body.toX, body.toY),
                            (255, 0, 0), 3)
                self._image_pub.publish(
                        self.bridge.cv2_to_imgmsg(show_img, 'bgr8'))
                #self._people_pub.publish(track_people)


def main(argv):
    person_info_node = PersonInfoNode()
    rospy.Subscriber('/head/kinect2/bodyArray', BodyArray, person_info_node.body_handler)
    rospy.Subscriber('/head/kinect2/rgb/image_color', Image, person_info_node.image_handler)
    rate = rospy.Rate(5)
    while not rospy.is_shutdown():
        person_info_node.pub_msgs()
        rate.sleep()

if __name__ == '__main__':
    rospy.init_node('generate_person_info', anonymous=False)
    main(sys.argv)
