$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2008, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author: Eitan Marder-Eppstein 00036 *********************************************************************/ 00037 #ifndef ACTIONLIB_SIMPLE_ACTION_SERVER_H_ 00038 #define ACTIONLIB_SIMPLE_ACTION_SERVER_H_ 00039 00040 #include <boost/thread/condition.hpp> 00041 #include <ros/ros.h> 00042 #include <actionlib/server/action_server.h> 00043 #include <actionlib/action_definition.h> 00044 00045 namespace actionlib { 00056 template <class ActionSpec> 00057 class SimpleActionServer { 00058 public: 00059 //generates typedefs that we'll use to make our lives easier 00060 ACTION_DEFINITION(ActionSpec); 00061 00062 typedef typename ActionServer<ActionSpec>::GoalHandle GoalHandle; 00063 typedef boost::function<void (const GoalConstPtr&)> ExecuteCallback; 00064 00073 SimpleActionServer(std::string name, ExecuteCallback execute_cb, bool auto_start); 00074 00080 SimpleActionServer(std::string name, bool auto_start); 00081 00089 ROS_DEPRECATED SimpleActionServer(std::string name, ExecuteCallback execute_cb = NULL); 00090 00100 SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb, bool auto_start); 00101 00108 SimpleActionServer(ros::NodeHandle n, std::string name, bool auto_start); 00109 00118 ROS_DEPRECATED SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = NULL); 00119 00120 ~SimpleActionServer(); 00121 00133 boost::shared_ptr<const Goal> acceptNewGoal(); 00134 00139 bool isNewGoalAvailable(); 00140 00141 00146 bool isPreemptRequested(); 00147 00152 bool isActive(); 00153 00159 void setSucceeded(const Result& result = Result(), const std::string& text = std::string("")); 00160 00166 void setAborted(const Result& result = Result(), const std::string& text = std::string("")); 00167 00168 00173 void publishFeedback(const FeedbackConstPtr& feedback); 00174 00179 void publishFeedback(const Feedback& feedback); 00180 00186 void setPreempted(const Result& result = Result(), const std::string& text = std::string("")); 00187 00192 void registerGoalCallback(boost::function<void ()> cb); 00193 00198 void registerPreemptCallback(boost::function<void ()> cb); 00199 00203 void start(); 00204 00208 void shutdown(); 00209 00210 private: 00214 void goalCallback(GoalHandle goal); 00215 00219 void preemptCallback(GoalHandle preempt); 00220 00224 void executeLoop(); 00225 00226 ros::NodeHandle n_; 00227 00228 boost::shared_ptr<ActionServer<ActionSpec> > as_; 00229 00230 GoalHandle current_goal_, next_goal_; 00231 00232 bool new_goal_, preempt_request_, new_goal_preempt_request_; 00233 00234 boost::recursive_mutex lock_; 00235 00236 boost::function<void ()> goal_callback_; 00237 boost::function<void ()> preempt_callback_; 00238 ExecuteCallback execute_callback_; 00239 00240 boost::condition execute_condition_; 00241 boost::thread* execute_thread_; 00242 00243 boost::mutex terminate_mutex_; 00244 bool need_to_terminate_; 00245 }; 00246 }; 00247 00248 //include the implementation here 00249 #include <actionlib/server/simple_action_server_imp.h> 00250 #endif