Fetch_Robot.Head.look_at(0.7,0,0.5,"base_link")
rospy.loginfo("Till head")
Fetch_Robot.Gripper.Open()
rospy.loginfo("Gripper Open")
Fetch_Robot.Arm.Tuck()
rospy.loginfo("Tuck Arm")
rospy.sleep(rospy.Duration(2))
#Take images
RGB_image = Fetch_Robot.GetRGBImage()
rospy.loginfo("Get RGB image")
Depth_image = Fetch_Robot.GetDepthImage()
rospy.loginfo("Get Depth image")
#Take point cloud
ArPose = Fetch_Robot.GetArPoses()
rospy.sleep(rospy.Duration(2))
ArPose = Fetch_Robot.GetArPoses()
while (ArPose.header.frame_id is None):
ArPose = Fetch_Robot.GetArPoses()
print("aaaaa")
rospy.loginfo("Get ArPoses")
print(ArPose)
#Calculate Poses
