class QueuedActionClient:
    ## @brief Constructs a QueuedActionClient and opens connections to an ActionServer.
    ##
    ## @param ns The namespace in which to access the action.  For
    ## example, the "goal" topic should occur under ns/goal
    ##
    ## @param ActionSpec The *Action message type.  The QueuedActionClient
    ## will grab the other message types from this type.
    def __init__(self, ns, ActionSpec):
        self.action_client = ActionClient(ns, ActionSpec)
        self.simple_state = SimpleGoalState.DONE
        self.gh = None
        self.done_condition = threading.Condition()

    ## @brief Blocks until the action server connects to this client
    ##
    ## @param timeout Max time to block before returning. A zero
    ## timeout is interpreted as an infinite timeout.
    ##
    ## @return True if the server connected in the allocated time. False on timeout
    def wait_for_server(self, timeout = rospy.Duration()):
        return self.action_client.wait_for_server(timeout)


    ## @brief Sends a goal to the ActionServer, and also registers callbacks
    ##
    ## If a previous goal is already active when this is called. We simply forget
    ## about that goal and start tracking the new goal. No cancel requests are made.
    ##
    ## @param done_cb Callback that gets called on transitions to
    ## Done.  The callback should take two parameters: the terminal
    ## state (as an integer from actionlib_msgs/GoalStatus) and the
    ## result.
    ##
    ## @param active_cb   No-parameter callback that gets called on transitions to Active.
    ##
    ## @param feedback_cb Callback that gets called whenever feedback
    ## for this goal is received.  Takes one parameter: the feedback.
    def send_goal(self, goal, done_cb = None, active_cb = None, feedback_cb = None):
        # destroys the old goal handle
        self.stop_tracking_goal()

        self.done_cb = done_cb
        self.active_cb = active_cb
        self.feedback_cb = feedback_cb

        self.simple_state = SimpleGoalState.PENDING
        self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)


    ## @brief Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary
    ##
    ## If a previous goal is already active when this is called. We simply forget
    ## about that goal and start tracking the new goal. No cancel requests are made.
    ##
    ## If the goal does not complete within the execute_timeout, the goal gets preempted
    ##
    ## If preemption of the goal does not complete withing the preempt_timeout, this
    ## method simply returns
    ##
    ## @param execute_timeout The time to wait for the goal to complete
    ##
    ## @param preempt_timeout The time to wait for preemption to complete
    ##
    ## @return The goal's state.
    def send_goal_and_wait(self, goal, execute_timeout = rospy.Duration(), preempt_timeout = rospy.Duration()):
        self.send_goal(goal)
        if not self.wait_for_result(execute_timeout):
            # preempt action
            rospy.logdebug("Canceling goal")
            self.cancel_goal()
            if self.wait_for_result(preempt_timeout):
                rospy.logdebug("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec()) 
            else:
                rospy.logdebug("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec()) 
        return self.get_state()


    ## @brief Blocks until this goal transitions to done
    ## @param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
    ## @return True if the goal finished. False if the goal didn't finish within the allocated timeout
    def wait_for_result(self, timeout = rospy.Duration()):
        if not self.gh:
            rospy.logerr("Called wait_for_goal_to_finish when no goal exists")
            return False

        timeout_time = rospy.get_rostime() + timeout
        loop_period = rospy.Duration(0.1)
        with self.done_condition:
            while not rospy.is_shutdown():
                time_left = timeout_time - rospy.get_rostime()
                if timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(0.0):
                    break

                if self.simple_state == SimpleGoalState.DONE:
                    break

                if time_left > loop_period or timeout == rospy.Duration():
                    time_left = loop_period

                self.done_condition.wait(time_left.to_sec())

        return self.simple_state == SimpleGoalState.DONE


    ## @brief Gets the Result of the current goal
    def get_result(self):
        if not self.gh:
            rospy.logerr("Called get_result when no goal is running")
            return None

        return self.gh.get_result()


    ## @brief Get the state information for this goal
    ##
    ## Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED,
    ## PREEMPTED, ABORTED, SUCCEEDED, LOST.
    ##
    ## @return The goal's state. Returns LOST if this
    ## QueuedActionClient isn't tracking a goal.
    def get_state(self):
        if not self.gh:
            rospy.logerr("Called get_state when no goal is running")
            return GoalStatus.LOST
        status = self.gh.get_goal_status()

        if status == GoalStatus.RECALLING:
            status = GoalStatus.PENDING
        elif status == GoalStatus.PREEMPTING:
            status = GoalStatus.ACTIVE

        return status


    ## @brief Returns the current status text of the goal.
    ##
    ## The text is sent by the action server. It is designed to
    ## help debugging issues on the server side.
    ##
    ## @return The current status text of the goal.
    def get_goal_status_text(self):
        if not self.gh:
            rospy.logerr("Called get_goal_status_text when no goal is running")
            return "ERROR: Called get_goal_status_text when no goal is running"

        return self.gh.get_goal_status_text()




    ## @brief Cancels all goals currently running on the action server
    ##
    ## This preempts all goals running on the action server at the point that
    ## this message is serviced by the ActionServer.
    def cancel_all_goals(self):
        self.action_client.cancel_all_goals()


    ## @brief Cancels all goals prior to a given timestamp
    ##
    ## This preempts all goals running on the action server for which the 
    ## time stamp is earlier than the specified time stamp
    ## this message is serviced by the ActionServer.

    def cancel_goals_at_and_before_time(self, time):
        self.action_client.cancel_goals_at_and_before_time(time)


    ## @brief Cancels the goal that we are currently pursuing
    def cancel_goal(self):
        if self.gh:
            self.gh.cancel()


    ## @brief Stops tracking the state of the current goal. Unregisters this goal's callbacks
    ##
    ## This is useful if we want to make sure we stop calling our callbacks before sending a new goal.
    ## Note that this does not cancel the goal, it simply stops looking for status info about this goal.
    def stop_tracking_goal(self):
        self.gh = None

    def _handle_transition(self, gh):
        comm_state = gh.get_comm_state()

        error_msg = "Received comm state %s when in simple state %s with QueuedActionClient in NS %s" % \
            (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state), rospy.resolve_name(self.action_client.ns))

        if comm_state == CommState.ACTIVE:
            if self.simple_state == SimpleGoalState.PENDING:
                self._set_simple_state(SimpleGoalState.ACTIVE)
                if self.active_cb:
                    self.active_cb()
            elif self.simple_state == SimpleGoalState.DONE:
                rospy.logerr(error_msg)
        elif comm_state == CommState.RECALLING:
            if self.simple_state != SimpleGoalState.PENDING:
                rospy.logerr(error_msg)
        elif comm_state == CommState.PREEMPTING:
            if self.simple_state == SimpleGoalState.PENDING:
                self._set_simple_state(SimpleGoalState.ACTIVE)
                if self.active_cb:
                    self.active_cb()
            elif self.simple_state == SimpleGoalState.DONE:
                rospy.logerr(error_msg)
        elif comm_state == CommState.DONE:
            if self.simple_state in [SimpleGoalState.PENDING, SimpleGoalState.ACTIVE]:
                self._set_simple_state(SimpleGoalState.DONE)
                if self.done_cb:
                    self.done_cb(gh.get_goal_status(), gh.get_result())
                with self.done_condition:
                    self.done_condition.notifyAll()
            elif self.simple_state == SimpleGoalState.DONE:
                rospy.logerr("QueuedActionClient received DONE twice")

    def _handle_feedback(self, gh, feedback):
        if not self.gh:
            rospy.logerr("Got a feedback callback when we're not tracking a goal. (id: %s)" % \
                             gh.comm_state_machine.action_goal.goal_id.id)
            return
        if gh != self.gh:
            rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" % \
                             (self.gh.comm_state_machine.action_goal.goal_id.id,
                              gh.comm_state_machine.action_goal.goal_id.id))
            return
        if self.feedback_cb:
            self.feedback_cb(feedback)


    def _set_simple_state(self, state):
        self.simple_state = state
class SimpleActionClient:
    ## @brief Constructs a SimpleActionClient and opens connections to an ActionServer.
    ##
    ## @param ns The namespace in which to access the action.  For
    ## example, the "goal" topic should occur under ns/goal
    ##
    ## @param ActionSpec The *Action message type.  The SimpleActionClient
    ## will grab the other message types from this type.
    def __init__(self, ns, ActionSpec):
        self.action_client = ActionClient(ns, ActionSpec)
        self.simple_state = SimpleGoalState.DONE
        self.gh = None
        self.done_condition = threading.Condition()

    ## @brief [Deprecated] Use wait_for_server
    def wait_for_action_server_to_start(self, timeout=rospy.Duration()):
        return self.wait_for_server(timeout)

    ## @brief Blocks until the action server connects to this client
    ##
    ## @param timeout Max time to block before returning. A zero
    ## timeout is interpreted as an infinite timeout.
    ##
    ## @return True if the server connected in the allocated time. False on timeout
    def wait_for_server(self, timeout=rospy.Duration()):
        return self.action_client.wait_for_server(timeout)

    ## @brief Sends a goal to the ActionServer, and also registers callbacks
    ##
    ## If a previous goal is already active when this is called. We simply forget
    ## about that goal and start tracking the new goal. No cancel requests are made.
    ##
    ## @param done_cb Callback that gets called on transitions to
    ## Done.  The callback should take two parameters: the terminal
    ## state (as an integer from actionlib_msgs/GoalStatus) and the
    ## result.
    ##
    ## @param active_cb   No-parameter callback that gets called on transitions to Active.
    ##
    ## @param feedback_cb Callback that gets called whenever feedback
    ## for this goal is received.  Takes one parameter: the feedback.
    def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None):
        # destroys the old goal handle
        self.stop_tracking_goal()

        self.done_cb = done_cb
        self.active_cb = active_cb
        self.feedback_cb = feedback_cb

        self.simple_state = SimpleGoalState.PENDING
        self.gh = self.action_client.send_goal(goal, self._handle_transition,
                                               self._handle_feedback)

    ## @brief [Deprecated] Use wait_for_result
    def wait_for_goal_to_finish(self, timeout=rospy.Duration()):
        return self.wait_for_result(timeout)

    ## @brief Blocks until this goal transitions to done
    ## @param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
    ## @return True if the goal finished. False if the goal didn't finish within the allocated timeout
    def wait_for_result(self, timeout=rospy.Duration()):
        if not self.gh:
            rospy.logerr("Called wait_for_goal_to_finish when no goal exists")
            return False

        timeout_time = rospy.get_rostime() + timeout
        loop_period = rospy.Duration(0.1)
        self.done_condition.acquire()
        while not rospy.is_shutdown():
            time_left = timeout_time - rospy.get_rostime()
            if timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(
                    0.0):
                break

            if self.simple_state == SimpleGoalState.DONE:
                break

            if time_left > loop_period:
                time_left = loop_period

            self.done_condition.wait(time_left.to_seconds())
        self.done_condition.release()

        return self.simple_state == SimpleGoalState.DONE

    ## @brief [Deprecated] Gets the current state of the goal: [PENDING], [ACTIVE], or [DONE]
    ##
    ## Deprecated.  Use get_state instead
    def get_goal_state(self):
        rospy.logwarn("SimpleActionClient.get_goal_state has been deprecated")
        if not self.gh:
            rospy.logerr("Called get_goal_state when no goal is running")
            return SimpleGoalState.DONE

        comm_state = self.gh.get_comm_state()
        if comm_state in [
                CommState.WAITING_FOR_GOAL_ACK, CommState.PENDING,
                CommState.RECALLING
        ]:
            return SimpleGoalState.PENDING
        elif comm_state in [CommState.ACTIVE, CommState.PREEMPTING]:
            return SimpleGoalState.ACTIVE
        elif comm_state in [CommState.DONE]:
            return SimpleGoalState.DONE
        elif comm_state in [
                CommState.WAITING_FOR_RESULT, CommState.WAITING_FOR_CANCEL_ACK
        ]:
            return self.simple_state

        rospy.logerr(
            "Error trying to interpret CommState in get_goal_state: %i" %
            comm_state)
        return SimpleGoalState.DONE

    ## @brief Gets the Result of the current goal
    def get_result(self):
        if not self.gh:
            rospy.logerr("Called get_result when no goal is running")
            return None

        return self.gh.get_result()

    ## @brief [Deprecated] Get the terminal state information for this goal
    ##
    ## Deprecated.  Use get_state instead.
    ##
    ## Possible States Are: RECALLED, REJECTED, PREEMPTED, ABORTED,
    ## SUCCEEDED, LOST.  This call only makes sense if
    ## SimpleGoalState==DONE. This will sends ROS_WARNs if we're not
    ## in DONE
    ##
    ## @return The terminal state
    def get_terminal_state(self):
        rospy.logwarn(
            "SimpleactionClient.get_terminal_state has been deprecated")
        if not self.gh:
            rospy.logerr("Called get_terminal_state when no goal is running")
            return GoalStatus.LOST
        return self.gh.get_terminal_state()

    ## @brief Get the state information for this goal
    ##
    ## Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED,
    ## PREEMPTED, ABORTED, SUCCEEDED, LOST.
    ##
    ## @return The goal's state. Returns LOST if this
    ## SimpleActionClient isn't tracking a goal.
    def get_state(self):
        if not self.gh:
            rospy.logerr("Called get_state when no goal is running")
            return GoalStatus.LOST
        status = gh.get_goal_status()

        if status == GoalStatus.RECALLING:
            status = GoalStatus.PENDING
        elif status == GoalStatus.PREEMPTING:
            status = GoalStatus.ACTIVE

        return status

    ## @brief Cancels all goals currently running on the action server
    ##
    ## This preempts all goals running on the action server at the point that
    ## this message is serviced by the ActionServer.
    def cancel_all_goals(self):
        self.action_client.cancel_all_goals()

    ## @brief Cancels the goal that we are currently pursuing
    def cancel_goal(self):
        if self.gh:
            self.gh.cancel()

    ## @brief Stops tracking the state of the current goal. Unregisters this goal's callbacks
    ##
    ## This is useful if we want to make sure we stop calling our callbacks before sending a new goal.
    ## Note that this does not cancel the goal, it simply stops looking for status info about this goal.
    def stop_tracking_goal(self):
        del self.gh
        self.gh = None

    def _handle_transition(self, gh):
        comm_state = self.gh.get_comm_state()

        error_msg = "Received comm state %s when in simple state %s" % \
            (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state))

        if comm_state == CommState.ACTIVE:
            if self.simple_state == SimpleGoalState.PENDING:
                self._set_simple_state(SimpleGoalState.ACTIVE)
                if self.active_cb:
                    self.active_cb()
            elif self.simple_state == SimpleGoalState.DONE:
                rospy.logerr(error_msg)
        elif comm_state == CommState.RECALLING:
            if self.simple_state != SimpleGoalState.PENDING:
                rospy.logerr(error_msg)
        elif comm_state == CommState.PREEMPTING:
            if self.simple_state == SimpleGoalState.PENDING:
                self._set_simple_state(SimpleGoalState.ACTIVE)
                if self.active_cb:
                    self.active_cb()
            elif self.simple_state == SimpleGoalState.DONE:
                rospy.logerr(error_msg)
        elif comm_state == CommState.DONE:
            if self.simple_state in [
                    SimpleGoalState.PENDING, SimpleGoalState.ACTIVE
            ]:
                self._set_simple_state(SimpleGoalState.DONE)
                if self.done_cb:
                    self.done_cb(self.gh.get_goal_status(),
                                 self.gh.get_result())
                self.done_condition.acquire()
                self.done_condition.notifyAll()
                self.done_condition.release()
            elif self.simple_state == SimpleGoalState.DONE:
                rospy.logerr("SimpleActionClient received DONE twice")

    def _handle_feedback(self, gh, feedback):
        if gh != self.gh:
            rospy.logerr(
                "Got a feedback callback on a goal handle that we're not tracking. %s vs %s"
                % (self.gh.comm_state_machine.action_goal.goal_id.id,
                   gh.comm_state_machine.action_goal.goal_id.id))
            return
        if self.feedback_cb:
            self.feedback_cb(feedback)

    def _set_simple_state(self, state):
        self.simple_state = state