Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21482] pkg/trunk
|
From: <vij...@us...> - 2009-08-10 22:44:59
|
Revision: 21482
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21482&view=rev
Author: vijaypradeep
Date: 2009-08-10 22:44:47 +0000 (Mon, 10 Aug 2009)
Log Message:
-----------
ActionClient GoalHandle refactoring and file reorganization
Modified Paths:
--------------
pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp
pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h
Added Paths:
-----------
pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_goal_handle.ipp
pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_helpers.h
pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.ipp
pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager.ipp
Removed Paths:
-------------
pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.cpp
pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.h
pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_handle.cpp
pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager.cpp
pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager.h
Modified: pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp 2009-08-10 22:43:50 UTC (rev 21481)
+++ pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp 2009-08-10 22:44:47 UTC (rev 21482)
@@ -45,7 +45,7 @@
typedef ActionClient<MoveBaseAction> MoveBaseClient;
-void transitionCallback(GoalHandle<MoveBaseAction> gh)
+void transitionCallback(ActionClient<MoveBaseAction>::GoalHandle gh)
{
ROS_DEBUG("In the transition");
@@ -57,7 +57,7 @@
ROS_DEBUG("NULL Result");
}
-void feedbackCallback(GoalHandle<MoveBaseAction> gh, const MoveBaseFeedbackConstPtr& fb)
+void feedbackCallback(ActionClient<MoveBaseAction>::GoalHandle gh, const MoveBaseFeedbackConstPtr& fb)
{
ROS_INFO("Got Feedback!");
}
@@ -83,7 +83,7 @@
MoveBaseGoal goal;
- GoalHandle<MoveBaseAction> gh = ac.sendGoal(goal, &transitionCallback, &feedbackCallback);
+ ActionClient<MoveBaseAction>::GoalHandle gh = ac.sendGoal(goal, &transitionCallback, &feedbackCallback);
sleep(1.0);
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h 2009-08-10 22:43:50 UTC (rev 21481)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h 2009-08-10 22:44:47 UTC (rev 21482)
@@ -36,7 +36,7 @@
#define ACTIONLIB_ACTION_CLIENT_H_
#include "ros/ros.h"
-#include "actionlib/client/goal_manager.h"
+#include "actionlib/client/client_helpers.h"
namespace actionlib
{
@@ -51,11 +51,14 @@
template <class ActionSpec>
class ActionClient
{
+public:
+ typedef ClientGoalHandle<ActionSpec> GoalHandle;
+
private:
ACTION_DEFINITION(ActionSpec);
typedef ActionClient<ActionSpec> ActionClientT;
- typedef boost::function<void (GoalHandle<ActionSpec>) > TransitionCallback;
- typedef boost::function<void (GoalHandle<ActionSpec>, const FeedbackConstPtr&) > FeedbackCallback;
+ typedef boost::function<void (GoalHandle) > TransitionCallback;
+ typedef boost::function<void (GoalHandle, const FeedbackConstPtr&) > FeedbackCallback;
typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
public:
@@ -88,12 +91,12 @@
* \param transition_cb Callback that gets called on every client state transition
* \param feedback_cb Callback that gets called whenever feedback for this goal is received
*/
- GoalHandle<ActionSpec> sendGoal(const Goal& goal,
- TransitionCallback transition_cb = TransitionCallback(),
- FeedbackCallback feedback_cb = FeedbackCallback())
+ GoalHandle sendGoal(const Goal& goal,
+ TransitionCallback transition_cb = TransitionCallback(),
+ FeedbackCallback feedback_cb = FeedbackCallback())
{
ROS_DEBUG("about to start initGoal()");
- GoalHandle<ActionSpec> gh = manager_.initGoal(goal, transition_cb, feedback_cb);
+ GoalHandle gh = manager_.initGoal(goal, transition_cb, feedback_cb);
ROS_DEBUG("Done with initGoal()");
return gh;
Copied: pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_goal_handle.ipp (from rev 21480, pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_handle.cpp)
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_goal_handle.ipp (rev 0)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_goal_handle.ipp 2009-08-10 22:44:47 UTC (rev 21482)
@@ -0,0 +1,187 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* This file has the template implementation for ClientGoalHandle. It should be included with the
+ * class definition.
+ */
+
+namespace actionlib
+{
+
+template<class ActionSpec>
+ClientGoalHandle<ActionSpec>::ClientGoalHandle()
+{
+ gm_ = NULL;
+ active_ = false;
+}
+
+template<class ActionSpec>
+ClientGoalHandle<ActionSpec>::ClientGoalHandle(GoalManagerT* gm, typename ManagedListT::Handle handle)
+{
+ gm_ = gm;
+ active_ = true;
+ list_handle_ = handle;
+}
+
+template<class ActionSpec>
+void ClientGoalHandle<ActionSpec>::reset()
+{
+ list_handle_.reset();
+ active_ = false;
+ gm_ = NULL;
+}
+
+template<class ActionSpec>
+bool ClientGoalHandle<ActionSpec>::isExpired() const
+{
+ return !active_;
+}
+
+
+template<class ActionSpec>
+CommState ClientGoalHandle<ActionSpec>::getCommState()
+{
+ if (!active_)
+ ROS_ERROR("Trying to getCommState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
+
+ assert(gm_);
+
+ boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
+ return list_handle_.getElem()->getCommState();
+}
+
+template<class ActionSpec>
+TerminalState ClientGoalHandle<ActionSpec>::getTerminalState()
+{
+ if (!active_)
+ ROS_ERROR("Trying to getTerminalState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
+
+ assert(gm_);
+
+ boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
+ CommState comm_state_ = list_handle_.getElem()->getCommState();
+ if (comm_state_ != CommState::DONE)
+ ROS_WARN("Asking for the terminal state when we're in [%s]", comm_state_.toString().c_str());
+
+ GoalStatus goal_status = list_handle_.getElem()->getGoalStatus();
+
+ switch (goal_status.status)
+ {
+ case GoalStatus::PENDING:
+ case GoalStatus::ACTIVE:
+ case GoalStatus::PREEMPTING:
+ case GoalStatus::RECALLING:
+ ROS_ERROR("Asking for terminal state, but latest goal status is %u", goal_status.status); return TerminalState::LOST;
+ case GoalStatus::PREEMPTED: return TerminalState::PREEMPTED;
+ case GoalStatus::SUCCEEDED: return TerminalState::SUCCEEDED;
+ case GoalStatus::ABORTED: return TerminalState::ABORTED;
+ case GoalStatus::REJECTED: return TerminalState::REJECTED;
+ case GoalStatus::RECALLED: return TerminalState::RECALLED;
+ case GoalStatus::LOST: return TerminalState::LOST;
+ default:
+ ROS_ERROR("Unknown goal status: %u", goal_status.status); break;
+ }
+
+ ROS_ERROR("Bug in determining terminal state");
+ return TerminalState::LOST;
+}
+
+template<class ActionSpec>
+typename ClientGoalHandle<ActionSpec>::ResultConstPtr ClientGoalHandle<ActionSpec>::getResult()
+{
+ if (!active_)
+ ROS_ERROR("Trying to getResult on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
+ assert(gm_);
+ boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
+ return list_handle_.getElem()->getResult();
+}
+
+template<class ActionSpec>
+void ClientGoalHandle<ActionSpec>::resend()
+{
+ if (!active_)
+ ROS_ERROR("Trying to resend() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
+ assert(gm_);
+ boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
+
+ ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();
+
+ if (!action_goal)
+ ROS_ERROR("BUG: Got a NULL action_goal");
+
+ if (gm_->send_goal_func_)
+ gm_->send_goal_func_(action_goal);
+}
+
+template<class ActionSpec>
+void ClientGoalHandle<ActionSpec>::cancel()
+{
+ if (!active_)
+ ROS_ERROR("Trying to cancel() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
+ assert(gm_);
+ boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
+
+ ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();
+
+ GoalID cancel_msg;
+ cancel_msg.stamp = ros::Time(0,0);
+ cancel_msg.id = list_handle_.getElem()->getActionGoal()->goal_id.id;
+
+ if (gm_->cancel_func_)
+ gm_->cancel_func_(cancel_msg);
+
+ list_handle_.getElem()->transitionToState(*this, CommState::WAITING_FOR_CANCEL_ACK);
+}
+
+template<class ActionSpec>
+bool ClientGoalHandle<ActionSpec>::operator==(const ClientGoalHandle<ActionSpec>& rhs)
+{
+ // Check if both are inactive
+ if (!active_ && !rhs.active_)
+ return true;
+
+ // Check if one or the other is inactive
+ if (!active_ || !rhs.active_)
+ return false;
+
+ return (list_handle_ == rhs.list_handle_) ;
+}
+
+template<class ActionSpec>
+bool ClientGoalHandle<ActionSpec>::operator!=(const ClientGoalHandle<ActionSpec>& rhs)
+{
+ return !(*this==rhs);
+}
+
+}
Copied: pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_helpers.h (from rev 21480, pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager.h)
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_helpers.h (rev 0)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_helpers.h 2009-08-10 22:44:47 UTC (rev 21482)
@@ -0,0 +1,265 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef ACTIONLIB_GOAL_MANAGER_H_
+#define ACTIONLIB_GOAL_MANAGER_H_
+
+#include <boost/thread/recursive_mutex.hpp>
+#include <boost/interprocess/sync/scoped_lock.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+
+
+#include "actionlib/action_definition.h"
+
+#include "actionlib/managed_list.h"
+#include "actionlib/enclosure_deleter.h"
+#include "actionlib/goal_id_generator.h"
+
+#include "actionlib/client/comm_state.h"
+#include "actionlib/client/terminal_state.h"
+
+// msgs
+#include "actionlib/GoalID.h"
+#include "actionlib/GoalStatusArray.h"
+#include "actionlib/RequestType.h"
+
+namespace actionlib
+{
+
+template <class ActionSpec>
+class ClientGoalHandle;
+
+template <class ActionSpec>
+class CommStateMachine;
+
+template <class ActionSpec>
+class GoalManager
+{
+public:
+ ACTION_DEFINITION(ActionSpec);
+ typedef GoalManager<ActionSpec> GoalManagerT;
+ typedef ClientGoalHandle<ActionSpec> GoalHandleT;
+ typedef boost::function<void (GoalHandleT) > TransitionCallback;
+ typedef boost::function<void (GoalHandleT, const FeedbackConstPtr&) > FeedbackCallback;
+ typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
+ typedef boost::function<void (const GoalID&)> CancelFunc;
+
+ GoalManager() { }
+
+ void registerSendGoalFunc(SendGoalFunc send_goal_func);
+ void registerCancelFunc(CancelFunc cancel_func);
+
+ GoalHandleT initGoal( const Goal& goal,
+ TransitionCallback transition_cb = TransitionCallback(),
+ FeedbackCallback feedback_cb = FeedbackCallback() );
+
+ void updateStatuses(const GoalStatusArrayConstPtr& status_array);
+ void updateFeedbacks(const ActionFeedbackConstPtr& action_feedback);
+ void updateResults(const ActionResultConstPtr& action_result);
+
+ friend class ClientGoalHandle<ActionSpec>;
+
+ // should be private
+ typedef ManagedList< boost::shared_ptr<CommStateMachine<ActionSpec> > > ManagedListT;
+ ManagedListT list_;
+private:
+ SendGoalFunc send_goal_func_ ;
+ CancelFunc cancel_func_ ;
+
+ boost::recursive_mutex list_mutex_;
+
+ GoalIDGenerator id_generator_;
+
+ void listElemDeleter(typename ManagedListT::iterator it);
+};
+
+/**
+ * \brief Client side handle to monitor goal progress
+ *
+ * A ClientGoalHandle is a reference counted object that is used to manipulate and monitor the progress
+ * of an already dispatched goal. Once all the goal handles go out of scope (or are reset), an
+ * ActionClient stops maintaining state for that goal.
+ */
+template <class ActionSpec>
+class ClientGoalHandle
+{
+private:
+ ACTION_DEFINITION(ActionSpec);
+
+public:
+ /**
+ * \brief Create an empty goal handle
+ *
+ * Constructs a goal handle that doesn't track any goal. Calling any method on an empty goal
+ * handle other than operator= will trigger an assertion.
+ */
+ ClientGoalHandle();
+
+ /**
+ * \brief Stops goal handle from tracking a goal
+ *
+ * Useful if you want to stop tracking the progress of a goal, but it is inconvenient to force
+ * the goal handle to go out of scope. Has pretty much the same semantics as boost::shared_ptr::reset()
+ */
+ void reset();
+
+ /**
+ * \brief Checks if this goal handle is tracking a goal
+ *
+ * Has pretty much the same semantics as boost::shared_ptr::expired()
+ * \return True if this goal handle is not tracking a goal
+ */
+ inline bool isExpired() const;
+
+ /**
+ * \brief Get the state of this goal's communication state machine from interaction with the server
+ *
+ * Possible States are: WAITING_FOR_GOAL_ACK, PENDING, ACTIVE, WAITING_FOR_RESULT,
+ * WAITING_FOR_CANCEL_ACK, RECALLING, PREEMPTING, DONE
+ * \return The current goal's communication state with the server
+ */
+ CommState getCommState();
+
+ /**
+ * \brief Get the terminal state information for this goal
+ *
+ * Possible States Are: RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST
+ * This call only makes sense if CommState==DONE. This will send ROS_WARNs if we're not in DONE
+ * \return The terminal state
+ */
+ TerminalState getTerminalState();
+
+ /**
+ * \brief Get result associated with this goal
+ *
+ * \return NULL if no reseult received. Otherwise returns shared_ptr to result.
+ */
+ ResultConstPtr getResult();
+
+ /**
+ * \brief Resends this goal [with the same GoalID] to the ActionServer
+ *
+ * Useful if the user thinks that the goal may have gotten lost in transit
+ */
+ void resend();
+
+ /**
+ * \brief Sends a cancel message for this specific goal to the ActionServer
+ *
+ * Also transitions the Communication State Machine to WAITING_FOR_CANCEL_ACK
+ */
+ void cancel();
+
+ /**
+ * \brief Check if two goal handles point to the same goal
+ * \return TRUE if both point to the same goal. Also returns TRUE if both handles are inactive.
+ */
+ bool operator==(const ClientGoalHandle<ActionSpec>& rhs);
+
+ /**
+ * \brief !(operator==())
+ */
+ bool operator!=(const ClientGoalHandle<ActionSpec>& rhs);
+
+ friend class GoalManager<ActionSpec>;
+private:
+ typedef GoalManager<ActionSpec> GoalManagerT;
+ typedef ManagedList< boost::shared_ptr<CommStateMachine<ActionSpec> > > ManagedListT;
+
+ ClientGoalHandle(GoalManagerT* gm, typename ManagedListT::Handle handle);
+
+ GoalManagerT* gm_;
+ bool active_;
+ //typename ManagedListT::iterator it_;
+ typename ManagedListT::Handle list_handle_;
+};
+
+template <class ActionSpec>
+class CommStateMachine
+{
+ private:
+ //generates typedefs that we'll use to make our lives easier
+ ACTION_DEFINITION(ActionSpec);
+
+ public:
+ typedef boost::function<void (const ClientGoalHandle<ActionSpec>&) > TransitionCallback;
+ typedef boost::function<void (const ClientGoalHandle<ActionSpec>&, const FeedbackConstPtr&) > FeedbackCallback;
+ typedef ClientGoalHandle<ActionSpec> GoalHandleT;
+
+ CommStateMachine(const ActionGoalConstPtr& action_goal,
+ TransitionCallback transition_callback,
+ FeedbackCallback feedback_callback);
+
+ ActionGoalConstPtr getActionGoal() const;
+ CommState getCommState() const;
+ GoalStatus getGoalStatus() const;
+ ResultConstPtr getResult() const;
+
+ // Transitions caused by messages
+ void updateStatus(GoalHandleT& gh, const GoalStatusArrayConstPtr& status_array);
+ void updateFeedback(GoalHandleT& gh, const ActionFeedbackConstPtr& feedback);
+ void updateResult(GoalHandleT& gh, const ActionResultConstPtr& result);
+
+ // Forced transitions
+ void transitionToState(GoalHandleT& gh, const CommState::StateEnum& next_state);
+ void transitionToState(GoalHandleT& gh, const CommState& next_state);
+ void processLost(GoalHandleT& gh);
+ private:
+ CommStateMachine();
+
+ // State
+ CommState state_;
+ ActionGoalConstPtr action_goal_;
+ GoalStatus latest_goal_status_;
+ ActionResultConstPtr latest_result_;
+
+ // Callbacks
+ TransitionCallback transition_cb_;
+ FeedbackCallback feedback_cb_;
+
+ // **** Implementation ****
+ //! Change the state, as well as print out ROS_DEBUG info
+ void setCommState(const CommState& state);
+ void setCommState(const CommState::StateEnum& state);
+ const GoalStatus* findGoalStatus(const std::vector<GoalStatus>& status_vec) const;
+};
+
+}
+
+#include "actionlib/client/goal_manager.ipp"
+#include "actionlib/client/client_goal_handle.ipp"
+#include "actionlib/client/comm_state_machine.ipp"
+
+#endif // ACTIONLIB_GOAL_MANAGER_H_
Deleted: pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.cpp
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.cpp 2009-08-10 22:43:50 UTC (rev 21481)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.cpp 2009-08-10 22:44:47 UTC (rev 21482)
@@ -1,400 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-/* This file has the template implementation for CommStateMachine. It should be included with the
- * class definition.
- */
-
-namespace actionlib
-{
-
-template <class ActionSpec>
-CommStateMachine<ActionSpec>::CommStateMachine(const ActionGoalConstPtr& action_goal,
- TransitionCallback transition_cb,
- FeedbackCallback feedback_cb) : state_(CommState::WAITING_FOR_GOAL_ACK)
-{
- assert(action_goal);
- action_goal_ = action_goal;
- transition_cb_ = transition_cb;
- feedback_cb_ = feedback_cb;
- //transitionToState( CommState::WAITING_FOR_GOAL_ACK );
-}
-
-template <class ActionSpec>
-typename CommStateMachine<ActionSpec>::ActionGoalConstPtr CommStateMachine<ActionSpec>::getActionGoal() const
-{
- return action_goal_;
-}
-
-template <class ActionSpec>
-CommState CommStateMachine<ActionSpec>::getCommState() const
-{
- return state_;
-}
-
-template <class ActionSpec>
-GoalStatus CommStateMachine<ActionSpec>::getGoalStatus() const
-{
- return latest_goal_status_;
-}
-
-template <class ActionSpec>
-typename CommStateMachine<ActionSpec>::ResultConstPtr CommStateMachine<ActionSpec>::getResult() const
-{
- ResultConstPtr result;
- if (latest_result_)
- {
- EnclosureDeleter<const ActionResult> d(latest_result_);
- result = ResultConstPtr(&(latest_result_->result), d);
- }
- return result;
-
-}
-
-template <class ActionSpec>
-void CommStateMachine<ActionSpec>::setCommState(const CommState::StateEnum& state)
-{
- setCommState(CommState(state));
-}
-
-template <class ActionSpec>
-void CommStateMachine<ActionSpec>::setCommState(const CommState& state)
-{
- ROS_DEBUG("Transitioning CommState from %s to %s", state_.toString().c_str(), state.toString().c_str());
- state_ = state;
-}
-
-template <class ActionSpec>
-const GoalStatus* CommStateMachine<ActionSpec>::findGoalStatus(const std::vector<GoalStatus>& status_vec) const
-{
- for (unsigned int i=0; i<status_vec.size(); i++)
- if (status_vec[i].goal_id.id == action_goal_->goal_id.id)
- return &status_vec[i];
- return NULL;
-}
-
-template <class ActionSpec>
-void CommStateMachine<ActionSpec>::updateFeedback(GoalHandleT& gh, const ActionFeedbackConstPtr& action_feedback)
-{
- // Check if this feedback is for us
- if (action_goal_->goal_id.id != action_feedback->status.goal_id.id)
- return;
-
- if (feedback_cb_)
- {
- EnclosureDeleter<const ActionFeedback> d(action_feedback);
- FeedbackConstPtr feedback(&(action_feedback->feedback), d);
- feedback_cb_(gh, feedback);
- }
-}
-
-template <class ActionSpec>
-void CommStateMachine<ActionSpec>::updateResult(GoalHandleT& gh, const ActionResultConstPtr& action_result)
-{
- // Check if this feedback is for us
- if (action_goal_->goal_id.id != action_result->status.goal_id.id)
- return;
- latest_goal_status_ = action_result->status;
- latest_result_ = action_result;
- switch (state_.state_)
- {
- case CommState::WAITING_FOR_GOAL_ACK:
- case CommState::PENDING:
- case CommState::ACTIVE:
- case CommState::WAITING_FOR_RESULT:
- case CommState::RECALLING:
- case CommState::PREEMPTING:
- transitionToState(gh, CommState::DONE);
- break;
- case CommState::DONE:
- ROS_ERROR("Got a result when we were already in the DONE state"); break;
- default:
- ROS_ERROR("In a funny comm state: %u", state_.state_); break;
- }
-}
-
-template <class ActionSpec>
-void CommStateMachine<ActionSpec>::updateStatus(GoalHandleT& gh, const GoalStatusArrayConstPtr& status_array)
-{
- const GoalStatus* goal_status = findGoalStatus(status_array->status_list);
-
- if (goal_status)
- latest_goal_status_ = *goal_status;
- else
- {
- if (state_ != CommState::WAITING_FOR_GOAL_ACK &&
- state_ != CommState::WAITING_FOR_RESULT &&
- state_ != CommState::DONE)
- {
- processLost(gh);
- }
- return;
- }
-
- switch (state_.state_)
- {
- case CommState::WAITING_FOR_GOAL_ACK:
- {
- if (goal_status)
- {
- switch (goal_status->status)
- {
- case GoalStatus::PENDING:
- transitionToState(gh, CommState::PENDING); break;
- case GoalStatus::ACTIVE:
- transitionToState(gh, CommState::ACTIVE); break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
- case GoalStatus::RECALLED:
- transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
- transitionToState(gh, CommState::PREEMPTING);
- case GoalStatus::RECALLING:
- transitionToState(gh, CommState::RECALLING);
- default:
- ROS_ERROR("BUG: Got an unknown status from the ActionServer. status = %u", goal_status->status);
- break;
- }
- }
- break;
- }
- case CommState::PENDING:
- {
- switch (goal_status->status)
- {
- case GoalStatus::PENDING:
- break;
- case GoalStatus::ACTIVE:
- transitionToState(gh, CommState::ACTIVE);
- break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
- case GoalStatus::RECALLED:
- transitionToState(gh, CommState::WAITING_FOR_RESULT);
- break;
- case GoalStatus::PREEMPTING:
- transitionToState(gh, CommState::PREEMPTING);
- break;
- case GoalStatus::RECALLING:
- transitionToState(gh, CommState::RECALLING);
- break;
- default:
- ROS_ERROR("BUG: Got an unknown goal status from the ActionServer. status = %u", goal_status->status);
- break;
- }
- break;
- }
- case CommState::ACTIVE:
- {
- switch (goal_status->status)
- {
- case GoalStatus::PENDING:
- ROS_ERROR("Invalid transition from ACTIVE to PENDING"); break;
- case GoalStatus::ACTIVE:
- break;
- case GoalStatus::REJECTED:
- ROS_ERROR("Invalid transition from ACTIVE to REJECTED"); break;
- case GoalStatus::RECALLING:
- ROS_ERROR("Invalid transition from ACTIVE to RECALLING"); break;
- case GoalStatus::RECALLED:
- ROS_ERROR("Invalid transition from ACTIVE to RECALLED"); break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
- transitionToState(gh, CommState::PREEMPTING); break;
- default:
- ROS_ERROR("BUG: Got an unknown goal status from the ActionServer. status = %u", goal_status->status);
- break;
- }
- break;
- }
- case CommState::WAITING_FOR_RESULT:
- {
- switch (goal_status->status)
- {
- case GoalStatus::PENDING :
- ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to PENDING"); break;
- case GoalStatus::PREEMPTING:
- ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to PREEMPTING"); break;
- case GoalStatus::RECALLING:
- ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to RECALLING"); break;
- case GoalStatus::ACTIVE:
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
- case GoalStatus::RECALLED:
- break;
- default:
- ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
- break;
- }
- break;
- }
- case CommState::WAITING_FOR_CANCEL_ACK:
- {
- switch (goal_status->status)
- {
- case GoalStatus::PENDING:
- break;
- case GoalStatus::ACTIVE:
- break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::RECALLED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
- transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
- transitionToState(gh, CommState::PREEMPTING); break;
- case GoalStatus::RECALLING:
- transitionToState(gh, CommState::RECALLING); break;
- default:
- ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
- break;
- }
- break;
- }
- case CommState::RECALLING:
- {
- switch (goal_status->status)
- {
- case GoalStatus::PENDING:
- ROS_ERROR("Invalid Transition from RECALLING to PENDING"); break;
- case GoalStatus::ACTIVE:
- ROS_ERROR("Invalid Transition from RECALLING to ACTIVE"); break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::RECALLED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
- transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
- transitionToState(gh, CommState::PREEMPTING); break;
- case GoalStatus::RECALLING:
- break;
- default:
- ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
- break;
- }
- break;
- }
- case CommState::PREEMPTING:
- {
- switch (goal_status->status)
- {
- case GoalStatus::PENDING:
- ROS_ERROR("Invalid Transition from PREEMPTING to PENDING"); break;
- case GoalStatus::ACTIVE:
- ROS_ERROR("Invalid Transition from PREEMPTING to ACTIVE"); break;
- case GoalStatus::REJECTED:
- ROS_ERROR("Invalid Transition from PREEMPTING to REJECTED"); break;
- case GoalStatus::RECALLING:
- ROS_ERROR("Invalid Transition from PREEMPTING to RECALLING"); break;
- case GoalStatus::RECALLED:
- ROS_ERROR("Invalid Transition from PREEMPTING to RECALLED"); break;
- break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
- break;
- default:
- ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
- break;
- }
- break;
- }
- case CommState::DONE:
- {
- switch (goal_status->status)
- {
- case GoalStatus::PENDING:
- ROS_ERROR("Invalid Transition from DONE to PENDING"); break;
- case GoalStatus::ACTIVE:
- ROS_ERROR("Invalid Transition from DONE to ACTIVE"); break;
- case GoalStatus::RECALLING:
- ROS_ERROR("Invalid Transition from DONE to RECALLING"); break;
- case GoalStatus::PREEMPTING:
- ROS_ERROR("Invalid Transition from DONE to PREEMPTING"); break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::RECALLED:
- case GoalStatus::REJECTED:
- break;
- default:
- ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
- break;
- }
- break;
- }
- default:
- ROS_ERROR("In a funny comm state: %u", state_.state_);
- break;
- }
-}
-
-
-template <class ActionSpec>
-void CommStateMachine<ActionSpec>::processLost(GoalHandleT& gh)
-{
- ROS_DEBUG("Processing LOST");
- latest_goal_status_.status = GoalStatus::LOST;
- transitionToState(gh, CommState::DONE);
-}
-
-template <class ActionSpec>
-void CommStateMachine<ActionSpec>::transitionToState(GoalHandleT& gh, const CommState::StateEnum& next_state)
-{
- transitionToState(gh, CommState(next_state));
-}
-
-template <class ActionSpec>
-void CommStateMachine<ActionSpec>::transitionToState(GoalHandleT& gh, const CommState& next_state)
-{
- ROS_DEBUG("Trying to transition to %s", next_state.toString().c_str());
- setCommState(next_state);
- if (transition_cb_)
- transition_cb_(gh);
-}
-
-}
Deleted: pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.h 2009-08-10 22:43:50 UTC (rev 21481)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.h 2009-08-10 22:44:47 UTC (rev 21482)
@@ -1,47 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef ACTIONLIB_COMM_STATE_MACHINE_H_
-#define ACTIONLIB_COMM_STATE_MACHINE_H_
-
-#include "ros/console.h"
-#include "actionlib/client/comm_state.h"
-
-namespace actionlib
-{
-
-};
-
-}
-#endif
Copied: pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.ipp (from rev 21480, pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.cpp)
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.ipp (rev 0)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine.ipp 2009-08-10 22:44:47 UTC (rev 21482)
@@ -0,0 +1,400 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* This file has the template implementation for CommStateMachine. It should be included with the
+ * class definition.
+ */
+
+namespace actionlib
+{
+
+template <class ActionSpec>
+CommStateMachine<ActionSpec>::CommStateMachine(const ActionGoalConstPtr& action_goal,
+ TransitionCallback transition_cb,
+ FeedbackCallback feedback_cb) : state_(CommState::WAITING_FOR_GOAL_ACK)
+{
+ assert(action_goal);
+ action_goal_ = action_goal;
+ transition_cb_ = transition_cb;
+ feedback_cb_ = feedback_cb;
+ //transitionToState( CommState::WAITING_FOR_GOAL_ACK );
+}
+
+template <class ActionSpec>
+typename CommStateMachine<ActionSpec>::ActionGoalConstPtr CommStateMachine<ActionSpec>::getActionGoal() const
+{
+ return action_goal_;
+}
+
+template <class ActionSpec>
+CommState CommStateMachine<ActionSpec>::getCommState() const
+{
+ return state_;
+}
+
+template <class ActionSpec>
+GoalStatus CommStateMachine<ActionSpec>::getGoalStatus() const
+{
+ return latest_goal_status_;
+}
+
+template <class ActionSpec>
+typename CommStateMachine<ActionSpec>::ResultConstPtr CommStateMachine<ActionSpec>::getResult() const
+{
+ ResultConstPtr result;
+ if (latest_result_)
+ {
+ EnclosureDeleter<const ActionResult> d(latest_result_);
+ result = ResultConstPtr(&(latest_result_->result), d);
+ }
+ return result;
+
+}
+
+template <class ActionSpec>
+void CommStateMachine<ActionSpec>::setCommState(const CommState::StateEnum& state)
+{
+ setCommState(CommState(state));
+}
+
+template <class ActionSpec>
+void CommStateMachine<ActionSpec>::setCommState(const CommState& state)
+{
+ ROS_DEBUG("Transitioning CommState from %s to %s", state_.toString().c_str(), state.toString().c_str());
+ state_ = state;
+}
+
+template <class ActionSpec>
+const GoalStatus* CommStateMachine<ActionSpec>::findGoalStatus(const std::vector<GoalStatus>& status_vec) const
+{
+ for (unsigned int i=0; i<status_vec.size(); i++)
+ if (status_vec[i].goal_id.id == action_goal_->goal_id.id)
+ return &status_vec[i];
+ return NULL;
+}
+
+template <class ActionSpec>
+void CommStateMachine<ActionSpec>::updateFeedback(GoalHandleT& gh, const ActionFeedbackConstPtr& action_feedback)
+{
+ // Check if this feedback is for us
+ if (action_goal_->goal_id.id != action_feedback->status.goal_id.id)
+ return;
+
+ if (feedback_cb_)
+ {
+ EnclosureDeleter<const ActionFeedback> d(action_feedback);
+ FeedbackConstPtr feedback(&(action_feedback->feedback), d);
+ feedback_cb_(gh, feedback);
+ }
+}
+
+template <class ActionSpec>
+void CommStateMachine<ActionSpec>::updateResult(GoalHandleT& gh, const ActionResultConstPtr& action_result)
+{
+ // Check if this feedback is for us
+ if (action_goal_->goal_id.id != action_result->status.goal_id.id)
+ return;
+ latest_goal_status_ = action_result->status;
+ latest_result_ = action_result;
+ switch (state_.state_)
+ {
+ case CommState::WAITING_FOR_GOAL_ACK:
+ case CommState::PENDING:
+ case CommState::ACTIVE:
+ case CommState::WAITING_FOR_RESULT:
+ case CommState::RECALLING:
+ case CommState::PREEMPTING:
+ transitionToState(gh, CommState::DONE);
+ break;
+ case CommState::DONE:
+ ROS_ERROR("Got a result when we were already in the DONE state"); break;
+ default:
+ ROS_ERROR("In a funny comm state: %u", state_.state_); break;
+ }
+}
+
+template <class ActionSpec>
+void CommStateMachine<ActionSpec>::updateStatus(GoalHandleT& gh, const GoalStatusArrayConstPtr& status_array)
+{
+ const GoalStatus* goal_status = findGoalStatus(status_array->status_list);
+
+ if (goal_status)
+ latest_goal_status_ = *goal_status;
+ else
+ {
+ if (state_ != CommState::WAITING_FOR_GOAL_ACK &&
+ state_ != CommState::WAITING_FOR_RESULT &&
+ state_ != CommState::DONE)
+ {
+ processLost(gh);
+ }
+ return;
+ }
+
+ switch (state_.state_)
+ {
+ case CommState::WAITING_FOR_GOAL_ACK:
+ {
+ if (goal_status)
+ {
+ switch (goal_status->status)
+ {
+ case GoalStatus::PENDING:
+ transitionToState(gh, CommState::PENDING); break;
+ case GoalStatus::ACTIVE:
+ transitionToState(gh, CommState::ACTIVE); break;
+ case GoalStatus::PREEMPTED:
+ case GoalStatus::SUCCEEDED:
+ case GoalStatus::ABORTED:
+ case GoalStatus::REJECTED:
+ case GoalStatus::RECALLED:
+ transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
+ case GoalStatus::PREEMPTING:
+ transitionToState(gh, CommState::PREEMPTING);
+ case GoalStatus::RECALLING:
+ transitionToState(gh, CommState::RECALLING);
+ default:
+ ROS_ERROR("BUG: Got an unknown status from the ActionServer. status = %u", goal_status->status);
+ break;
+ }
+ }
+ break;
+ }
+ case CommState::PENDING:
+ {
+ switch (goal_status->status)
+ {
+ case GoalStatus::PENDING:
+ break;
+ case GoalStatus::ACTIVE:
+ transitionToState(gh, CommState::ACTIVE);
+ break;
+ case GoalStatus::PREEMPTED:
+ case GoalStatus::SUCCEEDED:
+ case GoalStatus::ABORTED:
+ case GoalStatus::REJECTED:
+ case GoalStatus::RECALLED:
+ transitionToState(gh, CommState::WAITING_FOR_RESULT);
+ break;
+ case GoalStatus::PREEMPTING:
+ transitionToState(gh, CommState::PREEMPTING);
+ break;
+ case GoalStatus::RECALLING:
+ transitionToState(gh, CommState::RECALLING);
+ break;
+ default:
+ ROS_ERROR("BUG: Got an unknown goal status from the ActionServer. status = %u", goal_status->status);
+ break;
+ }
+ break;
+ }
+ case CommState::ACTIVE:
+ {
+ switch (goal_status->status)
+ {
+ case GoalStatus::PENDING:
+ ROS_ERROR("Invalid transition from ACTIVE to PENDING"); break;
+ case GoalStatus::ACTIVE:
+ break;
+ case GoalStatus::REJECTED:
+ ROS_ERROR("Invalid transition from ACTIVE to REJECTED"); break;
+ case GoalStatus::RECALLING:
+ ROS_ERROR("Invalid transition from ACTIVE to RECALLING"); break;
+ case GoalStatus::RECALLED:
+ ROS_ERROR("Invalid transition from ACTIVE to RECALLED"); break;
+ case GoalStatus::PREEMPTED:
+ case GoalStatus::SUCCEEDED:
+ case GoalStatus::ABORTED:
+ transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
+ case GoalStatus::PREEMPTING:
+ transitionToState(gh, CommState::PREEMPTING); break;
+ default:
+ ROS_ERROR("BUG: Got an unknown goal status from the ActionServer. status = %u", goal_status->status);
+ break;
+ }
+ break;
+ }
+ case CommState::WAITING_FOR_RESULT:
+ {
+ switch (goal_status->status)
+ {
+ case GoalStatus::PENDING :
+ ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to PENDING"); break;
+ case GoalStatus::PREEMPTING:
+ ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to PREEMPTING"); break;
+ case GoalStatus::RECALLING:
+ ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to RECALLING"); break;
+ case GoalStatus::ACTIVE:
+ case GoalStatus::PREEMPTED:
+ case GoalStatus::SUCCEEDED:
+ case GoalStatus::ABORTED:
+ case GoalStatus::REJECTED:
+ case GoalStatus::RECALLED:
+ break;
+ default:
+ ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
+ break;
+ }
+ break;
+ }
+ case CommState::WAITING_FOR_CANCEL_ACK:
+ {
+ switch (goal_status->status)
+ {
+ case GoalStatus::PENDING:
+ break;
+ case GoalStatus::ACTIVE:
+ break;
+ case GoalStatus::PREEMPTED:
+ case GoalStatus::RECALLED:
+ case GoalStatus::SUCCEEDED:
+ case GoalStatus::ABORTED:
+ case GoalStatus::REJECTED:
+ transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
+ case GoalStatus::PREEMPTING:
+ transitionToState(gh, CommState::PREEMPTING); break;
+ case GoalStatus::RECALLING:
+ transitionToState(gh, CommState::RECALLING); break;
+ default:
+ ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
+ break;
+ }
+ break;
+ }
+ case CommState::RECALLING:
+ {
+ switch (goal_status->status)
+ {
+ case GoalStatus::PENDING:
+ ROS_ERROR("Invalid Transition from RECALLING to PENDING"); break;
+ case GoalStatus::ACTIVE:
+ ROS_ERROR("Invalid Transition from RECALLING to ACTIVE"); break;
+ case GoalStatus::PREEMPTED:
+ case GoalStatus::RECALLED:
+ case GoalStatus::SUCCEEDED:
+ case GoalStatus::ABORTED:
+ case GoalStatus::REJECTED:
+ transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
+ case GoalStatus::PREEMPTING:
+ transitionToState(gh, CommState::PREEMPTING); break;
+ case GoalStatus::RECALLING:
+ break;
+ default:
+ ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
+ break;
+ }
+ break;
+ }
+ case CommState::PREEMPTING:
+ {
+ switch (goal_status->status)
+ {
+ case GoalStatus::PENDING:
+ ROS_ERROR("Invalid Transition from PREEMPTING to PENDING"); break;
+ case GoalStatus::ACTIVE:
+ ROS_ERROR("Invalid Transition from PREEMPTING to ACTIVE"); break;
+ case GoalStatus::REJECTED:
+ ROS_ERROR("Invalid Transition from PREEMPTING to REJECTED"); break;
+ case GoalStatus::RECALLING:
+ ROS_ERROR("Invalid Transition from PREEMPTING to RECALLING"); break;
+ case GoalStatus::RECALLED:
+ ROS_ERROR("Invalid Transition from PREEMPTING to RECALLED"); break;
+ break;
+ case GoalStatus::PREEMPTED:
+ case GoalStatus::SUCCEEDED:
+ case GoalStatus::ABORTED:
+ transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
+ case GoalStatus::PREEMPTING:
+ break;
+ default:
+ ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
+ break;
+ }
+ break;
+ }
+ case CommState::DONE:
+ {
+ switch (goal_status->status)
+ {
+ case GoalStatus::PENDING:
+ ROS_ERROR("Invalid Transition from DONE to PENDING"); break;
+ case GoalStatus::ACTIVE:
+ ROS_ERROR("Invalid Transition from DONE to ACTIVE"); break;
+ case GoalStatus::RECALLING:
+ ROS_ERROR("Invalid Transition from DONE to RECALLING"); break;
+ case GoalStatus::PREEMPTING:
+ ROS_ERROR("Invalid Transition from DONE to PREEMPTING"); break;
+ case GoalStatus::PREEMPTED:
+ case GoalStatus::SUCCEEDED:
+ case GoalStatus::ABORTED:
+ case GoalStatus::RECALLED:
+ case GoalStatus::REJECTED:
+ break;
+ default:
+ ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
+ break;
+ }
+ break;
+ }
+ default:
+ ROS_ERROR("In a funny comm state: %u", state_.state_);
+ break;
+ }
+}
+
+
+template <class ActionSpec>
+void CommStateMachine<ActionSpec>::processLost(GoalHandleT& gh)
+{
+ ROS_DEBUG("Processing LOST");
+ latest_goal_status_.status = GoalStatus::LOST;
+ transitionToState(gh, CommState::DONE);
+}
+
+template <class ActionSpec>
+void CommStateMachine<ActionSpec>::transitionToState(GoalHandleT& gh, const CommState::StateEnum& next_state)
+{
+ transitionToState(gh, CommState(next_state));
+}
+
+template <class ActionSpec>
+void CommStateMachine<ActionSpec>::transitionToState(GoalHandleT& gh, const CommState& next_state)
+{
+ ROS_DEBUG("Trying to transition to %s", next_state.toString().c_str());
+ setCommState(next_state);
+ if (transition_cb_)
+ transition_cb_(gh);
+}
+
+}
Deleted: pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_handle.cpp
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_handle.cpp 2009-08-10 22:43:50 UTC (rev 21481)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_handle.cpp 2009-08-10 22:44:47 UTC (rev 21482)
@@ -1,187 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-/* This file has the template implementation for GoalHandle. It should be included with the
- * class definition.
- */
-
-namespace actionlib
-{
-
-template<class ActionSpec>
-GoalHandle<ActionSpec>::GoalHandle()
-{
- gm_ = NULL;
- active_ = false;
-}
-
-template<class ActionSpec>
-GoalHandle<ActionSpec>::GoalHandle(GoalManagerT* gm, typename ManagedListT::Handle handle)
-{
- gm_ = gm;
- active_ = true;
- list_handle_ = handle;
-}
-
-template<class ActionSpec>
-void GoalHandle<ActionSpec>::reset()
-{
- list_handle_.reset();
- active_ = false;
- gm_ = NULL;
-}
-
-template<class ActionSpec>
-bool GoalHandle<ActionSpec>::isExpired() const
-{
- return !active_;
-}
-
-
-template<class ActionSpec>
-CommState GoalHandle<ActionSpec>::getCommState()
-{
- if (!active_)
- ROS_ERROR("Trying to getCommState on an inactive GoalHandle. You are incorrectly using a GoalHandle");
-
- assert(gm_);
-
- boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
- return list_handle_.getElem()->getCommState();
-}
-
-template<class ActionSpec>
-TerminalState GoalHandle<ActionSpec>::getTerminalState()
-{
- if (!active_)
- ROS_ERROR("Trying to getTerminalState on an inactive GoalHandle. You are incorrectly using a GoalHandle");
-
- assert(gm_);
-
- boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
- CommState comm_state_ = list_handle_.getElem()->getCommState();
- if (comm_state_ != CommState::DONE)
- ROS_WARN("Asking for the terminal state when we're in [%s]", comm_state_.toString().c_str());
-
- GoalStatus goal_status = list_handle_.getElem()->getGoalStatus();
-
- switch (goal_status.status)
- {
- case GoalStatus::PENDING:
- case GoalStatus::ACTIVE:
- case GoalStatus::PREEMPTING:
- case GoalStatus::RECALLING:
- ROS_ERROR("Asking for terminal state, but latest goal status is %u", goal_status.status); return TerminalState::LOST;
- case GoalStatus::PREEMPTED: return TerminalState::PREEMPTED;
- case GoalStatus::SUCCEEDED: return TerminalState::SUCCEEDED;
- case GoalStatus::ABORTED: return TerminalState::ABORTED;
- case GoalStatus::REJECTED: return TerminalState::REJECTED;
- case GoalStatus::RECALLED: return TerminalState::RECALLED;
- case GoalStatus::LOST: return TerminalState::LOST;
- default:
- ROS_ERROR("Unknown goal status: %u", goal_status.status); break;
- }
-
- ROS_ERROR("Bug in determining terminal state");
- return TerminalState::LOST;
-}
-
-template<class ActionSpec>
-typename GoalHandle<ActionSpec>::ResultConstPtr GoalHandle<ActionSpec>::getResult()
-{
- if (!active_)
- ROS_ERROR("Trying to getResult on an inactive GoalHandle. You are incorrectly using a GoalHandle");
- assert(gm_);
- boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
- return list_handle_.getElem()->getResult();
-}
-
-template<class ActionSpec>
-void GoalHandle<ActionSpec>::resend()
-{
- if (!active_)
- ROS_ERROR("Trying to resend() on an inactive GoalHandle. You are incorrectly using a GoalHandle");
- assert(gm_);
- boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
-
- ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();
-
- if (!action_goal)
- ROS_ERROR("BUG: Got a NULL action_goal");
-
- if (gm_->send_goal_func_)
- gm_->send_goal_func_(action_goal);
-}
-
-template<class ActionSpec>
-void GoalHandle<ActionSpec>::cancel()
-{
- if (!active_)
- ROS_ERROR("Trying to cancel() on an inactive GoalHandle. You are incorrectly using a GoalHandle");
- assert(gm_);
- boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
-
- ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();
-
- GoalID cancel_msg;
- cancel_msg.stamp = ros::Time(0,0);
- cancel_msg.id = list_handle_.getElem()->getActionGoal()->goal_id.id;
-
- if (gm_->cancel_func_)
- gm_->cancel_func_(cancel_msg);
-
- list_handle_.getElem()->transitionToState(*this, CommState::WAITING_FOR_CANCEL_ACK);
-}
-
-template<class ActionSpec>
-bool GoalHandle<ActionSpec>::operator==(const GoalHandle<ActionSpec>& rhs)
-{
- // Check if both are inactive
- if (!active_ && !rhs.active_)
- return true;
-
- // Check if one or the other is inactive
- if (!active_ || !rhs.active_)
- return false;
-
- return (list_handle_ == rhs.list_handle_) ;
-}
-
-template<class ActionSpec>
-bool GoalHandle<ActionSpec>::operator!=(const GoalHandle<ActionSpec>& rhs)
-{
- return !(*this==rhs);
-}
-
-}
Deleted: pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager.cpp
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager.cpp 2009-08-10 22:43:50 UTC (rev 21481)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager.cpp 2009-08-10 22:44:47 UTC (rev 21482)
@@ -1,130 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willo...
[truncated message content] |
