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
