def plotBoxCorners(corner_points,
viz_pub=None,
color_rgb=None,
namespace='corner_points',
isFeasible=None):
if viz_pub != None:
markers_msg = MarkerArray()
#~ markers_msg.markers.append(createAllMarker('corner_points'))
for i in range(0, len(corner_points)):
#~ if isFeasible[i] is not:
if color_rgb == None:
my_rgb = (1, 0, 1, 1)
else:
my_rgb = color_rgb
m = createSphereMarker(offset=tuple(corner_points[i]),
marker_id=(i + 1),
rgba=my_rgb,
orientation=(0.0, 0.0, 0.0, 1.0),
scale=(.015, .015, .015),
frame_id="/map",
ns=namespace)
markers_msg.markers.append(m)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
rospy.sleep(.1)
def vizPoint(pos):
"""point visualization"""
marker = createSphereMarker(offset=pos,
color=[0, 0, 1, 0.5],
scale=[0.01, 0.01, 0.01])
vizpub.publish(marker)
marker.id = 201
def vizSphere(vizpub, pose, scale, frame_id, rgba=(0.5,0.5,0.5,1), marker_id=2):
# prepare block visualization
meshmarker = createSphereMarker(color=rgba,
scale = scale,
offset=tuple(pose[0:3]),
orientation=tuple(pose[3:7]), frame_id=frame_id, marker_id = marker_id)
meshmarker.ns = frame_id
vizpub.publish(meshmarker)
def vizPoint(pos):
# prepare block visualization
global vizpub
marker = createSphereMarker(offset=pos,
color=[0, 0, 1, 0.5],
scale=[0.01, 0.01, 0.01])
vizpub.publish(marker)
rospy.sleep(0.1)
def plotPickPoints(pick_points,viz_pub=None,special_index=None):
if viz_pub!=None:
markers_msg = MarkerArray()
markers_msg.markers.append(createDeleteAllMarker('picking_points'))
for i in range(0,len(pick_points)):
if i==special_index:
my_rgb=(0,1,0,1)
else:
my_rgb=(1,0,1,1)
m=createSphereMarker(offset=tuple(pick_points[i]), marker_id = (i+1), rgba=my_rgb, orientation=(0.0,0.0,0.0,1.0), scale=(.03,.03,.03), frame_id="/map", ns = 'picking_points')
markers_msg.markers.append(m)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
rospy.sleep(.1)
def vizPoint(pos):
# prepare block visualization
global vizpub
marker = createSphereMarker(offset=pos, color=[0, 0, 1, 0.5], scale=[0.01,0.01,0.01])
vizpub.publish(marker)
rospy.sleep(0.1)