realtime_server_goal_handle.h
Go to the documentation of this file.
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


realtime_tools
Author(s): Stuart Glaser
autogenerated on Fri Feb 12 2016 00:05:42