def explore(self):
        print 'explore started '
        rate = rospy.Rate(30)
        self.newGoal = True
        if self.isExploring == False:
            #Change this later when we have a better exploration
            #self.isExploring = True
            while self.done == False:
                    time.sleep(1)
                    rospy.loginfo("Climb")
                    self.progress += 0.1
                    self.takeoff(5)
                    self.progress += 0.1
                    rospy.loginfo("Moving to Red_Object")
                    self.reached_object = False
                    red_object_id = -1
                    xspeed = 1
                    while self.reached_object == False:
                        self.client.send_goal(self.goal)
                        self.client.wait_for_result()
                        self.objects = self.client.get_result().tracked_objects.objects
                        islost = True
                        for obj in self.objects:
                            if red_object_id == -1 and (obj.color == 'RED' or obj.color == 'BLUE' or obj.color == 'GREEN'): #pick any nearby object
                                red_object_id = obj.object_id
                            if obj.object_id == red_object_id:
                                islost = False
                                print 'Moving to Drop zone', self.currentPoseX-obj.pose2.pose.position.x, self.currentPoseY-obj.pose2.pose.position.y, obj.pose.pose.position.x, obj.pose.pose.position.y
                                if fabs(obj.pose2.pose.position.x) < 0.01 and fabs(obj.pose2.pose.position.y) > 0.01:
                                    print 'Moving Y'
                                    self.setPose(self.x, self.currentPoseY+obj.pose2.pose.position.y*xspeed, self.z, 0 , False)
                                elif fabs(obj.pose2.pose.position.y) < 0.01 and fabs(obj.pose2.pose.position.x) > 0.01:
                                    print 'Moving X'
                                    self.setPose(self.currentPoseX-obj.pose2.pose.position.x*xspeed, self.y, self.z, 0 , False)
                                else:
                                    print 'Moving XY'
                                    self.setPose(self.currentPoseX-obj.pose2.pose.position.x*xspeed, self.currentPoseY+obj.pose2.pose.position.y*xspeed, self.z, 0 , True)
                                if fabs(obj.pose2.pose.position.x) < 0.3 and fabs(obj.pose2.pose.position.y) < 0.3 and self.z > 0.0:
                                    print 'Moving Z'
                                    land = 0.2
                                    if self.z <= 3:
                                        xspeed = 1
                                    if self.z <= 1.5:
                                        xspeed = 0.5
                                    if self.z < 0.5:
                                        land = 0.05
                                    self.setPose(self.x, self.y, self.z - land * xspeed, 1, False)
                                    if self.z <= 0.4:
                                        self.reached_object = True
                        if islost == True:
                            red_object_id = -1
                        if red_object_id == -1:
                            rospy.loginfo("No object in sight, exploring")
                            #self.setPose(self.x, self.y - 5, self.z, 1, True)
                        rate.sleep()   
                    time.sleep(10)                    
                    rospy.loginfo("Picked Object, climb")
                    self.takeoff(1)
                    self.takeoff(2)
                    self.takeoff(3)
                    self.takeoff(4)
                    self.takeoff(5)
                    #self.setPose(self.x, self.y, self.z)
                    time.sleep(10)
                    rospy.loginfo("Moving to DropZone")
                    self.setPose(1, -21, 5) ##Go near dropzone
                    self.progress += 0.1
                    self.reached_dropzone = False
                    xspeed = 3
                    while self.reached_dropzone == False:
                        self.client.send_goal(self.goal)
                        self.client.wait_for_result()
                        self.objects = self.client.get_result().tracked_objects.objects