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
