Ask Your Question
0

Skeleton visualization in Rviz

asked 2017-10-30 01:45:02 -0500

Ravi Joshi gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2017-10-30 06:31:28 -0500

john.j.oneill gravatar image

I think this is a perfectly fine way of visualizing a skeleton in RVIZ. I would recommend adding them all into one MarkerArray message, I found it provides a much better refresh than sending individual markers.

As for simplifying your if statements, I think you could use an array to map the numbers to different objects for a slight speedup, but I find your code perfectly readable, you just have a lot of different body parts.

edit flag offensive delete link more

Comments

Thank you very much. I learned something new, i.e., MarkerArray. I have a question. In the above code, I used marker.action = Marker.ADD, hence it keeps on displaying old lines as well. I want to delete old lines, hence I used marker.action = Marker.DELETE BUT it didn't work. Any comments?

Ravi Joshi gravatar image Ravi Joshi  ( 2017-10-30 07:17:07 -0500 )edit

You can set some short lifetime as described here.

l4ncelot gravatar image l4ncelot  ( 2017-10-30 07:35:50 -0500 )edit

I seem to recall that making an array solved the whole redraw/update problem. I had originally tried to set a short lifetime on my many Marker messages as @l4ncelot suggested, but now I'm using a MarkerArray with lifetime=1s and it doesn't show any old lines despite updating at 10 Hz.

john.j.oneill gravatar image john.j.oneill  ( 2017-10-30 09:19:00 -0500 )edit
1

answered 2017-10-30 02:29:38 -0500

gvdhoorn gravatar image

updated 2017-10-30 02:30:51 -0500

Is there any better way to visualize skeleton in RVIZ?

You could try the skeleton_markers package.

edit flag offensive delete link more

Comments

I noticed that the package you provide also using the similar code. Thanks a lot.

Ravi Joshi gravatar image Ravi Joshi  ( 2017-10-30 07:20:45 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-10-30 01:45:02 -0500

Seen: 323 times

Last updated: Oct 30 '17