Skeleton visualization in Rviz

edit

I want to visualize skeleton in Rviz. The skeleton tracking is done separately and skeleton data is subscribed by providing rostopic. Anyway, the skeleton can be considered as a combination of line strips, which is set of lines. Hence, I decomposed the skeleton into three LINE_STRIP as following:

1. upper_body # from head to spine base
2. hands # from left-hand tip to right-hand tip
3. legs # from left foot to right foot

I am adding points in these LINE_STRIP based on their location. The skeleton is moving. upper_body.action = Marker.ADD is keep on adding history points as well, which is not desired.

Below is the code snippet:

class SkeletonVisualization():
    def __init__(self):
        ns = 'skeleton_visualization'
        rate = rospy.get_param('~rate', None)
        body_topic = rospy.get_param('~body_topic')
        line_width = rospy.get_param('~line_width', None)
        fixed_frame = rospy.get_param('~fixed_frame', 'base')

        # define a subscriber to retrive tracked bodies
        rospy.Subscriber(body_topic, BodyArray, self.receive_bodies)
        body_pub = rospy.Publisher(ns, Marker, queue_size=10)

        # define the color of line strip
        green = ColorRGBA(0.12, 0.63, 0.42, 1.00)

        # from head to spine base
        self.upper_body  = Marker()
        self.upper_body.id  = 0
        self.upper_body.ns = ns
        self.upper_body.color = green
        self.upper_body.type = Marker.LINE_STRIP
        self.upper_body.scale.x = float(line_width)
        self.upper_body.header.frame_id = fixed_frame

        #from left hand tip to right hand tip
        self.hands  = Marker()
        self.hands.id  = 1
        self.hands.ns = ns
        self.hands.color = green
        self.hands.type = Marker.LINE_STRIP
        self.hands.scale.x = float(line_width)
        self.hands.header.frame_id = fixed_frame

        #from left foot to right foot
        self.legs  = Marker()
        self.legs.id  = 2
        self.legs.ns = ns
        self.legs.color = green
        self.legs.type = Marker.LINE_STRIP
        self.legs.scale.x = float(line_width)
        self.legs.header.frame_id = fixed_frame

        rate = rospy.Rate(rate)
        while not rospy.is_shutdown():
            body_pub.publish(self.upper_body)
            body_pub.publish(self.hands)
            body_pub.publish(self.legs)
            rate.sleep()

    def receive_bodies(self, data):
        if not data.bodies:
            print('[WARN] No body found')
            return;

        body = data.bodies[0] # ignore other people
        time = rospy.Time.now()

        self.upper_body.header.stamp = time
        self.hands.header.stamp = time
        self.legs.header.stamp = time

        self.upper_body.action = Marker.DELETE
        self.hands.action = Marker.DELETE
        self.legs.action = Marker.DELETE

        self.upper_body.action = Marker.ADD
        self.hands.action = Marker.ADD
        self.legs.action = Marker.ADD

        for joint in body.jointPositions:
            if not joint.trackingState:
                return
            if joint.jointType == 3: #head
                self.upper_body.points.append(joint.position)
            elif joint.jointType == 2: #neck
                self.upper_body.points.append(joint.position)
            elif joint.jointType == 20: #spine shoulder
                self.upper_body.points.append(joint.position)
            elif joint.jointType == 1: #spine mid
                self.upper_body.points.append(joint.position)
            elif joint.jointType == 0: #spine base
                self.upper_body.points.append(joint.position)

            elif joint.jointType == 21: #hand tip left
                self.hands.points.append(joint.position)
            elif joint.jointType == 7: #hand left
                self.hands.points.append(joint.position)
            elif joint.jointType == 6: #wrist left
                self.hands.points.append(joint.position)
            elif joint.jointType == 5: #elbow left
                self.hands.points.append(joint.position)
            elif joint.jointType == 4: #shoulder left
                self.hands.points.append(joint.position)
            elif ...