Problems in visualizing sequential points as connected line segments
Hi, I am trying to visualize sequential points as connected line segments using Rviz in ROS Indigo Ubuntu 14.04 LTS PC. Below is the snippet of data, received using rostopic echo command on terminal:
jointPositions:
-
trackingState: True
position:
x: -0.435596
y: -0.142820448
z: 1.82237065
jointType: 0
-
trackingState: True
position:
x: -0.4052974
y: 0.13666603
z: 1.73648691
jointType: 1
-
trackingState: True
position:
x: -0.3760757
y: 0.345686644
z: 1.65679443
jointType: 20
-
trackingState: True
position:
x: -0.36667335
y: 0.4120549
z: 1.62908721
jointType: 2
-
trackingState: True
position:
x: -0.324401945
y: 0.485795557
z: 1.58831823
jointType: 3
-
trackingState: True
position:
x: -0.5145141
y: 0.284414381
z: 1.65062976
jointType: 4
-
trackingState: True
position:
x: -0.612595141
y: 0.4024704
z: 1.55085456
jointType: 5
-
trackingState: True
position:
x: -0.6313961
y: 0.549976766
z: 1.4715327
jointType: 6
-
trackingState: True
position:
x: -0.6528342
y: 0.620140254
z: 1.448911
jointType: 7
-
trackingState: True
position:
x: -0.265343577
y: 0.278526247
z: 1.75809133
jointType: 8
-
trackingState: True
position:
x: -0.123725727
y: 0.2178925
z: 1.6266855
jointType: 9
-
trackingState: True
position:
x: -0.007252288
y: 0.183696628
z: 1.43388867
jointType: 10
-
trackingState: True
position:
x: 0.105958693
y: 0.157047316
z: 1.27662742
jointType: 11
-
trackingState: True
position:
x: -0.487728119
y: -0.1287106
z: 1.77689838
jointType: 12
-
trackingState: True
position:
x: -0.433793515
y: -0.344230652
z: 1.39414346
jointType: 13
-
trackingState: True
position:
x: -0.4238537
y: -0.59930253
z: 1.2503947
jointType: 14
-
trackingState: True
position:
x: -0.378920853
y: -0.645869136
z: 1.1385659
jointType: 15
-
trackingState: True
position:
x: -0.367541432
y: -0.151269555
z: 1.80178738
jointType: 16
-
trackingState: True
position:
x: -0.205929413
y: -0.399689019
z: 1.45195925
jointType: 17
-
trackingState: True
position:
x: -0.194217354
y: -0.525068
z: 1.39156806
jointType: 18
-
trackingState: True
position:
x: -0.1547204
y: -0.5737468
z: 1.27755761
jointType: 19
-
trackingState: True
position:
x: -0.682371557
y: 0.688038
z: 1.41724646
jointType: 21
-
trackingState: True
position:
x: -0.681404054
y: 0.5892486
z: 1.45565391
jointType: 22
-
trackingState: True
position:
x: 0.1445924
y: 0.141057357
z: 1.18761849
jointType: 23
-
trackingState: True
position:
x: 0.09335878
y: 0.201266348
z: 1.25917614
jointType: 24
The above data is received from Kinect and represents joints in the human skeleton, in which each joint has unique jointType field. Below is the more information about joint type as defined in Kinect v2 sensor:
I want to visualize this skeleton by showing it as connected line segments in Rviz. However, the displayed skeleton looks weird as below:
Here, the skeleton is considered as a combination of line strips, which is set of lines. Hence, I decomposed the skeleton into three LINE_STRIP as following:
upper_body# [In Red Color] from head to spine basehands# [In Green Color] from left-hand tip to right-hand tiplegs# [In Blue Color] from left foot to right foot
Below is the code snippet:
# define a subscriber to retrive tracked bodies
rospy.Subscriber(body_topic, BodyArray, self.receive_body)
self.body_pub = rospy.Publisher(ns, MarkerArray, queue_size=2)
# define the colors
red = ColorRGBA(0.98, 0.30, 0.30, 1.00)
green = ColorRGBA(0.12, 0.63, 0.42, 1.00)
blue = ColorRGBA(0.26, 0.09, 0.91, 1.00)
self.body_msg = MarkerArray()
self.upper_body = Marker()
self.upper_body.id = 0
self.upper_body.ns = ns
self.upper_body.color = red
self.upper_body.action = Marker.ADD
self.upper_body.type = Marker.LINE_STRIP
self.upper_body.scale.x = float(line_width)
self.upper_body.header.frame_id = fixed_frame
self.hands = Marker()
self.hands.id = 1
self.hands.ns = ns
self.hands.color = green
self.hands.action = Marker.ADD
self.hands.type = Marker.LINE_STRIP
self.hands.scale.x = float(line_width)
self.hands.header.frame_id = fixed_frame
self.legs = Marker()
self.legs.id = 2
self.legs.ns = ns
self.legs.color = blue
self.legs.action = Marker.ADD
self.legs.type = Marker.LINE_STRIP
self.legs.scale.x = float(line_width)
self.legs.header.frame_id = fixed_frame
self.body_msg.markers.append(self.upper_body)
self.body_msg.markers.append(self.hands)
self.body_msg.markers.append(self.legs)
rospy.spin()
def receive_body(self, data):
if not data.bodies:
print("[WARN] No body tracked")
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
# remove old points
self.upper_body.points = list()
self.hands.points = list()
self.legs.points = list()
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 joint.jointType == 20: #spine shoulder
self.hands.points.append(joint.position)
elif joint.jointType == 8: #shoulder right
self.hands.points.append(joint.position)
elif joint.jointType == 9: #elbow right
self.hands.points.append(joint.position)
elif joint.jointType == 10: #wrist right
self.hands.points.append(joint.position)
elif joint.jointType == 11: #hand right
self.hands.points.append(joint.position)
elif joint.jointType == 23: #hand tip right
self.hands.points.append(joint.position)
elif joint.jointType == 15: #foot left
self.legs.points.append(joint.position)
elif joint.jointType == 14: #ankle left
self.legs.points.append(joint.position)
elif joint.jointType == 13: #knee left
self.legs.points.append(joint.position)
elif joint.jointType == 12: #hip left
self.legs.points.append(joint.position)
elif joint.jointType == 0: #spine base
self.legs.points.append(joint.position)
elif joint.jointType == 16: #hip right
self.legs.points.append(joint.position)
elif joint.jointType == 17: #knee right
self.legs.points.append(joint.position)
elif joint.jointType == 18: #ankle right
self.legs.points.append(joint.position)
elif joint.jointType == 19: #foot right
self.legs.points.append(joint.position)
self.body_pub.publish(self.body_msg)
One workaround is to use Marker.POINTS instead of Marker.LINE_STRIP and show it as a collection of points. However, I want to show it as connected line segments. Any help, please.
Asked by ravijoshi on 2017-10-31 01:29:55 UTC
Comments