def stereo_handler(self,stamped_point): rospy.loginfo("STEREO POINT RECEIVED") if not util.BUSY: return else: point = Point2D(-stamped_point.point.y,-stamped_point.point.x) point = util.convert_from_world_frame(point) self.corrected_gripPoint_latest = point print "converted to",point.x(),point.y()
def poly_handler(self,stamped_poly): rospy.loginfo("RECEIVED A POINT") if(util.BUSY == True): return #self.robot.arms_test() points = stamped_poly.vertices #[Geometry2D.Point(point.x,point.y) for point in stamped_poly.vertices] vertices = [util.convert_from_world_frame(point) for point in points] """ for pt in vertices: print pt """ if SIM_FLAG: if os.environ['UTIL_MODE']: util.mode = os.environ['UTIL_MODE'] self.mode = util.mode print "PRINTING UTIL MODE", util.mode tbl =Geometry2D.Point(151.979, 405.130237) tbf = Geometry2D.Point(224.808244, 488.925433) tbr = Geometry2D.Point(350.632802, 468.785788) self.table_detector([tbl,tbf,tbr]) bl = bottomLeftCorners[self.mode] bl = Geometry2D.Point(bl[0], bl[1]) poly = Geometry2D.Polygon(*self.getModel(bl)) #(*vertices) #poly = Geometry2D.Polygon(*self.gui.makePants(vertices[0])) #bl = Geometry2D.Point(200,470) #poly = Geometry2D.Polygon(*self.makePolyFns[self.article_ind](bl)) #(*vertices) #poly = Geometry2D.Polygon(*self.gui.makePants(vertices[0])) self.poly_cache = poly cvPoly = CVPolygon(Colors.PALEGREEN,self.gui.front(self.gui.shapes),poly) self.gui.clearShapes() self.gui.addCVShape(cvPoly) self.handle_automatic_folds(self.gui.getPolys()[0].getShape().vertices()) return # Edited for simulation # the first 6 define the table edge if TABLE_FLAG: #print vertices vertices = vertices[3:] else: self.table_detector(vertices) return if len(vertices) == 0: return #self.robot.arms_test() #poly = Geometry2D.Polygon(*self.gui.makeVest(vertices[0])) #(*vertices) #poly = Geometry2D.Polygon(*vertices) #poly = Geometry2D.Polygon(*self.gui.makeBerkeleyProjectTee(Geometry2D.Point(199.512588,368.866753))) if os.environ['UTIL_MODE']: util.mode = os.environ['UTIL_MODE'] self.mode = util.mode print "Current Mode is", util.mode poly = Geometry2D.Polygon(*self.getModel(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makeBigTowel(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makeBlackWillowTee(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makeSmallRedTowel(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makePants(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makeShirt(vertices[0])) self.poly_cache = poly cvPoly = CVPolygon(Colors.GREEN,self.gui.front(self.gui.shapes),poly) self.gui.clearShapes() self.gui.addCVShape(cvPoly) self.handle_automatic_folds(self.gui.getPolys()[0].getShape().vertices())