Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22483] pkg/trunk
|
From: <vij...@us...> - 2009-08-20 23:00:39
|
Revision: 22483
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22483&view=rev
Author: vijaypradeep
Date: 2009-08-20 23:00:29 +0000 (Thu, 20 Aug 2009)
Log Message:
-----------
Renaming SingleGoalActionServer to SimpleActionServer
Modified Paths:
--------------
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_action_server.cpp
pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/averaging_action.cpp
pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/configurable_fibonacci_action.cpp
pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/configureable_fibonacci_action.cpp
pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/fibonacci_action.cpp
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
Added Paths:
-----------
pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h
Removed Paths:
-------------
pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h
Modified: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_action_server.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_action_server.cpp 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_action_server.cpp 2009-08-20 23:00:29 UTC (rev 22483)
@@ -33,7 +33,7 @@
*********************************************************************/
#include <ros/ros.h>
-#include <actionlib/server/single_goal_action_server.h>
+#include <actionlib/server/simple_action_server.h>
#include <pr2_calibration_actions/robot_pixels_capture.h>
#include <pr2_calibration_actions/CaptureRobotPixelsAction.h>
@@ -93,7 +93,7 @@
private:
- actionlib::SingleGoalActionServer<pr2_calibration_actions::CaptureRobotPixelsAction> as_;
+ actionlib::SimpleActionServer<pr2_calibration_actions::CaptureRobotPixelsAction> as_;
boost::mutex capture_mutex_;
boost::shared_ptr<RobotPixelsCapture> capture_;
Modified: pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp 2009-08-20 23:00:29 UTC (rev 22483)
@@ -36,7 +36,7 @@
#include <ros/ros.h>
-#include <actionlib/server/single_goal_action_server.h>
+#include <actionlib/server/simple_action_server.h>
#include <move_arm/ActuateGripperAction.h>
#include <std_msgs/Float64.h>
@@ -82,7 +82,7 @@
protected:
ros::NodeHandle nh_;
- actionlib::SingleGoalActionServer<move_arm::ActuateGripperAction> as_;
+ actionlib::SimpleActionServer<move_arm::ActuateGripperAction> as_;
ros::Publisher pub_;
};
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-20 23:00:29 UTC (rev 22483)
@@ -37,7 +37,7 @@
#include "move_arm/move_arm_setup.h"
-#include <actionlib/server/single_goal_action_server.h>
+#include <actionlib/server/simple_action_server.h>
#include <move_arm/MoveArmAction.h>
#include <manipulation_msgs/JointTraj.h>
@@ -1057,7 +1057,7 @@
ros::NodeHandle nh_;
MoveArmSetup &setup_;
- actionlib::SingleGoalActionServer<MoveArmAction> as_;
+ actionlib::SimpleActionServer<MoveArmAction> as_;
planning_environment::PlanningMonitor *planningMonitor_;
tf::TransformListener *tf_;
Copied: pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h (from rev 22482, pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h)
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h (rev 0)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h 2009-08-20 23:00:29 UTC (rev 22483)
@@ -0,0 +1,192 @@
+/*********************************************************************
+*
+* 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 Willow Garage, Inc. 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.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef ACTIONLIB_SIMPLE_ACTION_SERVER_H_
+#define ACTIONLIB_SIMPLE_ACTION_SERVER_H_
+
+#include <boost/thread/condition.hpp>
+#include <ros/ros.h>
+#include <actionlib/server/action_server.h>
+#include <actionlib/action_definition.h>
+
+namespace actionlib {
+ /** @class SimpleActionServer @brief The SimpleActionServer
+ * implements a singe goal policy on top of the ActionServer class. The
+ * specification of the policy is as follows: only one goal can have an
+ * active status at a time, new goals preempt previous goals based on the
+ * stamp in their GoalID field (later goals preempt earlier ones), an
+ * explicit preempt goal preempts all goals with timestamps that are less
+ * than or equal to the stamp associated with the preempt, accepting a new
+ * goal implies successful preemption of any old goal and the status of the
+ * old goal will be change automatically to reflect this.
+ */
+ template <class ActionSpec>
+ class SimpleActionServer {
+ public:
+ //generates typedefs that we'll use to make our lives easier
+ ACTION_DEFINITION(ActionSpec);
+
+ typedef typename ActionServer<ActionSpec>::GoalHandle GoalHandle;
+ typedef boost::function<void (const GoalConstPtr&)> ExecuteCallback;
+
+ /**
+ * @brief Constructor for a SimpleActionServer
+ * @param n A NodeHandle to create a namespace under
+ * @param name A name for the action server
+ * @param execute_cb 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.
+ */
+ SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = NULL);
+
+ ~SimpleActionServer();
+
+ /**
+ * @brief Accepts a new goal when one is available The status of this
+ * goal is set to active upon acceptance, and the status of any
+ * previously active goal is set to preempted. Preempts received for the
+ * new goal between checking if isNewGoalAvailable or invokation of a
+ * goal callback and the acceptNewGoal call will not trigger a preempt
+ * callback. This means, isPreemptReqauested should be called after
+ * accepting the goal even for callback-based implementations to make
+ * sure the new goal does not have a pending preempt request.
+ * @return A shared_ptr to the new goal.
+ */
+ boost::shared_ptr<const Goal> acceptNewGoal();
+
+ /**
+ * @brief Allows polling implementations to query about the availability of a new goal
+ * @return True if a new goal is available, false otherwise
+ */
+ bool isNewGoalAvailable();
+
+
+ /**
+ * @brief Allows polling implementations to query about preempt requests
+ * @return True if a preempt is requested, false otherwise
+ */
+ bool isPreemptRequested();
+
+ /**
+ * @brief Allows polling implementations to query about the status of the current goal
+ * @return True if a goal is active, false otherwise
+ */
+ bool isActive();
+
+ /**
+ * @brief Sets the status of the active goal to succeeded
+ * @param result An optional result to send back to any clients of the goal
+ */
+ void setSucceeded(const Result& result = Result());
+
+ /**
+ * @brief Sets the status of the active goal to aborted
+ * @param result An optional result to send back to any clients of the goal
+ */
+ void setAborted(const Result& result = Result());
+
+
+ /**
+ * @brief Publishes feedback for a given goal
+ * @param feedback Shared pointer to the feedback to publish
+ */
+ void publishFeedback(const FeedbackConstPtr& feedback);
+
+ /**
+ * @brief Publishes feedback for a given goal
+ * @param feedback The feedback to publish
+ */
+ void publishFeedback(const Feedback& feedback);
+
+ /**
+ * @brief Sets the status of the active goal to preempted
+ * @param result An optional result to send back to any clients of the goal
+ */
+ void setPreempted(const Result& result = Result());
+
+ /**
+ * @brief Allows users to register a callback to be invoked when a new goal is available
+ * @param cb The callback to be invoked
+ */
+ void registerGoalCallback(boost::function<void ()> cb);
+
+ /**
+ * @brief Allows users to register a callback to be invoked when a new preempt request is available
+ * @param cb The callback to be invoked
+ */
+ void registerPreemptCallback(boost::function<void ()> cb);
+
+ private:
+ /**
+ * @brief Callback for when the ActionServer receives a new goal and passes it on
+ */
+ void goalCallback(GoalHandle goal);
+
+ /**
+ * @brief Callback for when the ActionServer receives a new preempt and passes it on
+ */
+ void preemptCallback(GoalHandle preempt);
+
+ /**
+ * @brief Called from a separate thread to call blocking execute calls
+ */
+ void executeLoop();
+
+ ros::NodeHandle n_;
+
+ boost::shared_ptr<ActionServer<ActionSpec> > as_;
+
+ GoalHandle current_goal_, next_goal_;
+
+ bool new_goal_, preempt_request_, new_goal_preempt_request_;
+
+ boost::recursive_mutex lock_;
+
+ boost::function<void ()> goal_callback_;
+ boost::function<void ()> preempt_callback_;
+ ExecuteCallback execute_callback_;
+
+ boost::condition execute_condition_;
+ boost::thread* execute_thread_;
+
+ boost::mutex terminate_mutex_;
+ bool need_to_terminate_;
+ };
+};
+
+//include the implementation here
+#include <actionlib/server/simple_action_server_imp.h>
+#endif
Copied: pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h (from rev 22482, pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h)
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h (rev 0)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h 2009-08-20 23:00:29 UTC (rev 22483)
@@ -0,0 +1,259 @@
+/*********************************************************************
+*
+* 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 Willow Garage, Inc. 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.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+
+namespace actionlib {
+ template <class ActionSpec>
+ SimpleActionServer<ActionSpec>::SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback)
+ : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback), need_to_terminate_(false) {
+
+ //create the action server
+ as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
+ boost::bind(&SimpleActionServer::goalCallback, this, _1),
+ boost::bind(&SimpleActionServer::preemptCallback, this, _1)));
+
+ if (execute_callback_ != NULL)
+ {
+ execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
+ }
+ }
+
+ template <class ActionSpec>
+ SimpleActionServer<ActionSpec>::~SimpleActionServer()
+ {
+ if (execute_callback_)
+ {
+ {
+ boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
+ need_to_terminate_ = true;
+ }
+
+ assert(execute_thread_);
+ execute_thread_->join();
+ delete execute_thread_;
+ }
+ }
+
+ template <class ActionSpec>
+ boost::shared_ptr<const typename SimpleActionServer<ActionSpec>::Goal> SimpleActionServer<ActionSpec>::acceptNewGoal(){
+ boost::recursive_mutex::scoped_lock lock(lock_);
+
+ if(!new_goal_ || !next_goal_.getGoal()){
+ ROS_ERROR("Attempting to accept the next goal when a new goal is not available");
+ return boost::shared_ptr<const Goal>();
+ }
+
+ //check if we need to send a preempted message for the goal that we're currently pursuing
+ if(isActive()
+ && current_goal_.getGoal()
+ && current_goal_ != next_goal_){
+ current_goal_.setCanceled();
+ }
+
+ ROS_DEBUG("Accepting a new goal");
+
+ //accept the next goal
+ current_goal_ = next_goal_;
+ new_goal_ = false;
+
+ //set preempt to request to equal the preempt state of the new goal
+ preempt_request_ = new_goal_preempt_request_;
+ new_goal_preempt_request_ = false;
+
+ //set the status of the current goal to be active
+ current_goal_.setAccepted();
+
+ return current_goal_.getGoal();
+ }
+
+ template <class ActionSpec>
+ bool SimpleActionServer<ActionSpec>::isNewGoalAvailable(){
+ return new_goal_;
+ }
+
+
+ template <class ActionSpec>
+ bool SimpleActionServer<ActionSpec>::isPreemptRequested(){
+ return preempt_request_;
+ }
+
+ template <class ActionSpec>
+ bool SimpleActionServer<ActionSpec>::isActive(){
+ if(!current_goal_.getGoal())
+ return false;
+ unsigned int status = current_goal_.getGoalStatus().status;
+ return status == GoalStatus::ACTIVE || status == GoalStatus::PREEMPTING;
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::setSucceeded(const Result& result){
+ boost::recursive_mutex::scoped_lock lock(lock_);
+ current_goal_.setSucceeded(result);
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::setAborted(const Result& result){
+ boost::recursive_mutex::scoped_lock lock(lock_);
+ current_goal_.setAborted(result);
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::setPreempted(const Result& result){
+ boost::recursive_mutex::scoped_lock lock(lock_);
+ ROS_DEBUG("Setting the current goal as canceled");
+ current_goal_.setCanceled(result);
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::registerGoalCallback(boost::function<void ()> cb){
+ // Cannot register a goal callback if an execute callback exists
+ if (execute_callback_)
+ ROS_WARN("Cannot call SimpleActionServer::registerGoalCallback() because an executeCallback exists. Not going to register it.");
+ else
+ goal_callback_ = cb;
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::registerPreemptCallback(boost::function<void ()> cb){
+ preempt_callback_ = cb;
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::publishFeedback(const FeedbackConstPtr& feedback)
+ {
+ current_goal_.publishFeedback(*feedback);
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::publishFeedback(const Feedback& feedback)
+ {
+ current_goal_.publishFeedback(feedback);
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::goalCallback(GoalHandle goal){
+ boost::recursive_mutex::scoped_lock lock(lock_);
+ ROS_DEBUG("A new goal has been recieved by the single goal action server");
+
+ //check that the timestamp is past that of the current goal and the next goal
+ if((!current_goal_.getGoal() || goal.getGoalID().stamp > current_goal_.getGoalID().stamp)
+ && (!next_goal_.getGoal() || goal.getGoalID().stamp > next_goal_.getGoalID().stamp)){
+
+ //if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
+ if(next_goal_.getGoal() && (!current_goal_.getGoal() || next_goal_ != current_goal_)){
+ next_goal_.setCanceled();
+ }
+
+ next_goal_ = goal;
+ new_goal_ = true;
+ new_goal_preempt_request_ = false;
+
+ //if the user has defined a goal callback, we'll call it now
+ if(goal_callback_)
+ goal_callback_();
+
+ // Trigger runLoop to call execute()
+ execute_condition_.notify_all();
+ }
+ else{
+ //the goal requested has already been preempted by a different goal, so we're not going to execute it
+ goal.setCanceled();
+ }
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::preemptCallback(GoalHandle preempt){
+ boost::recursive_mutex::scoped_lock lock(lock_);
+ ROS_DEBUG("A preempt has been received by the SimpleActionServer");
+
+ //if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
+ if(preempt == current_goal_){
+ ROS_DEBUG("Setting preempt_request bit for the current goal to TRUE and invoking callback");
+ preempt_request_ = true;
+
+ //if the user has registered a preempt callback, we'll call it now
+ if(preempt_callback_)
+ preempt_callback_();
+ }
+ //if the preempt applies to the next goal, we'll set the preempt bit for that
+ else if(preempt == next_goal_){
+ ROS_DEBUG("Setting preempt request bit for the next goal to TRUE");
+ new_goal_preempt_request_ = true;
+ }
+ }
+
+ template <class ActionSpec>
+ void SimpleActionServer<ActionSpec>::executeLoop(){
+
+ ros::Duration loop_duration = ros::Duration().fromSec(.1);
+
+ while (n_.ok())
+ {
+ {
+ boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
+ if (need_to_terminate_)
+ break;
+ }
+
+ boost::recursive_mutex::scoped_lock lock(lock_);
+ if (isActive())
+ ROS_ERROR("Should never reach this code with an active goal");
+ else if (isNewGoalAvailable())
+ {
+ GoalConstPtr goal = acceptNewGoal();
+
+ ROS_FATAL_COND(!execute_callback_, "execute_callback_ must exist. This is a bug in SimpleActionServer");
+
+ // Make sure we're not locked when we call execute
+ lock.unlock();
+ execute_callback_(goal);
+ lock.lock();
+
+ if (isActive())
+ {
+ ROS_WARN("Your executeCallback did not set the goal to a terminal status.\n"
+ "This is a bug in your ActionServer implementation. Fix your code!\n"
+ "For now, the ActionServer will set this goal to aborted");
+ setAborted();
+ }
+ }
+ else
+ execute_condition_.timed_wait(lock, boost::posix_time::milliseconds(loop_duration.toSec() * 1000.0f));
+ }
+ }
+
+};
+
Deleted: pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h 2009-08-20 23:00:29 UTC (rev 22483)
@@ -1,192 +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 Willow Garage, Inc. 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.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef ACTIONLIB_SINGLE_GOAL_ACTION_SERVER_H_
-#define ACTIONLIB_SINGLE_GOAL_ACTION_SERVER_H_
-
-#include <boost/thread/condition.hpp>
-#include <ros/ros.h>
-#include <actionlib/server/action_server.h>
-#include <actionlib/action_definition.h>
-
-namespace actionlib {
- /** @class SingleGoalActionServer @brief The SingleGoalActionServer
- * implements a singe goal policy on top of the ActionServer class. The
- * specification of the policy is as follows: only one goal can have an
- * active status at a time, new goals preempt previous goals based on the
- * stamp in their GoalID field (later goals preempt earlier ones), an
- * explicit preempt goal preempts all goals with timestamps that are less
- * than or equal to the stamp associated with the preempt, accepting a new
- * goal implies successful preemption of any old goal and the status of the
- * old goal will be change automatically to reflect this.
- */
- template <class ActionSpec>
- class SingleGoalActionServer {
- public:
- //generates typedefs that we'll use to make our lives easier
- ACTION_DEFINITION(ActionSpec);
-
- typedef typename ActionServer<ActionSpec>::GoalHandle GoalHandle;
- typedef boost::function<void (const GoalConstPtr&)> ExecuteCallback;
-
- /**
- * @brief Constructor for a SingleGoalActionServer
- * @param n A NodeHandle to create a namespace under
- * @param name A name for the action server
- * @param execute_cb 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.
- */
- SingleGoalActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = NULL);
-
- ~SingleGoalActionServer();
-
- /**
- * @brief Accepts a new goal when one is available The status of this
- * goal is set to active upon acceptance, and the status of any
- * previously active goal is set to preempted. Preempts received for the
- * new goal between checking if isNewGoalAvailable or invokation of a
- * goal callback and the acceptNewGoal call will not trigger a preempt
- * callback. This means, isPreemptReqauested should be called after
- * accepting the goal even for callback-based implementations to make
- * sure the new goal does not have a pending preempt request.
- * @return A shared_ptr to the new goal.
- */
- boost::shared_ptr<const Goal> acceptNewGoal();
-
- /**
- * @brief Allows polling implementations to query about the availability of a new goal
- * @return True if a new goal is available, false otherwise
- */
- bool isNewGoalAvailable();
-
-
- /**
- * @brief Allows polling implementations to query about preempt requests
- * @return True if a preempt is requested, false otherwise
- */
- bool isPreemptRequested();
-
- /**
- * @brief Allows polling implementations to query about the status of the current goal
- * @return True if a goal is active, false otherwise
- */
- bool isActive();
-
- /**
- * @brief Sets the status of the active goal to succeeded
- * @param result An optional result to send back to any clients of the goal
- */
- void setSucceeded(const Result& result = Result());
-
- /**
- * @brief Sets the status of the active goal to aborted
- * @param result An optional result to send back to any clients of the goal
- */
- void setAborted(const Result& result = Result());
-
-
- /**
- * @brief Publishes feedback for a given goal
- * @param feedback Shared pointer to the feedback to publish
- */
- void publishFeedback(const FeedbackConstPtr& feedback);
-
- /**
- * @brief Publishes feedback for a given goal
- * @param feedback The feedback to publish
- */
- void publishFeedback(const Feedback& feedback);
-
- /**
- * @brief Sets the status of the active goal to preempted
- * @param result An optional result to send back to any clients of the goal
- */
- void setPreempted(const Result& result = Result());
-
- /**
- * @brief Allows users to register a callback to be invoked when a new goal is available
- * @param cb The callback to be invoked
- */
- void registerGoalCallback(boost::function<void ()> cb);
-
- /**
- * @brief Allows users to register a callback to be invoked when a new preempt request is available
- * @param cb The callback to be invoked
- */
- void registerPreemptCallback(boost::function<void ()> cb);
-
- private:
- /**
- * @brief Callback for when the ActionServer receives a new goal and passes it on
- */
- void goalCallback(GoalHandle goal);
-
- /**
- * @brief Callback for when the ActionServer receives a new preempt and passes it on
- */
- void preemptCallback(GoalHandle preempt);
-
- /**
- * @brief Called from a separate thread to call blocking execute calls
- */
- void executeLoop();
-
- ros::NodeHandle n_;
-
- boost::shared_ptr<ActionServer<ActionSpec> > as_;
-
- GoalHandle current_goal_, next_goal_;
-
- bool new_goal_, preempt_request_, new_goal_preempt_request_;
-
- boost::recursive_mutex lock_;
-
- boost::function<void ()> goal_callback_;
- boost::function<void ()> preempt_callback_;
- ExecuteCallback execute_callback_;
-
- boost::condition execute_condition_;
- boost::thread* execute_thread_;
-
- boost::mutex terminate_mutex_;
- bool need_to_terminate_;
- };
-};
-
-//include the implementation here
-#include <actionlib/server/single_goal_action_server_imp.h>
-#endif
Deleted: pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h 2009-08-20 23:00:29 UTC (rev 22483)
@@ -1,260 +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 Willow Garage, Inc. 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.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef ACTION_LIB_SINGLE_GOAL_ACTION_SERVER_H_
-#define ACTION_LIB_SINGLE_GOAL_ACTION_SERVER_H_
-namespace actionlib {
- template <class ActionSpec>
- SingleGoalActionServer<ActionSpec>::SingleGoalActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback)
- : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback), need_to_terminate_(false) {
-
- //create the action server
- as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
- boost::bind(&SingleGoalActionServer::goalCallback, this, _1),
- boost::bind(&SingleGoalActionServer::preemptCallback, this, _1)));
-
- if (execute_callback_ != NULL)
- {
- execute_thread_ = new boost::thread(boost::bind(&SingleGoalActionServer::executeLoop, this));
- }
- }
-
- template <class ActionSpec>
- SingleGoalActionServer<ActionSpec>::~SingleGoalActionServer()
- {
- if (execute_callback_)
- {
- {
- boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
- need_to_terminate_ = true;
- }
-
- assert(execute_thread_);
- execute_thread_->join();
- delete execute_thread_;
- }
- }
-
- template <class ActionSpec>
- boost::shared_ptr<const typename SingleGoalActionServer<ActionSpec>::Goal> SingleGoalActionServer<ActionSpec>::acceptNewGoal(){
- boost::recursive_mutex::scoped_lock lock(lock_);
-
- if(!new_goal_ || !next_goal_.getGoal()){
- ROS_ERROR("Attempting to accept the next goal when a new goal is not available");
- return boost::shared_ptr<const Goal>();
- }
-
- //check if we need to send a preempted message for the goal that we're currently pursuing
- if(isActive()
- && current_goal_.getGoal()
- && current_goal_ != next_goal_){
- current_goal_.setCanceled();
- }
-
- ROS_DEBUG("Accepting a new goal");
-
- //accept the next goal
- current_goal_ = next_goal_;
- new_goal_ = false;
-
- //set preempt to request to equal the preempt state of the new goal
- preempt_request_ = new_goal_preempt_request_;
- new_goal_preempt_request_ = false;
-
- //set the status of the current goal to be active
- current_goal_.setAccepted();
-
- return current_goal_.getGoal();
- }
-
- template <class ActionSpec>
- bool SingleGoalActionServer<ActionSpec>::isNewGoalAvailable(){
- return new_goal_;
- }
-
-
- template <class ActionSpec>
- bool SingleGoalActionServer<ActionSpec>::isPreemptRequested(){
- return preempt_request_;
- }
-
- template <class ActionSpec>
- bool SingleGoalActionServer<ActionSpec>::isActive(){
- if(!current_goal_.getGoal())
- return false;
- unsigned int status = current_goal_.getGoalStatus().status;
- return status == GoalStatus::ACTIVE || status == GoalStatus::PREEMPTING;
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::setSucceeded(const Result& result){
- boost::recursive_mutex::scoped_lock lock(lock_);
- current_goal_.setSucceeded(result);
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::setAborted(const Result& result){
- boost::recursive_mutex::scoped_lock lock(lock_);
- current_goal_.setAborted(result);
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::setPreempted(const Result& result){
- boost::recursive_mutex::scoped_lock lock(lock_);
- ROS_DEBUG("Setting the current goal as canceled");
- current_goal_.setCanceled(result);
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::registerGoalCallback(boost::function<void ()> cb){
- // Cannot register a goal callback if an execute callback exists
- if (execute_callback_)
- ROS_WARN("Cannot call SingleGoalActionServer::registerGoalCallback() because an executeCallback exists. Not going to register it.");
- else
- goal_callback_ = cb;
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::registerPreemptCallback(boost::function<void ()> cb){
- preempt_callback_ = cb;
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::publishFeedback(const FeedbackConstPtr& feedback)
- {
- current_goal_.publishFeedback(*feedback);
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::publishFeedback(const Feedback& feedback)
- {
- current_goal_.publishFeedback(feedback);
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::goalCallback(GoalHandle goal){
- boost::recursive_mutex::scoped_lock lock(lock_);
- ROS_DEBUG("A new goal has been recieved by the single goal action server");
-
- //check that the timestamp is past that of the current goal and the next goal
- if((!current_goal_.getGoal() || goal.getGoalID().stamp > current_goal_.getGoalID().stamp)
- && (!next_goal_.getGoal() || goal.getGoalID().stamp > next_goal_.getGoalID().stamp)){
-
- //if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
- if(next_goal_.getGoal() && (!current_goal_.getGoal() || next_goal_ != current_goal_)){
- next_goal_.setCanceled();
- }
-
- next_goal_ = goal;
- new_goal_ = true;
- new_goal_preempt_request_ = false;
-
- //if the user has defined a goal callback, we'll call it now
- if(goal_callback_)
- goal_callback_();
-
- // Trigger runLoop to call execute()
- execute_condition_.notify_all();
- }
- else{
- //the goal requested has already been preempted by a different goal, so we're not going to execute it
- goal.setCanceled();
- }
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::preemptCallback(GoalHandle preempt){
- boost::recursive_mutex::scoped_lock lock(lock_);
- ROS_DEBUG("A preempt has been received by the SingleGoalActionServer");
-
- //if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
- if(preempt == current_goal_){
- ROS_DEBUG("Setting preempt_request bit for the current goal to TRUE and invoking callback");
- preempt_request_ = true;
-
- //if the user has registered a preempt callback, we'll call it now
- if(preempt_callback_)
- preempt_callback_();
- }
- //if the preempt applies to the next goal, we'll set the preempt bit for that
- else if(preempt == next_goal_){
- ROS_DEBUG("Setting preempt request bit for the next goal to TRUE");
- new_goal_preempt_request_ = true;
- }
- }
-
- template <class ActionSpec>
- void SingleGoalActionServer<ActionSpec>::executeLoop(){
-
- ros::Duration loop_duration = ros::Duration().fromSec(.1);
-
- while (n_.ok())
- {
- {
- boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
- if (need_to_terminate_)
- break;
- }
-
- boost::recursive_mutex::scoped_lock lock(lock_);
- if (isActive())
- ROS_ERROR("Should never reach this code with an active goal");
- else if (isNewGoalAvailable())
- {
- GoalConstPtr goal = acceptNewGoal();
-
- ROS_FATAL_COND(!execute_callback_, "execute_callback_ must exist. This is a bug in SingleGoalActionServer");
-
- // Make sure we're not locked when we call execute
- lock.unlock();
- execute_callback_(goal);
- lock.lock();
-
- if (isActive())
- {
- ROS_WARN("Your executeCallback did not set the goal to a terminal status.\n"
- "This is a bug in your ActionServer implementation. Fix your code!\n"
- "For now, the ActionServer will set this goal to aborted");
- setAborted();
- }
- }
- else
- execute_condition_.timed_wait(lock, boost::posix_time::milliseconds(loop_duration.toSec() * 1000.0f));
- }
- }
-
-};
-#endif
Modified: pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/averaging_action.cpp
===================================================================
--- pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/averaging_action.cpp 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/averaging_action.cpp 2009-08-20 23:00:29 UTC (rev 22483)
@@ -36,7 +36,7 @@
#include <ros/ros.h>
-#include <actionlib/server/single_goal_action_server.h>
+#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/AveragingAction.h>
#include <std_msgs/Float32.h>
@@ -107,7 +107,7 @@
protected:
ros::NodeHandle nh_;
- actionlib::SingleGoalActionServer<actionlib_tutorials::AveragingAction> as_;
+ actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction> as_;
std::string action_name_;
int data_count_, goal_;
float sum_, sum_sq_;
Modified: pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/configurable_fibonacci_action.cpp
===================================================================
--- pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/configurable_fibonacci_action.cpp 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/configurable_fibonacci_action.cpp 2009-08-20 23:00:29 UTC (rev 22483)
@@ -36,7 +36,7 @@
#include <sstream>
#include <ros/ros.h>
-#include <actionlib/server/single_goal_action_server.h>
+#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>
class FibonacciAction
@@ -120,7 +120,7 @@
protected:
ros::NodeHandle nh_;
- actionlib::SingleGoalActionServer<actionlib_tutorials::FibonacciAction> as_;
+ actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_;
std::string action_name_;
int seed0_, seed1_;
};
Modified: pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/configureable_fibonacci_action.cpp
===================================================================
--- pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/configureable_fibonacci_action.cpp 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/configureable_fibonacci_action.cpp 2009-08-20 23:00:29 UTC (rev 22483)
@@ -36,7 +36,7 @@
#include <sstream>
#include <ros/ros.h>
-#include <actionlib/server/single_goal_action_server.h>
+#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>
class FibonacciAction
@@ -116,7 +116,7 @@
protected:
ros::NodeHandle nh_;
- actionlib::SingleGoalActionServer<actionlib_tutorials::FibonacciAction> as_;
+ actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_;
std::string action_name_;
// create messages that are used to published feedback/result
actionlib_tutorials::FibonacciFeedback feedback_;
Modified: pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/fibonacci_action.cpp
===================================================================
--- pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/fibonacci_action.cpp 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/stacks/common/actionlib_tutorials/simple_action_servers/fibonacci_action.cpp 2009-08-20 23:00:29 UTC (rev 22483)
@@ -35,7 +35,7 @@
/* Author: Melonee Wise */
#include <ros/ros.h>
-#include <actionlib/server/single_goal_action_server.h>
+#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>
class FibonacciAction
@@ -97,7 +97,7 @@
protected:
ros::NodeHandle nh_;
- actionlib::SingleGoalActionServer<actionlib_tutorials::FibonacciAction> as_;
+ actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_;
std::string action_name_;
// create messages that are used to published feedback/result
actionlib_tutorials::FibonacciFeedback feedback_;
Modified: pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-20 22:55:12 UTC (rev 22482)
+++ pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-20 23:00:29 UTC (rev 22483)
@@ -38,7 +38,7 @@
#define NAV_MOVE_BASE_ACTION_H_
#include <ros/ros.h>
-#include <actionlib/server/single_goal_action_server.h>
+#include <actionlib/server/simple_action_server.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <nav_core/base_local_planner.h>
@@ -56,7 +56,7 @@
namespace move_base {
//typedefs to help us out with the action server so that we don't hace to type so much
- typedef actionlib::SingleGoalActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
+ typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
enum MoveBaseState {
PLANNING,
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
