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