def update2( self, particles ):
xyz = np.column_stack([ particles[:,0:2], np.zeros( particles.shape[0] )]).T
w = particles[:,2] # becomes 1D
# print w
wmin = np.min( w )
wmax = np.max( w )
# import pdb
# pdb.set_trace()
if wmin == wmax:
colors = np.row_stack([ np.ones( particles.shape[0] ),
np.zeros( particles.shape[0] ),
np.zeros( particles.shape[0] ),
np.ones( particles.shape[0] ) ])
iv = np.ones( len( w )).tolist()
else:
iv = [ int( 1.0 * ( wi - wmin ) / (wmax - wmin) * 255.0 ) for wi in w ]
colors = np.array([ pl.cm.jet( ivi ) for ivi in iv ]).T
# colors[3] *= 0.3
print np.array(iv)
ind = np.where( np.array(iv) > 5 )[0]
aind = np.argsort( w[ind] ) # sort them so that some come to top.
m = viz.list_marker( xyz[:,ind][:,aind], colors[:,ind][:,aind], [0.05, 0.05, 0.025], 'points', '/map', 30 )
m.header.stamp = rospy.Time.now()
for i in xrange( 10 ):
self.pub_mark.publish( m )
rospy.sleep( 0.2 )
return
def update(self, particles):
xyz = np.column_stack(
[particles[:, 0:2],
np.zeros(particles.shape[0])]).T
w = particles[:, 2] # becomes 1D
# print w
wmin = np.min(w)
wmax = np.max(w)
# import pdb
# pdb.set_trace()
if wmin == wmax:
colors = np.row_stack([
np.ones(particles.shape[0]),
np.zeros(particles.shape[0]),
np.zeros(particles.shape[0]),
np.ones(particles.shape[0])
])
else:
colors = np.array([
pl.cm.jet(int(1.0 * (wi - wmin) / (wmax - wmin) * 255.0))
for wi in w
]).T
m = viz.list_marker(xyz, colors, [0.05, 0.05, 0.05], 'points', '/map',
300)
m.header.stamp = rospy.Time.now()
for i in xrange(10):
self.pub_mark.publish(m)
rospy.sleep(0.2)
return
def update( self, particles ):
xyz = np.column_stack([ particles[:,0:2], np.zeros( particles.shape[0] )]).T
w = particles[:,2] # becomes 1D
# print w
wmin = np.min( w )
wmax = np.max( w )
# import pdb
# pdb.set_trace()
if wmin == wmax:
colors = np.row_stack([ np.ones( particles.shape[0] ),
np.zeros( particles.shape[0] ),
np.zeros( particles.shape[0] ),
np.ones( particles.shape[0] ) ])
else:
colors = np.array([ pl.cm.jet( int( 1.0 * ( wi - wmin ) / (wmax - wmin) * 255.0 ))
for wi in w ]).T
m = viz.list_marker( xyz, colors, [0.05, 0.05, 0.05], 'points', '/map', 300 )
m.header.stamp = rospy.Time.now()
for i in xrange( 10 ):
self.pub_mark.publish( m )
rospy.sleep( 0.2 )
return
def visualize_contact_dict(cd, marker_pub, fts):
color_list = [(1.,0.,0.), (0.,1.,0.), (0.,0.,1.), (1.,1.,0.),
(1.,0.,1.), (0.,1.,1.), (0.5,1.,0.), (0.5,0.,1.),
(0.,0.5,1.) ]
pts_list = []
cs_list = []
marker_list = []
for i, k in enumerate(cd.keys()):
pts = np.matrix(cd[k]).T
c = color_list[i]
cs = np.ones((4, pts.shape[1]))
cs[0,:] = c[0]
cs[1,:] = c[1]
cs[2,:] = c[2]
pts_list.append(pts)
cs_list.append(cs)
print '# of contact points:', pts.shape[1]
mn = np.mean(pts, 1)
f = fts.get_forces()
m1, m2 = fvt.get_arrow_text_markers(mn, f, 'base_footprint',
m_id = 2*i+1, duration=0.5)
marker_pub.publish(m1)
marker_pub.publish(m2)
m = hv.list_marker(np.column_stack(pts_list),
np.column_stack(cs_list), (0.01, 0.01, 0.01),
'points', 'base_footprint', duration=1.0,
m_id=0)
t_now = rospy.Time.now()
m.header.stamp = t_now
marker_pub.publish(m)
for m in marker_list:
m.header.stamp = rospy.Time.now()
marker_pub.publish(m)
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0,:] = 243/255.0
colors[1,:] = 132/255.0
colors[2,:] = 0.
colors[3,:] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T, colors, scale, 'points',
frame, duration=duration, m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.0)[0] #was .01
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.)[0] #was 0.2
#force_marker_scale = 0.04
force_marker_scale = 1.0
duration = 0.4
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
if False:
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0,0] * force_marker_scale
if 'electric' not in roslib.__path__[0]:
scale = (l1, 0.2, 0.2)
else:
scale = (0.2, 0.2, 0.2) #something weird here, was (0.2, 0.2, l1)
scale = (0.4, 0.4, l1)
m = hv.single_marker(p, q1, 'arrow', frame, duration=duration,
scale=scale, m_id=3*i+1)
m.header.stamp = stamp
#markers.markers.append(m)
if True:
if np.linalg.norm(n2) < 0.30:
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
m = Marker()
m.points.append(Point(p[0,0], p[1,0], p[2,0]))
m.points.append(Point(p[0,0]+n2[0,0], p[1,0]+n2[1,0], p[2,0]+n2[2,0]))
m.scale = Point(0.02, 0.04, 0.0)
m.header.frame_id = frame
m.id = 3*i+2
m.type = Marker.ARROW
m.action = Marker.ADD
m.color.r = 0.
m.color.g = 0.8
m.color.b = 0.
m.color.a = 1.
m.lifetime = rospy.Duration(duration)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2/np.linalg.norm(n2) * l2 * 1.2, q2, 'text_view_facing', frame,
(0.07, 0.07, 0.07), m_id = 3*i+3,
duration = duration, color=(0.5,0.5,0.5,1.))
m.text = '%.2fm'%(np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0, :] = 243 / 255.0
colors[1, :] = 132 / 255.0
colors[2, :] = 0.
colors[3, :] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T,
colors,
scale,
'points',
frame,
duration=duration,
m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.01)[0]
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.2)[0]
force_marker_scale = 0.04
duration = 0.02
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0, 0] * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l1, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l1)
m = hv.single_marker(p,
q1,
'arrow',
frame,
duration=duration,
scale=scale,
m_id=3 * i + 1)
m.header.stamp = stamp
markers.markers.append(m)
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l2, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l2)
m = hv.single_marker(p,
q2,
'arrow',
frame,
duration=duration,
scale=scale,
color=(0., 0.5, 0., 1.0),
m_id=3 * i + 2)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2 / np.linalg.norm(n2) * l2 * 1.6,
q2,
'text_view_facing',
frame, (0.07, 0.07, 0.07),
m_id=3 * i + 3,
duration=duration,
color=(0., 0.5, 0., 1.))
m.text = '%.1fN' % (np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0,:] = 243/255.0
colors[1,:] = 132/255.0
colors[2,:] = 0.
colors[3,:] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T, colors, scale, 'points',
frame, duration=duration, m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.01)[0]
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.2)[0]
force_marker_scale = 0.04
duration = 0.02
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0,0] * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l1, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l1)
m = hv.single_marker(p, q1, 'arrow', frame, duration=duration,
scale=scale, m_id=3*i+1)
m.header.stamp = stamp
markers.markers.append(m)
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l2, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l2)
m = hv.single_marker(p, q2, 'arrow', frame, duration=duration,
scale=scale, color=(0.,0.5,0.,1.0),
m_id=3*i+2)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2/np.linalg.norm(n2) * l2 * 1.6, q2, 'text_view_facing', frame,
(0.07, 0.07, 0.07), m_id = 3*i+3,
duration = duration, color=(0.,0.5,0.,1.))
m.text = '%.1fN'%(np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0, :] = 243 / 255.0
colors[1, :] = 132 / 255.0
colors[2, :] = 0.
colors[3, :] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T,
colors,
scale,
'points',
frame,
duration=duration,
m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.0)[0] #was .01
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.)[0] #was 0.2
#force_marker_scale = 0.04
force_marker_scale = 1.0
duration = 0.4
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
if False:
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0, 0] * force_marker_scale
if 'electric' not in roslib.__path__[0]:
scale = (l1, 0.2, 0.2)
else:
scale = (0.2, 0.2, 0.2
) #something weird here, was (0.2, 0.2, l1)
scale = (0.4, 0.4, l1)
m = hv.single_marker(p,
q1,
'arrow',
frame,
duration=duration,
scale=scale,
m_id=3 * i + 1)
m.header.stamp = stamp
#markers.markers.append(m)
if True:
if np.linalg.norm(n2) < 0.30:
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
m = Marker()
m.points.append(Point(p[0, 0], p[1, 0], p[2, 0]))
m.points.append(
Point(p[0, 0] + n2[0, 0], p[1, 0] + n2[1, 0],
p[2, 0] + n2[2, 0]))
m.scale = Point(0.02, 0.04, 0.0)
m.header.frame_id = frame
m.id = 3 * i + 2
m.type = Marker.ARROW
m.action = Marker.ADD
m.color.r = 0.
m.color.g = 0.8
m.color.b = 0.
m.color.a = 1.
m.lifetime = rospy.Duration(duration)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2 / np.linalg.norm(n2) * l2 * 1.2,
q2,
'text_view_facing',
frame, (0.07, 0.07, 0.07),
m_id=3 * i + 3,
duration=duration,
color=(0.5, 0.5, 0.5, 1.))
m.text = '%.2fm' % (np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)
