diff --git a/include/actionlib/client/simple_action_client.h b/include/actionlib/client/simple_action_client.h index dfe2f70a..1058295e 100644 --- a/include/actionlib/client/simple_action_client.h +++ b/include/actionlib/client/simple_action_client.h @@ -1,36 +1,36 @@ /********************************************************************* -* 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. -*********************************************************************/ + * 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__CLIENT__SIMPLE_ACTION_CLIENT_H_ #define ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_ @@ -61,431 +61,569 @@ namespace actionlib { -/** - * \brief A Simple client implementation of the ActionInterface which supports only one goal at a time - * - * The SimpleActionClient wraps the exisitng ActionClient, and exposes a limited set of easy-to-use hooks - * for the user. Note that the concept of GoalHandles has been completely hidden from the user, and that - * they must query the SimplyActionClient directly in order to monitor a goal. - */ -template -class SimpleActionClient -{ -private: - ACTION_DEFINITION(ActionSpec); - typedef ClientGoalHandle GoalHandleT; - typedef SimpleActionClient SimpleActionClientT; - -public: - typedef boost::function SimpleDoneCallback; - typedef boost::function SimpleActiveCallback; - typedef boost::function SimpleFeedbackCallback; - - /** - * \brief Simple constructor - * - * Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface - * \param name The action name. Defines the namespace in which the action communicates - * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false, - * then the user has to call ros::spin() themselves. Defaults to True - */ - SimpleActionClient(const std::string & name, bool spin_thread = true) - : cur_simple_state_(SimpleGoalState::PENDING) - { - initSimpleClient(nh_, name, spin_thread); - } - - /** - * \brief Constructor with namespacing options - * - * Constructs a SingleGoalActionClient and sets up the necessary ros topics for - * the ActionInterface, and namespaces them according the a specified NodeHandle - * \param n The node handle on top of which we want to namespace our action - * \param name The action name. Defines the namespace in which the action communicates - * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false, - * then the user has to call ros::spin() themselves. Defaults to True - */ - SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true) - : cur_simple_state_(SimpleGoalState::PENDING) - { - initSimpleClient(n, name, spin_thread); - } - - ~SimpleActionClient(); - - /** - * \brief Waits for the ActionServer to connect to this client - * - * Often, it can take a second for the action server & client to negotiate - * a connection, thus, risking the first few goals to be dropped. This call lets - * the user wait until the network connection to the server is negotiated. - * NOTE: Using this call in a single threaded ROS application, or any - * application where the action client's callback queue is not being - * serviced, will not work. Without a separate thread servicing the queue, or - * a multi-threaded spinner, there is no way for the client to tell whether - * or not the server is up because it can't receive a status message. - * \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 - */ - bool waitForServer(const ros::Duration & timeout = ros::Duration(0, 0) ) const - { - return ac_->waitForActionServerToStart(timeout); - } - - /** - * @brief Checks if the action client is successfully connected to the action server - * @return True if the server is connected, false otherwise - */ - bool isServerConnected() const - { - return ac_->isServerConnected(); - } - - /** - * \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 - * \param active_cb Callback that gets called on transitions to Active - * \param feedback_cb Callback that gets called whenever feedback for this goal is received - */ - void sendGoal(const Goal & goal, - SimpleDoneCallback done_cb = SimpleDoneCallback(), - SimpleActiveCallback active_cb = SimpleActiveCallback(), - SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback()); - - /** - * \brief Sends a goal to the ActionServer, and waits until the goal completes or a timeout is exceeded - * - * If the goal doesn't complete by the execute_timeout, then a preempt message is sent. This call - * then waits up to the preempt_timeout for the goal to then finish. - * - * \param goal The goal to be sent to the ActionServer - * \param execute_timeout Time to wait until a preempt is sent. 0 implies wait forever - * \param preempt_timeout Time to wait after a preempt is sent. 0 implies wait forever - * \return The state of the goal when this call is completed - */ - SimpleClientGoalState sendGoalAndWait(const Goal & goal, - const ros::Duration & execute_timeout = ros::Duration(0, 0), - const ros::Duration & preempt_timeout = ros::Duration(0, 0)); - - /** - * \brief Blocks until this goal finishes - * \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 - */ - bool waitForResult(const ros::Duration & timeout = ros::Duration(0, 0)); - - /** - * \brief Get the Result of the current goal - * \return shared pointer to the result. Note that this pointer will NEVER be NULL - */ - ResultConstPtr getResult() const; - - /** - * \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. - */ - SimpleClientGoalState getState() const; - /** - * \brief Cancel all goals currently running on the action server + * \brief A Simple client implementation of the ActionInterface which supports only one goal at a time * - * This preempts all goals running on the action server at the point that - * this message is serviced by the ActionServer. + * The SimpleActionClient wraps the exisitng ActionClient, and exposes a limited set of easy-to-use hooks + * for the user. Note that the concept of GoalHandles has been completely hidden from the user, and that + * they must query the SimplyActionClient directly in order to monitor a goal. */ - void cancelAllGoals(); - - /** - * \brief Cancel all goals that were stamped at and before the specified time - * \param time All goals stamped at or before `time` will be canceled - */ - void cancelGoalsAtAndBeforeTime(const ros::Time & time); + template + class SimpleActionClient + { + private: + ACTION_DEFINITION(ActionSpec); + typedef ClientGoalHandle GoalHandleT; + typedef SimpleActionClient SimpleActionClientT; + + public: + typedef boost::function SimpleDoneCallback; + typedef boost::function SimpleActiveCallback; + typedef boost::function SimpleFeedbackCallback; + + /** + * \brief Default constructor + * + * Doesn't do anything - should call SimpleActionClient::initialize(...). + */ + SimpleActionClient() + : cur_simple_state_(SimpleGoalState::PENDING) + { + } + + /** + * \brief Simple constructor + * + * Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface + * \param name The action name. Defines the namespace in which the action communicates + * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false, + * then the user has to call ros::spin() themselves. Defaults to True + */ + SimpleActionClient(const std::string & name, bool spin_thread = true) + : cur_simple_state_(SimpleGoalState::PENDING) + { + initSimpleClient(nh_, name, spin_thread); + } + + /** + * \brief Simple constructor + * + * Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface + * \param name The action name. Defines the namespace in which the action communicates + * \param cbq The callback queue we should use for this ActionClient. + */ + SimpleActionClient(std::string name, ros::CallbackQueue *cbq) + : cur_simple_state_(SimpleGoalState::PENDING), + spin_thread_(NULL) + { + ac_.reset(new ActionClientT(nh_, name, cbq)); + } + + /** + * \brief Simple constructor + * + * Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface + * \param n The node handle on top of which we want to namespace our action + * \param name The action name. Defines the namespace in which the action communicates + * \param cbq The callback queue we should use for this ActionClient. + */ + SimpleActionClient(ros::NodeHandle & n, const std::string & name, ros::CallbackQueue *cbq) + : cur_simple_state_(SimpleGoalState::PENDING), + spin_thread_(NULL) + { + ac_.reset(new ActionClientT(nh_, name, cbq)); + } + + /** + * \brief Constructor with namespacing options + * + * Constructs a SingleGoalActionClient and sets up the necessary ros topics for + * the ActionInterface, and namespaces them according the a specified NodeHandle + * \param n The node handle on top of which we want to namespace our action + * \param name The action name. Defines the namespace in which the action communicates + * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false, + * then the user has to call ros::spin() themselves. Defaults to True + */ + SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true) + : cur_simple_state_(SimpleGoalState::PENDING) + { + initSimpleClient(n, name, spin_thread); + } + + ~SimpleActionClient(); + + /** + * \brief + * + * Sets up the necessary ros topics for + * the ActionInterface, and namespaces them according the a specified NodeHandle + * \param n The node handle on top of which we want to namespace our action + * \param name The action name. Defines the namespace in which the action communicates + * \param cbq The callback queue we should use for this ActionClient. + */ + void initialize(ros::NodeHandle & n, const std::string & name, ros::CallbackQueue *cbq) + { + ac_.reset(new ActionClientT(n, name, cbq)); + } - /** - * \brief Cancel the goal that we are currently pursuing - */ - void cancelGoal(); + /** + * \brief + * + * Sets up the necessary ros topics for + * the ActionInterface, and namespaces them according the a specified NodeHandle + * \param n The node handle on top of which we want to namespace our action + * \param name The action name. Defines the namespace in which the action communicates + * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false, + * then the user has to call ros::spin() themselves. Defaults to True + */ + void initialize(ros::NodeHandle & n, const std::string & name, bool spin_thread = true) + { + initSimpleClient(n, name, spin_thread); + } - /** - * \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. - */ - void stopTrackingGoal(); + /** + * \brief + * + * Sets up the necessary ros topics for + * the ActionInterface, and namespaces them according the a specified NodeHandle + * \param name The action name. Defines the namespace in which the action communicates + * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false, + * then the user has to call ros::spin() themselves. Defaults to True + */ + void initialize(const std::string & name, bool spin_thread = true) + { + initSimpleClient(nh_, name, spin_thread); + } -private: - typedef ActionClient ActionClientT; - ros::NodeHandle nh_; - GoalHandleT gh_; + /** + * \brief Waits for the ActionServer to connect to this client + * + * Often, it can take a second for the action server & client to negotiate + * a connection, thus, risking the first few goals to be dropped. This call lets + * the user wait until the network connection to the server is negotiated. + * NOTE: Using this call in a single threaded ROS application, or any + * application where the action client's callback queue is not being + * serviced, will not work. Without a separate thread servicing the queue, or + * a multi-threaded spinner, there is no way for the client to tell whether + * or not the server is up because it can't receive a status message. + * \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 + */ + bool waitForServer(const ros::Duration & timeout = ros::Duration(0, 0) ) const + { + return ac_->waitForActionServerToStart(timeout); + } - SimpleGoalState cur_simple_state_; + /** + * @brief Checks if the action client is successfully connected to the action server + * @return True if the server is connected, false otherwise + */ + bool isServerConnected() const + { + return ac_->isServerConnected(); + } - // Signalling Stuff - boost::condition done_condition_; - boost::mutex done_mutex_; + /** + * @brief Allows users to register a callback to be invoked on transitions to Done + * @param cb The callback to be invoked + */ + void registerDoneCallback(SimpleDoneCallback cb); + + /** + * @brief Allows users to register a callback to be invoked on transitions to Active + * @param cb The callback to be invoked + */ + void registerActiveCallback(SimpleActiveCallback cb); + + /** + * @brief Allows users to register a callback to be invoked whenever feedback is received + * @param cb The callback to be invoked + */ + void registerFeedbackCallback(SimpleFeedbackCallback cb); + + /** + * \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. + * If the callbacks are not provided, we use the callbacks that have been registered. + * If the callbacks are provided, we update the registered callbacks. + * \param done_cb Callback that gets called on transitions to Done + * \param active_cb Callback that gets called on transitions to Active + * \param feedback_cb Callback that gets called whenever feedback for this goal is received + */ + void sendGoal(const Goal & goal, + SimpleDoneCallback done_cb = SimpleDoneCallback(), + SimpleActiveCallback active_cb = SimpleActiveCallback(), + SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback()); + + /** + * \brief Sends a goal to the ActionServer, and waits until the goal completes or a timeout is exceeded + * + * If the goal doesn't complete by the execute_timeout, then a preempt message is sent. This call + * then waits up to the preempt_timeout for the goal to then finish. + * + * \param goal The goal to be sent to the ActionServer + * \param execute_timeout Time to wait until a preempt is sent. 0 implies wait forever + * \param preempt_timeout Time to wait after a preempt is sent. 0 implies wait forever + * \return The state of the goal when this call is completed + */ + SimpleClientGoalState sendGoalAndWait(const Goal & goal, + const ros::Duration & execute_timeout = ros::Duration(0, 0), + const ros::Duration & preempt_timeout = ros::Duration(0, 0)); + + /** + * \brief Blocks until this goal finishes + * \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 + */ + bool waitForResult(const ros::Duration & timeout = ros::Duration(0, 0)); + + /** + * \brief Get the Result of the current goal + * \return shared pointer to the result. Note that this pointer will NEVER be NULL + */ + ResultConstPtr getResult() const; + + /** + * \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. + */ + SimpleClientGoalState getState() const; + + /** + * \brief Cancel 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. + */ + void cancelAllGoals(); + + /** + * \brief Cancel all goals that were stamped at and before the specified time + * \param time All goals stamped at or before `time` will be canceled + */ + void cancelGoalsAtAndBeforeTime(const ros::Time & time); + + /** + * \brief Cancel the goal that we are currently pursuing + */ + void cancelGoal(); + + /** + * \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. + */ + void stopTrackingGoal(); + + private: + typedef ActionClient ActionClientT; + ros::NodeHandle nh_; + GoalHandleT gh_; + + SimpleGoalState cur_simple_state_; + + // Signalling Stuff + boost::condition done_condition_; + boost::mutex done_mutex_; + + // User Callbacks + SimpleDoneCallback done_cb_; + SimpleActiveCallback active_cb_; + SimpleFeedbackCallback feedback_cb_; + + // Spin Thread Stuff + boost::mutex terminate_mutex_; + bool need_to_terminate_; + boost::thread * spin_thread_; + ros::CallbackQueue callback_queue; + + boost::scoped_ptr ac_; // Action client depends on callback_queue, so it must be destroyed before callback_queue + + // ***** Private Funcs ***** + void initSimpleClient(ros::NodeHandle & n, const std::string & name, bool spin_thread); + void handleTransition(GoalHandleT gh); + void handleFeedback(GoalHandleT gh, const FeedbackConstPtr & feedback); + void setSimpleState(const SimpleGoalState::StateEnum & next_state); + void setSimpleState(const SimpleGoalState & next_state); + void spinThread(); + }; + + + template + void SimpleActionClient::initSimpleClient(ros::NodeHandle & n, const std::string & name, + bool spin_thread) + { + if (spin_thread) { + ROS_DEBUG_NAMED("actionlib", "Spinning up a thread for the SimpleActionClient"); + need_to_terminate_ = false; + spin_thread_ = + new boost::thread(boost::bind(&SimpleActionClient::spinThread, this)); + ac_.reset(new ActionClientT(n, name, &callback_queue)); + } else { + spin_thread_ = NULL; + ac_.reset(new ActionClientT(n, name)); + } + } - // User Callbacks - SimpleDoneCallback done_cb_; - SimpleActiveCallback active_cb_; - SimpleFeedbackCallback feedback_cb_; + template + SimpleActionClient::~SimpleActionClient() + { + if (spin_thread_) { + { + boost::mutex::scoped_lock terminate_lock(terminate_mutex_); + need_to_terminate_ = true; + } + spin_thread_->join(); + delete spin_thread_; + } + gh_.reset(); + ac_.reset(); + } - // Spin Thread Stuff - boost::mutex terminate_mutex_; - bool need_to_terminate_; - boost::thread * spin_thread_; - ros::CallbackQueue callback_queue; + template + void SimpleActionClient::spinThread() + { + while (nh_.ok()) { + { + boost::mutex::scoped_lock terminate_lock(terminate_mutex_); + if (need_to_terminate_) { + break; + } + } + callback_queue.callAvailable(ros::WallDuration(0.1f)); + } + } - boost::scoped_ptr ac_; // Action client depends on callback_queue, so it must be destroyed before callback_queue + template + void SimpleActionClient::setSimpleState(const SimpleGoalState::StateEnum & next_state) + { + setSimpleState(SimpleGoalState(next_state) ); + } - // ***** Private Funcs ***** - void initSimpleClient(ros::NodeHandle & n, const std::string & name, bool spin_thread); - void handleTransition(GoalHandleT gh); - void handleFeedback(GoalHandleT gh, const FeedbackConstPtr & feedback); - void setSimpleState(const SimpleGoalState::StateEnum & next_state); - void setSimpleState(const SimpleGoalState & next_state); - void spinThread(); -}; + template + void SimpleActionClient::setSimpleState(const SimpleGoalState & next_state) + { + ROS_DEBUG_NAMED("actionlib", "Transitioning SimpleState from [%s] to [%s]", + cur_simple_state_.toString().c_str(), + next_state.toString().c_str()); + cur_simple_state_ = next_state; + } + template + void SimpleActionClient::registerDoneCallback(SimpleDoneCallback cb) + { + if (done_cb_) { + ROS_WARN_NAMED("actionlib", + "SimpleActionClient: A doneCallback already exists, overwriting it!"); + } + done_cb_ = cb; + } -template -void SimpleActionClient::initSimpleClient(ros::NodeHandle & n, const std::string & name, - bool spin_thread) -{ - if (spin_thread) { - ROS_DEBUG_NAMED("actionlib", "Spinning up a thread for the SimpleActionClient"); - need_to_terminate_ = false; - spin_thread_ = - new boost::thread(boost::bind(&SimpleActionClient::spinThread, this)); - ac_.reset(new ActionClientT(n, name, &callback_queue)); - } else { - spin_thread_ = NULL; - ac_.reset(new ActionClientT(n, name)); - } -} - -template -SimpleActionClient::~SimpleActionClient() -{ - if (spin_thread_) { + template + void SimpleActionClient::registerActiveCallback(SimpleActiveCallback cb) { - boost::mutex::scoped_lock terminate_lock(terminate_mutex_); - need_to_terminate_ = true; + if (active_cb_) { + ROS_WARN_NAMED("actionlib", + "SimpleActionClient: A activeCallback already exists, overwriting it!"); + } + active_cb_ = cb; } - spin_thread_->join(); - delete spin_thread_; - } - gh_.reset(); - ac_.reset(); -} - -template -void SimpleActionClient::spinThread() -{ - while (nh_.ok()) { + + template + void SimpleActionClient::registerFeedbackCallback(SimpleFeedbackCallback cb) { - boost::mutex::scoped_lock terminate_lock(terminate_mutex_); - if (need_to_terminate_) { - break; + if (feedback_cb_) { + ROS_WARN_NAMED("actionlib", + "SimpleActionClient: A feedbackCallback already exists, overwriting it!"); } + feedback_cb_ = cb; } - callback_queue.callAvailable(ros::WallDuration(0.1f)); - } -} -template -void SimpleActionClient::setSimpleState(const SimpleGoalState::StateEnum & next_state) -{ - setSimpleState(SimpleGoalState(next_state) ); -} + template + void SimpleActionClient::sendGoal(const Goal & goal, + SimpleDoneCallback done_cb, + SimpleActiveCallback active_cb, + SimpleFeedbackCallback feedback_cb) + { + // Reset the old GoalHandle, so that our callbacks won't get called anymore + gh_.reset(); -template -void SimpleActionClient::setSimpleState(const SimpleGoalState & next_state) -{ - ROS_DEBUG_NAMED("actionlib", "Transitioning SimpleState from [%s] to [%s]", - cur_simple_state_.toString().c_str(), - next_state.toString().c_str()); - cur_simple_state_ = next_state; -} - -template -void SimpleActionClient::sendGoal(const Goal & goal, - SimpleDoneCallback done_cb, - SimpleActiveCallback active_cb, - SimpleFeedbackCallback feedback_cb) -{ - // Reset the old GoalHandle, so that our callbacks won't get called anymore - gh_.reset(); + // Store all the callbacks if our stored cb is empty or we get a new cb + if (done_cb_.empty() || !done_cb.empty()) { + done_cb_ = done_cb; + } + if (active_cb_.empty() || !active_cb.empty()) { + active_cb_ = active_cb; + } + if (feedback_cb_.empty() || !feedback_cb.empty()) { + feedback_cb_ = feedback_cb; + } - // Store all the callbacks - done_cb_ = done_cb; - active_cb_ = active_cb; - feedback_cb_ = feedback_cb; + cur_simple_state_ = SimpleGoalState::PENDING; - cur_simple_state_ = SimpleGoalState::PENDING; + // Send the goal to the ActionServer + gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1), + boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2)); + } - // Send the goal to the ActionServer - gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1), - boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2)); -} + template + SimpleClientGoalState SimpleActionClient::getState() const + { + if (gh_.isExpired()) { + ROS_ERROR_NAMED("actionlib", + "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient"); + return SimpleClientGoalState(SimpleClientGoalState::LOST); + } -template -SimpleClientGoalState SimpleActionClient::getState() const -{ - if (gh_.isExpired()) { - ROS_ERROR_NAMED("actionlib", - "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient"); - return SimpleClientGoalState(SimpleClientGoalState::LOST); - } - - CommState comm_state_ = gh_.getCommState(); - - switch (comm_state_.state_) { - case CommState::WAITING_FOR_GOAL_ACK: - case CommState::PENDING: - case CommState::RECALLING: - return SimpleClientGoalState(SimpleClientGoalState::PENDING); - case CommState::ACTIVE: - case CommState::PREEMPTING: - return SimpleClientGoalState(SimpleClientGoalState::ACTIVE); - case CommState::DONE: - { - switch (gh_.getTerminalState().state_) { + CommState comm_state_ = gh_.getCommState(); + + switch (comm_state_.state_) { + case CommState::WAITING_FOR_GOAL_ACK: + case CommState::PENDING: + case CommState::RECALLING: + return SimpleClientGoalState(SimpleClientGoalState::PENDING); + case CommState::ACTIVE: + case CommState::PREEMPTING: + return SimpleClientGoalState(SimpleClientGoalState::ACTIVE); + case CommState::DONE: + { + switch (gh_.getTerminalState().state_) { case TerminalState::RECALLED: return SimpleClientGoalState(SimpleClientGoalState::RECALLED, - gh_.getTerminalState().text_); + gh_.getTerminalState().text_); case TerminalState::REJECTED: return SimpleClientGoalState(SimpleClientGoalState::REJECTED, - gh_.getTerminalState().text_); + gh_.getTerminalState().text_); case TerminalState::PREEMPTED: return SimpleClientGoalState(SimpleClientGoalState::PREEMPTED, - gh_.getTerminalState().text_); + gh_.getTerminalState().text_); case TerminalState::ABORTED: return SimpleClientGoalState(SimpleClientGoalState::ABORTED, - gh_.getTerminalState().text_); + gh_.getTerminalState().text_); case TerminalState::SUCCEEDED: return SimpleClientGoalState(SimpleClientGoalState::SUCCEEDED, - gh_.getTerminalState().text_); + gh_.getTerminalState().text_); case TerminalState::LOST: return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_); default: ROS_ERROR_NAMED("actionlib", - "Unknown terminal state [%u]. This is a bug in SimpleActionClient", - gh_.getTerminalState().state_); + "Unknown terminal state [%u]. This is a bug in SimpleActionClient", + gh_.getTerminalState().state_); return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_); - } - } - case CommState::WAITING_FOR_RESULT: - case CommState::WAITING_FOR_CANCEL_ACK: - { - switch (cur_simple_state_.state_) { + } + } + case CommState::WAITING_FOR_RESULT: + case CommState::WAITING_FOR_CANCEL_ACK: + { + switch (cur_simple_state_.state_) { case SimpleGoalState::PENDING: return SimpleClientGoalState(SimpleClientGoalState::PENDING); case SimpleGoalState::ACTIVE: return SimpleClientGoalState(SimpleClientGoalState::ACTIVE); case SimpleGoalState::DONE: ROS_ERROR_NAMED("actionlib", - "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in SimpleGoalState DONE. This is a bug in SimpleActionClient"); + "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in SimpleGoalState DONE. This is a bug in SimpleActionClient"); return SimpleClientGoalState(SimpleClientGoalState::LOST); default: ROS_ERROR_NAMED("actionlib", - "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient", - cur_simple_state_.state_); - } + "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient", + cur_simple_state_.state_); + } + } + default: + break; } - default: - break; - } - ROS_ERROR_NAMED("actionlib", "Error trying to interpret CommState - %u", comm_state_.state_); - return SimpleClientGoalState(SimpleClientGoalState::LOST); -} - -template -typename SimpleActionClient::ResultConstPtr SimpleActionClient::getResult() -const -{ - if (gh_.isExpired()) { - ROS_ERROR_NAMED("actionlib", - "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient"); - } + ROS_ERROR_NAMED("actionlib", "Error trying to interpret CommState - %u", comm_state_.state_); + return SimpleClientGoalState(SimpleClientGoalState::LOST); + } - if (gh_.getResult()) { - return gh_.getResult(); - } + template + typename SimpleActionClient::ResultConstPtr SimpleActionClient::getResult() + const + { + if (gh_.isExpired()) { + ROS_ERROR_NAMED("actionlib", + "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient"); + } - return ResultConstPtr(new Result); -} + if (gh_.getResult()) { + return gh_.getResult(); + } + return ResultConstPtr(new Result); + } -template -void SimpleActionClient::cancelAllGoals() -{ - ac_->cancelAllGoals(); -} -template -void SimpleActionClient::cancelGoalsAtAndBeforeTime(const ros::Time & time) -{ - ac_->cancelGoalsAtAndBeforeTime(time); -} + template + void SimpleActionClient::cancelAllGoals() + { + ac_->cancelAllGoals(); + } -template -void SimpleActionClient::cancelGoal() -{ - if (gh_.isExpired()) { - ROS_ERROR_NAMED("actionlib", - "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient"); - } + template + void SimpleActionClient::cancelGoalsAtAndBeforeTime(const ros::Time & time) + { + ac_->cancelGoalsAtAndBeforeTime(time); + } - gh_.cancel(); -} + template + void SimpleActionClient::cancelGoal() + { + if (gh_.isExpired()) { + ROS_ERROR_NAMED("actionlib", + "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient"); + } -template -void SimpleActionClient::stopTrackingGoal() -{ - if (gh_.isExpired()) { - ROS_ERROR_NAMED("actionlib", - "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient"); - } - gh_.reset(); -} - -template -void SimpleActionClient::handleFeedback(GoalHandleT gh, - const FeedbackConstPtr & feedback) -{ - if (gh_ != gh) { - ROS_ERROR_NAMED("actionlib", - "Got a callback on a goalHandle that we're not tracking. \ + gh_.cancel(); + } + + template + void SimpleActionClient::stopTrackingGoal() + { + if (gh_.isExpired()) { + ROS_ERROR_NAMED("actionlib", + "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient"); + } + gh_.reset(); + } + + template + void SimpleActionClient::handleFeedback(GoalHandleT gh, + const FeedbackConstPtr & feedback) + { + if (gh_ != gh) { + ROS_ERROR_NAMED("actionlib", + "Got a callback on a goalHandle that we're not tracking. \ This is an internal SimpleActionClient/ActionClient bug. \ This could also be a GoalID collision"); - } - if (feedback_cb_) { - feedback_cb_(feedback); - } -} - -template -void SimpleActionClient::handleTransition(GoalHandleT gh) -{ - CommState comm_state_ = gh.getCommState(); - switch (comm_state_.state_) { - case CommState::WAITING_FOR_GOAL_ACK: - ROS_ERROR_NAMED("actionlib", - "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK"); - break; - case CommState::PENDING: - ROS_ERROR_COND(cur_simple_state_ != SimpleGoalState::PENDING, - "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]", - comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); - break; - case CommState::ACTIVE: - switch (cur_simple_state_.state_) { + } + if (feedback_cb_) { + feedback_cb_(feedback); + } + } + + template + void SimpleActionClient::handleTransition(GoalHandleT gh) + { + CommState comm_state_ = gh.getCommState(); + switch (comm_state_.state_) { + case CommState::WAITING_FOR_GOAL_ACK: + ROS_ERROR_NAMED("actionlib", + "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK"); + break; + case CommState::PENDING: + ROS_ERROR_COND(cur_simple_state_ != SimpleGoalState::PENDING, + "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]", + comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); + break; + case CommState::ACTIVE: + switch (cur_simple_state_.state_) { case SimpleGoalState::PENDING: setSimpleState(SimpleGoalState::ACTIVE); if (active_cb_) { @@ -496,25 +634,25 @@ void SimpleActionClient::handleTransition(GoalHandleT gh) break; case SimpleGoalState::DONE: ROS_ERROR_NAMED("actionlib", - "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]", - comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); + "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]", + comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); break; default: ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_); break; - } - break; - case CommState::WAITING_FOR_RESULT: - break; - case CommState::WAITING_FOR_CANCEL_ACK: - break; - case CommState::RECALLING: - ROS_ERROR_COND(cur_simple_state_ != SimpleGoalState::PENDING, - "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]", - comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); - break; - case CommState::PREEMPTING: - switch (cur_simple_state_.state_) { + } + break; + case CommState::WAITING_FOR_RESULT: + break; + case CommState::WAITING_FOR_CANCEL_ACK: + break; + case CommState::RECALLING: + ROS_ERROR_COND(cur_simple_state_ != SimpleGoalState::PENDING, + "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]", + comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); + break; + case CommState::PREEMPTING: + switch (cur_simple_state_.state_) { case SimpleGoalState::PENDING: setSimpleState(SimpleGoalState::ACTIVE); if (active_cb_) { @@ -525,16 +663,16 @@ void SimpleActionClient::handleTransition(GoalHandleT gh) break; case SimpleGoalState::DONE: ROS_ERROR_NAMED("actionlib", - "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]", - comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); + "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]", + comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); break; default: ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_); break; - } - break; - case CommState::DONE: - switch (cur_simple_state_.state_) { + } + break; + case CommState::DONE: + switch (cur_simple_state_.state_) { case SimpleGoalState::PENDING: case SimpleGoalState::ACTIVE: { @@ -554,89 +692,89 @@ void SimpleActionClient::handleTransition(GoalHandleT gh) default: ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_); break; + } + break; + default: + ROS_ERROR_NAMED("actionlib", "Unknown CommState received [%u]", comm_state_.state_); + break; } - break; - default: - ROS_ERROR_NAMED("actionlib", "Unknown CommState received [%u]", comm_state_.state_); - break; - } -} - -template -bool SimpleActionClient::waitForResult(const ros::Duration & timeout) -{ - if (gh_.isExpired()) { - ROS_ERROR_NAMED("actionlib", - "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient"); - return false; - } + } - if (timeout < ros::Duration(0, 0)) { - ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec()); - } + template + bool SimpleActionClient::waitForResult(const ros::Duration & timeout) + { + if (gh_.isExpired()) { + ROS_ERROR_NAMED("actionlib", + "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient"); + return false; + } - ros::Time timeout_time = ros::Time::now() + timeout; + if (timeout < ros::Duration(0, 0)) { + ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec()); + } - boost::mutex::scoped_lock lock(done_mutex_); + ros::Time timeout_time = ros::Time::now() + timeout; - // Hardcode how often we check for node.ok() - ros::Duration loop_period = ros::Duration().fromSec(.1); + boost::mutex::scoped_lock lock(done_mutex_); - while (nh_.ok()) { - // Determine how long we should wait - ros::Duration time_left = timeout_time - ros::Time::now(); + // Hardcode how often we check for node.ok() + ros::Duration loop_period = ros::Duration().fromSec(.1); - // Check if we're past the timeout time - if (timeout > ros::Duration(0, 0) && time_left <= ros::Duration(0, 0) ) { - break; - } + while (nh_.ok()) { + // Determine how long we should wait + ros::Duration time_left = timeout_time - ros::Time::now(); + + // Check if we're past the timeout time + if (timeout > ros::Duration(0, 0) && time_left <= ros::Duration(0, 0) ) { + break; + } + + if (cur_simple_state_ == SimpleGoalState::DONE) { + break; + } - if (cur_simple_state_ == SimpleGoalState::DONE) { - break; - } + // Truncate the time left + if (time_left > loop_period || timeout == ros::Duration()) { + time_left = loop_period; + } - // Truncate the time left - if (time_left > loop_period || timeout == ros::Duration()) { - time_left = loop_period; + done_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f)); + } + + return cur_simple_state_ == SimpleGoalState::DONE; } - done_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f)); - } + template + SimpleClientGoalState SimpleActionClient::sendGoalAndWait(const Goal & goal, + const ros::Duration & execute_timeout, + const ros::Duration & preempt_timeout) + { + sendGoal(goal); - return cur_simple_state_ == SimpleGoalState::DONE; -} + // See if the goal finishes in time + if (waitForResult(execute_timeout)) { + ROS_DEBUG_NAMED("actionlib", "Goal finished within specified execute_timeout [%.2f]", + execute_timeout.toSec()); + return getState(); + } -template -SimpleClientGoalState SimpleActionClient::sendGoalAndWait(const Goal & goal, - const ros::Duration & execute_timeout, - const ros::Duration & preempt_timeout) -{ - sendGoal(goal); - - // See if the goal finishes in time - if (waitForResult(execute_timeout)) { - ROS_DEBUG_NAMED("actionlib", "Goal finished within specified execute_timeout [%.2f]", - execute_timeout.toSec()); - return getState(); - } - - ROS_DEBUG_NAMED("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]", - execute_timeout.toSec()); - - // It didn't finish in time, so we need to preempt it - cancelGoal(); - - // Now wait again and see if it finishes - if (waitForResult(preempt_timeout)) { - ROS_DEBUG_NAMED("actionlib", "Preempt finished within specified preempt_timeout [%.2f]", - preempt_timeout.toSec()); - } else { - ROS_DEBUG_NAMED("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]", - preempt_timeout.toSec()); - } - return getState(); -} + ROS_DEBUG_NAMED("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]", + execute_timeout.toSec()); + + // It didn't finish in time, so we need to preempt it + cancelGoal(); + + // Now wait again and see if it finishes + if (waitForResult(preempt_timeout)) { + ROS_DEBUG_NAMED("actionlib", "Preempt finished within specified preempt_timeout [%.2f]", + preempt_timeout.toSec()); + } else { + ROS_DEBUG_NAMED("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]", + preempt_timeout.toSec()); + } + return getState(); + } } // namespace actionlib diff --git a/include/actionlib/server/action_server.h b/include/actionlib/server/action_server.h index 4dae47c7..5c8067e7 100644 --- a/include/actionlib/server/action_server.h +++ b/include/actionlib/server/action_server.h @@ -89,7 +89,8 @@ class ActionServer : public ActionServerBase ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, boost::function cancel_cb, - bool auto_start); + bool auto_start, + ros::CallbackQueue *cbq = NULL); /** * @brief Constructor for an ActionServer @@ -100,7 +101,8 @@ class ActionServer : public ActionServerBase */ ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, - bool auto_start); + bool auto_start, + ros::CallbackQueue *cbq = NULL); /** * @brief DEPRECATED Constructor for an ActionServer @@ -120,7 +122,8 @@ class ActionServer : public ActionServerBase * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. */ ActionServer(ros::NodeHandle n, std::string name, - bool auto_start); + bool auto_start, + ros::CallbackQueue *cbq = NULL); /** * @brief DEPRECATED Constructor for an ActionServer diff --git a/include/actionlib/server/action_server_base.h b/include/actionlib/server/action_server_base.h index 1385f85e..623fb1df 100644 --- a/include/actionlib/server/action_server_base.h +++ b/include/actionlib/server/action_server_base.h @@ -38,6 +38,7 @@ #define ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_ #include +#include #include #include #include @@ -75,11 +76,13 @@ class ActionServerBase * @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire * @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. + * @param cbq A pointer to a ros::CallbackQueue that, if not NULL, will be used for subscriptions. */ ActionServerBase( boost::function goal_cb, boost::function cancel_cb, - bool auto_start = false); + bool auto_start = false, + ros::CallbackQueue *cbq = NULL); /** @@ -115,6 +118,11 @@ class ActionServerBase */ void cancelCallback(const boost::shared_ptr & goal_id); + /** + * @brief Get a pointer to the ros::CallbackQueue associated with this ActionServer + */ + ros::CallbackQueue* getCallbackQueue(); + protected: // Allow access to protected fields for helper classes friend class ServerGoalHandle; @@ -158,17 +166,21 @@ class ActionServerBase GoalIDGenerator id_generator_; bool started_; boost::shared_ptr guard_; + + ros::CallbackQueue *cbq_; }; template ActionServerBase::ActionServerBase( boost::function goal_cb, boost::function cancel_cb, - bool auto_start) + bool auto_start, + ros::CallbackQueue *cbq) : goal_callback_(goal_cb), cancel_callback_(cancel_cb), started_(auto_start), - guard_(new DestructionGuard) + guard_(new DestructionGuard), + cbq_(cbq) { } @@ -199,6 +211,11 @@ void ActionServerBase::start() publishStatus(); } +template +ros::CallbackQueue* ActionServerBase::getCallbackQueue() +{ + return cbq_; +} template void ActionServerBase::goalCallback(const boost::shared_ptr & goal) diff --git a/include/actionlib/server/action_server_imp.h b/include/actionlib/server/action_server_imp.h index b0be4682..25804e9c 100644 --- a/include/actionlib/server/action_server_imp.h +++ b/include/actionlib/server/action_server_imp.h @@ -46,9 +46,10 @@ template ActionServer::ActionServer( ros::NodeHandle n, std::string name, - bool auto_start) + bool auto_start, + ros::CallbackQueue *cbq) : ActionServerBase( - boost::function(), boost::function(), auto_start), + boost::function(), boost::function(), auto_start, cbq), node_(n, name) { // if we're to autostart... then we'll initialize things @@ -79,8 +80,9 @@ template ActionServer::ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, boost::function cancel_cb, - bool auto_start) -: ActionServerBase(goal_cb, cancel_cb, auto_start), + bool auto_start, + ros::CallbackQueue *cbq) + : ActionServerBase(goal_cb, cancel_cb, auto_start, cbq), node_(n, name) { // if we're to autostart... then we'll initialize things @@ -113,8 +115,9 @@ ActionServer::ActionServer(ros::NodeHandle n, std::string name, template ActionServer::ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, - bool auto_start) -: ActionServerBase(goal_cb, boost::function(), auto_start), + bool auto_start, + ros::CallbackQueue *cbq) +: ActionServerBase(goal_cb, boost::function(), auto_start, cbq), node_(n, name) { // if we're to autostart... then we'll initialize things @@ -174,12 +177,25 @@ void ActionServer::initialize() boost::bind(&ActionServer::publishStatus, this, _1)); } - goal_sub_ = node_.subscribe("goal", static_cast(sub_queue_size), - boost::bind(&ActionServerBase::goalCallback, this, _1)); + ros::SubscribeOptions goal_sub_opts; + goal_sub_opts = ros::SubscribeOptions::create( + "goal", + static_cast(sub_queue_size), + boost::bind(&ActionServerBase::goalCallback, this, _1), + ros::VoidPtr(), + this->getCallbackQueue() + ); + goal_sub_ = node_.subscribe(goal_sub_opts); - cancel_sub_ = - node_.subscribe("cancel", static_cast(sub_queue_size), - boost::bind(&ActionServerBase::cancelCallback, this, _1)); + ros::SubscribeOptions cancel_sub_opts; + cancel_sub_opts = ros::SubscribeOptions::create( + "cancel", + static_cast(sub_queue_size), + boost::bind(&ActionServerBase::cancelCallback, this, _1), + ros::VoidPtr(), + this->getCallbackQueue() + ); + cancel_sub_ = node_.subscribe(cancel_sub_opts); } template diff --git a/include/actionlib/server/simple_action_server.h b/include/actionlib/server/simple_action_server.h index aad48838..cc29a4f1 100644 --- a/include/actionlib/server/simple_action_server.h +++ b/include/actionlib/server/simple_action_server.h @@ -66,6 +66,11 @@ class SimpleActionServer typedef typename ActionServer::GoalHandle GoalHandle; typedef boost::function ExecuteCallback; + /** + * @brief Default constructor for a SimpleActionServer + */ + SimpleActionServer(); + /** * @brief Constructor for a SimpleActionServer * @param name A name for the action server @@ -138,6 +143,29 @@ class SimpleActionServer */ boost::shared_ptr acceptNewGoal(); + /** + * @brief Initialization for the Action Server + * @param name A name for the action server + * @param execute_callback Optional callback that gets called in a separate thread whenever + * a new goal is received, allowing users to have blocking callbacks. + * Adding an execute callback also deactivates the goalCallback. + * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. + * @param cbq A pointer to the ros::CallbackQueue to be used + */ + void initialize(std::string name, ExecuteCallback execute_callback, bool auto_start, ros::CallbackQueue *cbq = NULL); + + /** + * @brief Initialization for the Action Server + * @param n A NodeHandle to create a namespace under + * @param name A name for the action server + * @param execute_callback Optional callback that gets called in a separate thread whenever + * a new goal is received, allowing users to have blocking callbacks. + * Adding an execute callback also deactivates the goalCallback. + * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. + * @param cbq A pointer to the ros::CallbackQueue to be used + */ + void initialize(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback, bool auto_start, ros::CallbackQueue *cbq = NULL); + /** * @brief Allows polling implementations to query about the availability of a new goal * @return True if a new goal is available, false otherwise diff --git a/include/actionlib/server/simple_action_server_imp.h b/include/actionlib/server/simple_action_server_imp.h index 3975ec62..59f085ab 100644 --- a/include/actionlib/server/simple_action_server_imp.h +++ b/include/actionlib/server/simple_action_server_imp.h @@ -42,6 +42,13 @@ namespace actionlib { +template +SimpleActionServer::SimpleActionServer() +: new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_( + NULL), execute_thread_(NULL), need_to_terminate_(false) +{ +} + template SimpleActionServer::SimpleActionServer(std::string name, ExecuteCallback execute_callback, @@ -210,6 +217,46 @@ ::acceptNewGoal() return current_goal_.getGoal(); } +template +void SimpleActionServer::initialize(std::string name, + ExecuteCallback execute_callback, + bool auto_start, + ros::CallbackQueue *cbq) +{ + execute_callback_ = execute_callback; + if (execute_callback_ != NULL) { + execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this)); + } + + // create the action server + as_ = boost::shared_ptr >(new ActionServer(n_, name, + boost::bind(&SimpleActionServer::goalCallback, this, _1), + boost::bind(&SimpleActionServer::preemptCallback, this, _1), + auto_start, + cbq)); +} + +template +void SimpleActionServer::initialize(ros::NodeHandle n, + std::string name, + ExecuteCallback execute_callback, + bool auto_start, + ros::CallbackQueue *cbq) +{ + n_ = ros::NodeHandle(n); + execute_callback_ = execute_callback; + if (execute_callback_ != NULL) { + execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this)); + } + + // create the action server + as_ = boost::shared_ptr >(new ActionServer(n, name, + boost::bind(&SimpleActionServer::goalCallback, this, _1), + boost::bind(&SimpleActionServer::preemptCallback, this, _1), + auto_start, + cbq)); +} + template bool SimpleActionServer::isNewGoalAvailable() {