00001 00002 // Copyright (c) 2008, Willow Garage, Inc. 00003 // 00004 // Redistribution and use in source and binary forms, with or without 00005 // modification, are permitted provided that the following conditions are met: 00006 // * Redistributions of source code must retain the above copyright notice, 00007 // this list of conditions and the following disclaimer. 00008 // * Redistributions in binary form must reproduce the above copyright 00009 // notice, this list of conditions and the following disclaimer in the 00010 // documentation and/or other materials provided with the distribution. 00011 // * Neither the name of hiDOF, Inc. nor the names of its 00012 // contributors may be used to endorse or promote products derived from 00013 // this software without specific prior written permission. 00014 // 00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00025 // POSSIBILITY OF SUCH DAMAGE. 00027 00029 00030 #ifndef REALTIME_TOOLS_REALTIME_SERVER_GOAL_HANDLE_H 00031 #define REALTIME_TOOLS_REALTIME_SERVER_GOAL_HANDLE_H 00032 00033 // Standard 00034 #include <inttypes.h> 00035 00036 // Boost 00037 #include <boost/shared_ptr.hpp> 00038 00039 // Actionlib 00040 #include <actionlib/server/action_server.h> 00041 00042 namespace realtime_tools 00043 { 00044 00045 template <class Action> 00046 class RealtimeServerGoalHandle 00047 { 00048 private: 00049 ACTION_DEFINITION(Action); 00050 00051 typedef actionlib::ServerGoalHandle<Action> GoalHandle; 00052 typedef boost::shared_ptr<Result> ResultPtr; 00053 00054 uint8_t state_; 00055 00056 bool req_abort_; 00057 bool req_succeed_; 00058 ResultConstPtr req_result_; 00059 00060 public: 00061 GoalHandle gh_; 00062 ResultPtr preallocated_result_; // Preallocated so it can be used in realtime 00063 00064 RealtimeServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL)) 00065 : req_abort_(false), 00066 req_succeed_(false), 00067 gh_(gh), 00068 preallocated_result_(preallocated_result) 00069 { 00070 if (!preallocated_result_) 00071 preallocated_result_.reset(new Result); 00072 } 00073 00074 void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL)) 00075 { 00076 if (!req_succeed_ && !req_abort_) 00077 { 00078 req_result_ = result; 00079 req_abort_ = true; 00080 } 00081 } 00082 00083 void setSucceeded(ResultConstPtr result = ResultConstPtr((Result*)NULL)) 00084 { 00085 if (!req_succeed_ && !req_abort_) 00086 { 00087 req_result_ = result; 00088 req_succeed_ = true; 00089 } 00090 } 00091 00092 bool valid() 00093 { 00094 return gh_.getGoal() != NULL; 00095 } 00096 00097 void runNonRealtime(const ros::TimerEvent &te) 00098 { 00099 using namespace actionlib_msgs; 00100 if (valid()) 00101 { 00102 actionlib_msgs::GoalStatus gs = gh_.getGoalStatus(); 00103 if (req_abort_ && gs.status == GoalStatus::ACTIVE) 00104 { 00105 if (req_result_) 00106 gh_.setAborted(*req_result_); 00107 else 00108 gh_.setAborted(); 00109 } 00110 else if (req_succeed_ && gs.status == GoalStatus::ACTIVE) 00111 { 00112 if (req_result_) 00113 gh_.setSucceeded(*req_result_); 00114 else 00115 gh_.setSucceeded(); 00116 } 00117 } 00118 } 00119 }; 00120 00121 } // namespace 00122 00123 #endif // header guard