def drawToImage(self,img,imgtype): circle = self.shape center = circle.center() if (imgtype == 'temp'): center = Geometry2D.translatePt(center, 500,0) elif(imgtype == 'sel'): center = Geometry2D.translatePt(center, 1000,0) if self.filled(): return cv.Circle(img, center.toTuple(),int(circle.radius()),self.getDrawColor(),-1) else: return cv.Circle(img,center.toTuple(),int(circle.radius()),self.getDrawColor(),1)
def correct_foldpoints(self,state,scoot_prev = 0,child=None): """ human in the loop correction """ action = state.action self.gui.clearProposed() # draw gripper_point #drag_x, drag_y = state.getDragDistance() #newPolys = self.gui.translatePolys(state.parent.get_polys(), drag_x, drag_y) if not EXECUTE_FLAG: drag_x,drag_y = state.parent.getDragDistance() newPolys = self.gui.translatePolys(state.parent.get_polys(), drag_x, drag_y) for poly in newPolys: self.gui.addPropCVShape(poly) gripPts_new = [] endPts_new = [] print "previous scoot ",scoot_prev #print "CLICK GRIP-POINT" i = 0 for gripPt_old, endPt_old in zip(action.get_gripPoints(), action.get_endPoints()): if action.get_actionType() == "fold" and action.get_foldType() == "red": self.gui.drawGripper(endPt_old) self.gui.highlightPt(endPt_old) print "old endpoint",endPt_old else: self.gui.drawGripper(gripPt_old) self.gui.highlightPt(gripPt_old) print "old grippoint",gripPt_old,"old endpoint",endPt_old print "ClICK Corresponding Point and hit enter or Hit p to proceed with old grippoint" if (raw_input() == 'p'): self.corrected_gripPoint_latest = gripPt_old else: ''' while not self.corrected_gripPoint_latest: print self.corrected_gripPoint_latest print "Click correspondiong grippoint and hit enter or hit p to proceed with old grippoint" raw_input() ''' if self.corrected_gripPoint_latest == None: print 'Click corrected grip point then press enter' raw_input() #stamped_poly = rospy.wait_for_message('/poly_maker_output', PolyStamped) self.corrected_gripPoint_latest.xval += scoot_prev gripPts_new.append(deepcopy(self.corrected_gripPoint_latest)) #ptMove = Geometry2D.ptDiff(deepcopy(self.corrected_gripPoint_latest), gripPt_old) if action.get_actionType() == "fold" and action.get_foldType() == "red": ptMove = Geometry2D.ptDiff(deepcopy(self.corrected_gripPoint_latest), endPt_old) ptMove.yval = 0 else: ptMove = Geometry2D.ptDiff(deepcopy(self.corrected_gripPoint_latest), gripPt_old) child_action = child.action if child else None if action.get_actionType() == "drag" and child_action and child_action.get_actionType() == "fold" and child_action.get_foldType() == "red": child_gripPt_old,child_endPt_old = child_action.get_gripPoints()[i],child_action.get_endPoints()[i] print "child: old gripPt",child_gripPt_old,"child: old endpoint",child_endPt_old child_gripPt_old.translate(ptMove.x()-scoot_prev,ptMove.y()) child_endPt_old.translate(ptMove.x()-scoot_prev,ptMove.y()) print "child: new gripPt",child_gripPt_old,"child: new endpoint",child_endPt_old print "error", ptMove.x(), ptMove.y() self.corrected_gripPoint_latest = None endPt_new = Geometry2D.translatePt(endPt_old, ptMove.x(), ptMove.y()) print "old grippoint",gripPt_old,"old endpoint",endPt_old print "new grippoint",gripPts_new[-1], "new endpoint",endPt_new endPts_new.append(endPt_new) i +=1 print "correction done" return gripPts_new, endPts_new