if len(xyzs_blob) == 0: continue
model, _, _ = ransac(xyzs_blob,
ConstantModel,
minDataPts=1,
nIter=10,
threshold=.04,
nCloseRequired=self.args.min_pix)
if model is not None:
centers.append(model.mean)
centers = np.array(centers).reshape(-1, 3)
plot_image = cv_drawing.drawNums(bgr, pcl_utils.xyz2uv(centers))
cv2.imshow(self.window_name, plot_image)
if self.args.pause:
cv2.waitKey(0)
else:
cv2.waitKey(5)
return centers
def add_extra_arguments(self, parser):
parser.add_argument('label', type=int)
parser.add_argument('--min_pix', type=int, default=10, nargs='?')
parser.add_argument('--max_blobs', type=int, default=10, nargs='?')
parser.add_argument('--pause', action="store_true", default=False)
return parser
comm.initComm()
M = GetRegionCenters()
M.run()
#brett.rgrip.set_angle_goal(rgrip[i])
#sleep(.04)
#except Exception:
#cmclient.switch_controllers(["l_arm_controller"], ["l_cart"])
#cmclient.switch_controllers(["r_arm_controller"], ["r_cart"])
#traceback.print_exc()
#exit(0)
# TODO: set saturation arguments for velocity and torque
# also reduce max effort arguments
rospy.init_node("knot_tying",disable_signals=True)
comm.initComm();
rope_getter = comm.LatestFileGetter("rope_model","txt",comm.ArrayMessage)
library = rope_library.RopeTrajectoryLibrary(args.dbname, "write")
brett = pr2.PR2()
cmclient = ControllerManagerClient()
while True:
rope_k3 = rope_getter.recv_latest().data
draw_curve(rope_k3)
_,rars = library.get_closest_and_warp(rope_k3)
left_used = (rars["grab_l"] > -1).any()
right_used = (rars["grab_r"] > -1).any()
l_quat = rars["quat_l"]
import cv2
class LabelImages(Mapper):
def __init__(self):
Mapper.__init__(self, ["bmp"],
[comm.MultiChannelImageMessage],
"bmp", comm.SingleChannelImageMessage)
self.window_name = "labeling"
cv2.namedWindow(self.window_name,cv2.cv.CV_WINDOW_NORMAL)
with open(self.args.classifier,"r") as fh:
print "unpickling"
self.classifier = cPickle.load(fh)
print "done"
self.mask = cv2.imread(self.args.mask,0)
def add_extra_arguments(self, parser):
parser.add_argument('classifier',type=str)
parser.add_argument('--mask',default=join(comm.DATA_ROOT,"once/roi_mask.png"))
return parser
def func(self, bgr):
label = self.classifier.predict(bgr,self.mask)
cv2.imshow(self.window_name,annotation.colorizeLabels(bgr,label))
cv2.waitKey(5)
return label
comm.initComm()
M = LabelImages()
M.run()
