def update(self):
'''update the marker to reflect the current joint angle'''
p1 = self.pose_base.p
p2 = self.pose_base * (self.axis*self.chi)
marker.align(self.marker, p1, p2, config.axisWidth)
self.marker.color = color_code(abs(self.chi - self.chi_desired) / config.error_length)
def axis_marker(tw, id = 0, ns = 'twist'):
""" make a marker message showing the instantaneous
rotation axis of a twist message"""
t = kdl.Twist(kdl.Vector(tw.linear.x, tw.linear.y, tw.linear.z),
kdl.Vector(tw.angular.x, tw.angular.y, tw.angular.z))
try:
(x, rot) = listener.lookupTransform(target_frame, ref_frame, rospy.Time(0))
(trans, rotp) = listener.lookupTransform(target_frame, ref_point, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
print 'tf exception!'
return marker.create(id=id, ns=ns, action=Marker.DELETE)
# put the twist in the right frame
f = posemath.fromTf( (trans, rot) )
t = f*t
direction = t.rot
location = t.rot * t.vel / kdl.dot(t.rot, t.rot)
lr = t.rot.Norm()
lt = t.vel.Norm()
m = marker.create(id=id, ns=ns, type=Marker.CYLINDER)
if lr < 0.0001 and lt < 0.0001:
return marker.create(id=id, ns=ns, action=Marker.DELETE)
if lr < 0.001:
# very short axis, assume linear movement
location = kdl.Vector(0,0,0)
direction = t.vel
if lt < min_length:
direction *= min_length / lt
m.type = Marker.CUBE
m = marker.align(m, location, location + direction, 0.02)
elif lr < min_length:
direction = direction / lr * min_length
m = marker.align(m, location - direction, location + direction, 0.02)
elif lr > max_length:
direction = direction / lr * max_length
m = marker.align(m, location - direction, location + direction, 0.02)
else:
#BAH! How do I make this better?
m = marker.align(m, location - direction, location + direction, 0.02)
m.header.frame_id = target_frame
m.frame_locked = True
if(use_colors):
m.color = colors[id % len(colors)]
else:
m.color = ColorRGBA(0.3, 0.3, 0.3, 1)
return m
def display_feature(feature, frame, frame_id, id=1): m = marker.create(type=marker.Marker.CYLINDER, id=id) p = frame * feature.position d = (frame * (feature.position + feature.direction)) - p s = feature.size marker.align(m, p-d*s, p+d*s, 0.01) m.header.frame_id = frame_id m.color = marker.ColorRGBA(0.0, 1.0, 0.0, 1.0) marker.publish(m)
def update(self):
'''update the marker to reflect the current joint angle'''
p1 = self.pose_base * (-self.axis * config.axisLength)
p2 = self.pose_base * ( self.axis * config.axisLength)
marker.align(self.marker, p1, p2, config.axisWidth)
self.marker.color = color_code(abs(self.chi - self.chi_desired) / config.error_angle)
if self.weight == 0.0:
self.marker.color = ColorRGBA(0.0, 0.0, 1.0, 1.0)
def cylinders(frm, to, diameter, ns=''): m1 = marker.create(id=21, ns=ns, type=Marker.CYLINDER) m1.color = ColorRGBA(1, 0, 0, 0.7) m1 = marker.align(m1, frm, kdl.Vector(to[0], frm[1], frm[2]), diameter) m2 = marker.create(id=22, ns=ns, type=Marker.CYLINDER) m2.color = ColorRGBA(0, 1, 0, 0.7) m2 = marker.align(m2, frm, kdl.Vector(frm[0], to[1], frm[2]), diameter) m3 = marker.create(id=23, ns=ns, type=Marker.CYLINDER) m3.color = ColorRGBA(0, 0, 1, 0.7) m3 = marker.align(m3, frm, kdl.Vector(frm[0], frm[1], to[2]), diameter) return [m1, m2, m3]
def markers(self):
''' get the markers for visualization '''
self._compute_lengths()
frame = self._transform()
mrks = []
pos = kdl.Vector(0,0,0)
l_index = 0
for i,t in enumerate(self.jac):
twist = frame*t
lr = twist.rot.Norm()
lv = twist.vel.Norm()
if lr < self.eps and lv < self.eps:
# null twist, make a delete marker
mrks.append(marker.create(id=i, ns=self.name+str(i), action=Marker.DELETE))
continue
if lr < self.eps:
# pure translational twist
location = pos
direction = twist.vel / lv * self.lengths[l_index]
m = marker.create(id=i, ns=self.name+str(i), type=Marker.CUBE)
m = marker.align(m, location, location + direction, self.width)
l_index += 1
frame.p = frame.p - direction # move target point to end of this 'axis'
pos += direction # remember current end point
else:
# rotational twist
location = twist.rot * twist.vel / kdl.dot(twist.rot, twist.rot) + pos
if self.independent_directions:
direction = twist.rot * self.rot_length / lr
else:
# take axis from interaction matrix and transform to location
dir_H = self.H[i].rot - location*self.H[i].vel
direction = dir_H * self.rot_length / dir_H.Norm()
m = marker.create(id=i, ns=self.name+str(i), type=Marker.CYLINDER)
m = marker.align(m, location - direction, location + direction, self.width)
m.color = self._color(i)
m.header.frame_id = self.target_frame
mrks.append(m)
return mrks
def marker_line(start, end, **args): global _config_ m = marker_base(type=marker.Marker.CYLINDER, **args) marker.align(m, start, end, m.scale.z) return m
