def create_linear_motion_task_planner(self, target_pose, time=4.0, steps=400.0):
        """ An *incredibly simple* linearly-interpolated Cartesian move """

        # self.trajectory_planner.enable_collision_table1 = False
        # self.trajectory_planner.clear_parameters()
        # self.trajectory_planner.scene.remove_world_object("table1")
        # rospy.sleep(1.0)

        self.trajectory_planner.table1_z = -1.0
        rospy.logwarn("Targetpose:" + str(target_pose))
        jnts = self.sawyer_robot._limb.ik_request(target_pose, self.sawyer_robot._tip_name)
        rospy.logwarn("JNTS:" + str(jnts))
        success = self.safe_goto_joint_position(jnts).result()
        # success = self.trajectory_planner.move_to_joint_target(jnts.values(),attempts=300)
        self.trajectory_planner.table1_z = 0.0

        if not success:
            self.create_wait_forever_task().result()

        return True 

def execute_task(self, fn, args=[]):
        """
        :param fn:
        :return:
        """
        # INTERRUPT HERE CURRENT TASK AND SAVE STATE

        self.stop()

        while len(self.tasks) >0:
            rospy.sleep(0.5)

        #launch asynchronosuly
        fn(*args)

        self.robot_sayt2s("REQUESTED TASK COMPLETED")


        # self.create_main_loop_task().result() 

def connect(self):
        try:
            print "Connecting to Arduino on port", self.port, "..."
            self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
            # The next line is necessary to give the firmware time to wake up.
            time.sleep(1)
            test = self.get_baud()
            if test != self.baudrate:
                time.sleep(1)
                test = self.get_baud()   
                if test != self.baudrate:
                    raise SerialException
            print "Connected at", self.baudrate
            print "Arduino is ready."

        except SerialException:
            print "Serial Exception:"
            print sys.exc_info()
            print "Traceback follows:"
            traceback.print_exc(file=sys.stdout)
            print "Cannot connect to Arduino!"
            os._exit(1) 

def move_to_eep(self, target_pose, duration=1.5):
        """
        :param target_pose: Cartesian pose (x,y,z, quat). 
        :param duration: Total time trajectory will take before ending
        """
        p1, q1 = self.get_xyz_quat()
        p2, q2 = target_pose[:3], target_pose[3:]

        last_pos = self.get_joint_angles()
        last_cmd = self._limb.joint_angles()
        joint_names = self._limb.joint_names()

        interp_jas = precalculate_interpolation(p1, q1, p2, q2, duration, last_pos, last_cmd, joint_names)

        i = 0
        self._control_rate.sleep()
        start_time = rospy.get_time()
        t = rospy.get_time()
        while t - start_time < duration:
            lookup_index = min(int(min((t - start_time), duration) / CONTROL_PERIOD), len(interp_jas) - 1)
            self._send_pos_command(interp_jas[lookup_index])
            i += 1
            self._control_rate.sleep()
            t = rospy.get_time()
        logging.getLogger('robot_logger').debug('Effective rate: {} Hz'.format(i / (rospy.get_time() - start_time))) 

def move_to_eep(self, target_pose, duration=1.5):
        """
        :param target_pose: Cartesian pose (x,y,z, quat). 
        :param duration: Total time trajectory will take before ending
        """
        p1, q1 = self.get_xyz_quat()
        p2, q2 = target_pose[:3], target_pose[3:]

        last_pos = self.get_joint_angles()
        last_cmd = self._limb.joint_angles()
        joint_names = self._limb.joint_names()

        interp_jas = precalculate_interpolation(p1, q1, p2, q2, duration, last_pos, last_cmd, joint_names)

        i = 0
        self._control_rate.sleep()
        start_time = rospy.get_time()
        t = rospy.get_time()
        while t - start_time < duration:
            lookup_index = min(int(min((t - start_time), duration) / CONTROL_PERIOD), len(interp_jas) - 1)
            self._send_pos_command(interp_jas[lookup_index])
            i += 1
            self._control_rate.sleep()
            t = rospy.get_time()
        logging.getLogger('robot_logger').debug('Effective rate: {} Hz'.format(i / (rospy.get_time() - start_time))) 

def visualize_stn(data, title):
    input_tensor = data.cpu()
    input_tensor = torch.squeeze(input_tensor)
    input_tensor = input_tensor.detach().numpy()
    N = len(input_tensor)

    fig=plt.figure(num = 2)

    columns = N
    rows = 1
    for i in range(1, columns*rows +1):
        img = input_tensor[i-1]
        fig.add_subplot(rows, columns, i)
        plt.imshow(img, cmap='gray', interpolation='none')

    figManager = plt.get_current_fig_manager()
    figManager.resize(*figManager.window.maxsize())
    # figManager.window.state('zoomed')
    plt.show(block=False)
    # time.sleep(4)
    # plt.close() 

def evaluate_training(self, save_path):
        """
        Evaluates an agent during training. Results are saved in <evaluation_data_path>/evaluation_data/train
        :param agent_name: name of the agent (.pkl)
        """

        while True:
            self.__timestep -= 1

            #Waiting for new task
            while not self.__new_task_started:
                time.sleep(0.001)
            self.__done = False
            self.__new_task_started = False
            result = self.evaluate_episode(True)
            with open('%s.pickle' % (save_path), 'ab') as handle:
                pickle.dump(result, handle, protocol=pickle.HIGHEST_PROTOCOL) 

def __init__(self):
	
        # Create a publisher for our custom message.
        self.pub = rospy.Publisher('from_ros_to_slack', String, queue_size=10)
	rospy.Subscriber("from_slack_to_ros", String, self.callback)

        # Main while loop.
        while not rospy.is_shutdown():
	    # Sleep for a while before publishing new messages. Division is so rate != period.
            rospy.sleep(5.0) 

def __init__(self):
        # Get the ~private namespace parameters from command line or launch file.
        self.token = rospy.get_param('~token', 'xoxp-123456789')
        self.channel = rospy.get_param('~channel', 'G1234567Q')
        self.username = rospy.get_param('~username', 'ros-bot')
	
        # Create a publisher for our custom message.
        pub = rospy.Publisher('from_slack_to_ros', String, queue_size=10)
	rospy.Subscriber("from_ros_to_slack", String, self.callback)
	rospy.Subscriber("send_file_to_slack", String, self.filecallback)

	# Create the slack client
	self.sc = SlackClient(self.token)

	if self.sc.rtm_connect():

            # Main while loop.
            while not rospy.is_shutdown():
                for reply in self.sc.rtm_read():
                    if "type" in reply and "user" in reply:
                        #print reply
                        if reply["type"] == "message" and reply["channel"] == self.channel:
                            pub.publish(reply["text"])
                
	        # Sleep for a while before publishing new messages. Division is so rate != period.
                rospy.sleep(2.0) 

def reset(self):
        self._SerialDataGateway.Reset()
        rospy.sleep(3) 

def start_countdown(seconds):
    def timeout_countdown(seconds):
        time.sleep(seconds)
        rospy.logfatal("COUNTDOWN ERROR RUNTIME TEST")
        os.kill(os.getpid(), signal.SIGUSR1)
        sys.exit(1)
        
    t= Thread(target = lambda: timeout_countdown(seconds))
    t.start()
#---------------------------------------------
# launch countdown 

def move_gripper(limb, action):
    # initialize interfaces
    print("Getting robot state...")
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state()
    gripper = None
    original_deadzone = None
    
    def clean_shutdown():
        if gripper and original_deadzone:
            gripper.set_dead_zone(original_deadzone)
        print("Finished.")
    
    try:
        gripper = intera_interface.Gripper(limb + '_gripper')
    except (ValueError, OSError) as e:
        rospy.logerr("Could not detect an electric gripper attached to the robot.")
        clean_shutdown()
        return
    rospy.on_shutdown(clean_shutdown)

    original_deadzone = gripper.get_dead_zone()
    # WARNING: setting the deadzone below this can cause oscillations in
    # the gripper position. However, setting the deadzone to this
    # value is required to achieve the incremental commands in this example
    gripper.set_dead_zone(0.001)
    rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone()))
    rospy.loginfo("Enabling robot...")
    rs.enable()
    print("Controlling grippers.")
    if (action == "open"):
        gripper.open()
        rospy.sleep(1.0)
        print("Opened grippers")
    elif (action == "close"):
        gripper.close()
        rospy.sleep(1.0)
        print("Closed grippers")
    
    # force shutdown call if caught by key handler
    rospy.signal_shutdown("Example finished.") 

def gripper_open(self):
        """
        :return:
        """
        rospy.logwarn("OPENING GRIPPER")
        self._gripper.open()
        while self._gripper.is_moving() and not rospy.is_shutdown():
            rospy.sleep(0.4) 

def gripper_close(self):
        """
        :return:
        """
        rospy.logwarn("CLOSING GRIPPER")
        self._gripper.close()

        while self._gripper.is_moving() and not rospy.is_shutdown():
            rospy.sleep(0.1) 

def create_go_vision_head_pose_task(self):
        """
        :return:
        """

        self.sawyer_robot.gripper_open()

        self.trajectory_planner.ceilheight = 0.95
        self.trajectory_planner.update_ceiling_obstacle()

        # joint_angles = self.sawyer_robot._limb.joint_angles()

        oldceil = self.trajectory_planner.ceilheight

        joint_angles = self.head_pose_joints

        self.trajectory_planner.ceilheight = 2.0

        # self.sawyer_robot._limb.move_to_joint_positions(joint_angles)
        found = self.safe_goto_joint_position(joint_angles).result()
        # self.delay_task(0.2).result()
        rospy.sleep(0.5)
        self.trajectory_planner.ceilheight = oldceil
        """
        rospy.logwarn(self.sawyer_robot._limb.endpoint_pose())

        targetpose = Pose()
        targetpose.position.x = 0.4595219280890743
        targetpose.position.y = 0.1473752184292072
        targetpose.position.z = 0.019578584407653032

        targetpose.orientation.x = -0.011325648436031916
        targetpose.orientation.y = 0.9998115142702567
        targetpose.orientation.z = -0.006101035043221461
        targetpose.orientation.w = 0.014541079448283218

        found = self.trajectory_planner.move_to_cartesian_target(targetpose)
        """

        if not found:
            self.create_wait_forever_task().result() 

def create_linear_motion_task(self, target_pose, time=4.0, steps=500):
        """ An *incredibly simple* linearly-interpolated Cartesian move """

        r = rospy.Rate(1 / (time / steps))  # Defaults to 100Hz command rate
        current_pose = self.sawyer_robot._limb.endpoint_pose()
        ik_delta = Pose()
        ik_delta.position.x = (current_pose['position'].x - target_pose.position.x) / steps
        ik_delta.position.y = (current_pose['position'].y - target_pose.position.y) / steps
        ik_delta.position.z = (current_pose['position'].z - target_pose.position.z) / steps
        ik_delta.orientation.x = (current_pose['orientation'].x - target_pose.orientation.x) / steps
        ik_delta.orientation.y = (current_pose['orientation'].y - target_pose.orientation.y) / steps
        ik_delta.orientation.z = (current_pose['orientation'].z - target_pose.orientation.z) / steps
        ik_delta.orientation.w = (current_pose['orientation'].w - target_pose.orientation.w) / steps
        for d in range(int(steps), -1, -1):
            if rospy.is_shutdown():
                return
            ik_step = Pose()
            ik_step.position.x = d * ik_delta.position.x + target_pose.position.x
            ik_step.position.y = d * ik_delta.position.y + target_pose.position.y
            ik_step.position.z = d * ik_delta.position.z + target_pose.position.z
            ik_step.orientation.x = d * ik_delta.orientation.x + target_pose.orientation.x
            ik_step.orientation.y = d * ik_delta.orientation.y + target_pose.orientation.y
            ik_step.orientation.z = d * ik_delta.orientation.z + target_pose.orientation.z
            ik_step.orientation.w = d * ik_delta.orientation.w + target_pose.orientation.w
            joint_angles = self.sawyer_robot._limb.ik_request(ik_step, self.sawyer_robot._tip_name)
            if joint_angles:
                self.sawyer_robot._limb.set_joint_positions(joint_angles)
            else:
                rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.")

            r.sleep()
        r.sleep() 

def approach_hover(self, pose, time, hover_distance, approach_speed=0.001):
        """
        :param pose:
        :param time:
        :param approach_speed:
        :return:
        """
        approach_pose = copy.deepcopy(pose)
        rospy.logwarn("approach pose:" + str(approach_pose))
        rospy.logwarn("hover distance:" + str(hover_distance))
        # approach with a pose the hover-distance above the requested pose

        rospy.logwarn("approach prev z :" + str(approach_pose.position.z))
        approach_pose.position.z = approach_pose.position.z + hover_distance
        rospy.logwarn("approach pos z :" + str(approach_pose.position.z))
        # joint_angles = self._limb.ik_request(approach, self._tip_name)

        # self._limb.set_joint_position_speed(0.0001)
        # self._guarded_move_to_joint_position(joint_angles)
        success = self.create_linear_motion_task(approach_pose, time=time).result()

        if not success:
            self.create_wait_forever_task().result()

        rospy.sleep(0.1)
        # self._limb.set_joint_position_speed(0.0001) 

def create_main_loop_task(self):
        """
        This is the main plan of the application
        :return:
        """
        #self.create_go_home_task(check_obstacles=False).result()

        for i in xrange(1000):
            self.create_go_vision_head_pose_task().result()

            blocks = self.create_head_vision_processing_on_table().result()

            if blocks is None:
                self.scheduler_yield()



            self.create_move_all_cubes_to_trays(blocks).result()

            self.robot_say("I FINISHED SORTING THE BLOCKS").result()
            rospy.sleep(3)
            self.robot_say("NOW I WILL PUT THEM BACK ON THE TABLE").result()

            self.pick_all_pieces_from_tray_and_put_on_table().result()

            self.delay_task(1).result()

        self.create_wait_forever_task().result() 

def stop(self):
        """
        :return:
        """
        self.sawyer_robot.disable()
        self.sawyer_robot.enable()

        # self.disable_robot_task().result()
        # self.enable_robot_task().result()

        self.cancel_signal = True

        rospy.logwarn("cancelling all tasks")

        try:
            self.mutex.acquire()
            for task in self.tasks:
                rospy.logwarn("cancelling task: " + task.name)
                task.cancel()
        finally:
            self.mutex.release()

        # wait until all tasks are finished
        while len(self.tasks) > 0 and not rospy.is_shutdown():
            rospy.sleep(0.2)

        self.print_tasks()

        self.cancel_signal = False 

def __init__(self):
        """
        
        """
        self.ceilheight = 0.75
        rospy.sleep(0.4)
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.sleep(0.4)

        self.scene = moveit_commander.PlanningSceneInterface()
        self.robot = moveit_commander.RobotCommander()

        rospy.sleep(0.1)

        self.group = moveit_commander.MoveGroupCommander("right_arm")
        self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                            moveit_msgs.msg.DisplayTrajectory)
        self.planning_scene_diff_publisher = rospy.Publisher("planning_scene", moveit_msgs.msg.PlanningScene,
                                                             queue_size=1)

        rospy.sleep(0.1)

        self.set_default_planner()

        print "============ Reference frame: %s" % self.group.get_planning_frame()

        print "============ Reference frame: %s" % self.group.get_end_effector_link()
        print self.robot.get_group_names()
        print self.robot.get_current_state()
        self.enable_collision_table1 = True
        self.enable_orientation_constraint = False
        self.set_default_tables_z()
        self.registered_blocks = []

        self.tableshape = (0.913, 0.913, 0.01)
        # self.tableshape = (1.2, 1.2, 0.01)

        rospy.sleep(0.2) 

def register_box(self, block):
        if not block in self.registered_blocks:
            block.perception_id = "block"+ str(len(self.registered_blocks))
            self.registered_blocks.append(block)
            rospy.sleep(0.1) 

def set_default_planner(self):
        # self.group.set_planner_id("OMPL")
        #self.group.set_planner_id("RRTConnectkConfigDefault")
        # self.group.set_planner_id("LBKPIECEkConfigDefault")
        self.group.set_planner_id("ESTkConfigDefault")
        # commit
        rospy.sleep(0.2) 

def update_block(self, block):
        rospy.logwarn("updating block collision info")
        if not block in self.registered_blocks:
            raise Exception("block does not exist")

        rospy.logwarn("current blocks: " + str(self.registered_blocks))
        block_pose = geometry_msgs.msg.PoseStamped()
        block_pose.pose = block.grasp_pose
        block_pose.header.stamp = rospy.Time.now()
        block_pose.header.frame_id = self.robot.get_planning_frame()
        block_index = self.registered_blocks.index(block)
        self.scene.add_box("block" + str(block_index), block_pose, size=(0.04, 0.04, 0.04))
        rospy.sleep(0.5)
        return block_index 

def clear_parameters(self):
        """
        
        :return: 
        """
        self.group.clear_pose_targets()
        self.group.clear_path_constraints()
        rospy.sleep(0.5) 

def test_right_hand_ros():
    """
    Test the cube orientation sensing using ROS
    """
    rospy.init_node('cv_detection_right_hand_camera')

    camera_name = "right_hand_camera"

    camera_helper = CameraHelper(camera_name, "base", 0)

    bridge = CvBridge()

    try:
        while not rospy.is_shutdown():
            # Take picture
            img_data = camera_helper.take_single_picture()

            # Convert to OpenCV format
            cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")

            # Save for debugging
            #cv2.imwrite("/tmp/debug.png", cv_image)

            # Get cube rotation
            angles = get_cubes_z_rotation(cv_image)
            print(angles)

            # Wait for a key press
            cv2.waitKey(1)

            rospy.sleep(0.1)
    except CvBridgeError, err:
        rospy.logerr(err)

    # Exit 

def shutdown(self):
        # Stop the robot
        try:
            rospy.loginfo("Stopping the robot...")
            self.cmd_vel_pub.Publish(Twist())
            rospy.sleep(2)
        except:
            pass
        rospy.loginfo("Shutting down Arduino Node...") 

def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('imu', Imu, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      self.last_imu_angle = self.imu_angle
      rospy.sleep(10)
      rospy.loginfo("imu_drift:"+str((self.imu_angle-self.last_imu_angle)*180/3.1415926)) 

def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1) 

def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('odom', Odometry, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      rospy.sleep(0.3)
      rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926)) 

def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('robot_pose_ekf/odom_combined', PoseWithCovarianceStamped, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      rospy.sleep(0.1)
      rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926))