def show(self):
jac = self.robot.jac()
J = numpy.matrix([[jac[i,j] for j in range(jac.columns())]
for i in range(jac.rows())])
F = self.robot.pose()
(p,q,s,k) = get_ellipsoid(F,J)
c = marker.color_code(k)
m = marker.create(type=marker.Marker.SPHERE, ns='ellipsoid', id=42)
m.header.frame_id = self.base_frame_id
m.header.stamp = rospy.Time.now()
m.pose.position = Point(x=p[0], y=p[1], z=p[2])
m.pose.orientation = Quaternion(x=q[1], y=q[2], z=q[3], w=q[0])
m.color = ColorRGBA(r=c[0], g=c[1], b=c[2], a=k)
m.scale = Vector3(x=s[0]*self.scale, y=s[1]*self.scale, z=s[2]*self.scale)
marker.publish(m)
def _paint(self, event):
#determine size
border = 10
size_r = (self.Parent.Size[0] - 2*border) / self.values.shape[1]
size_c = (self.Parent.Size[1] - 2*border) / self.values.shape[0]
size = max(1, min(size_r, size_c))
value_range = 1.0
f_col = (lambda x: marker.color_code(x, [-value_range, 0.0, value_range],
[[0.0, 0.0, 1.0], [1.0, 1.0, 1.0], [1.0, 0.0, 0.0]]))
colors = numpy.array([[f_col(c) for c in r] for r in self.values])
width = colors.shape[1]*size
height = colors.shape[0]*size
rgb = numpy.array(255.99*colors, dtype='uint8').repeat(size,0).repeat(size,1)
bitmap = wx.BitmapFromBuffer(width, height, rgb.flatten())
dc = wx.PaintDC(self)
dc.DrawBitmap(bitmap, border, border)
