def convert_to_world_frame(pt2D): global TfTime # first scale from gui to gui_frame world_y = -(pt2D.x()-95-150)/scale_factor world_x = -(pt2D.y()-550 + 100)/scale_factor newPt = gPoint() newPt.ps.header.stamp=rospy.Time(0) newPt.ps.header.frame_id = gui_frame newPt.ps.point.x = world_x newPt.ps.point.y = world_y newPt.ps.point.z = z_offset - pt2D.get_zOffset()/scale_factor newPt.hangedge = pt2D.get_plane() # now convert to base_footprint #t = rospy.get_time() #listener.waitForTransform(poly_frame,newPt.ps.header.frame_id,newPt.ps.header.stamp,rospy.Duration(10.0)) #newPt.ps = listener.transformPoint(poly_frame,newPt.ps) #TfTime += rospy.get_time() - t #print "convert_to_world_frame", (pt2D.x(),pt2D.y()), "------->",(newPt.ps.point.x,newPt.ps.point.y,newPt.ps.point.z) return newPt
def dupl_gPoint(pt): newPt = gPoint() newPt.ps = dupl_PointStamped(pt.ps) newPt.hangedge = pt.hangedge return newPt