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()