realtime_server_goal_handle.h
Go to the documentation of this file.
1 // Copyright (c) 2008, Willow Garage, Inc.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of hiDOF, Inc. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #ifndef REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H
31 #define REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H
32 
33 // Standard
34 #include <memory>
35 
36 // Actionlib
38 
39 namespace realtime_tools
40 {
41 
42 template <class Action>
44 {
45 private:
46  ACTION_DEFINITION(Action);
47 
49 
50  uint8_t state_;
51 
52  bool req_abort_;
55  ResultConstPtr req_result_;
56  FeedbackConstPtr req_feedback_;
57 
58 public:
59  GoalHandle gh_;
60  ResultPtr preallocated_result_; // Preallocated so it can be used in realtime
61  FeedbackPtr preallocated_feedback_; // Preallocated so it can be used in realtime
62 
63  RealtimeServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL), const FeedbackPtr &preallocated_feedback = FeedbackPtr((Feedback*)NULL))
64  : req_abort_(false),
65  req_cancel_(false),
66  req_succeed_(false),
67  gh_(gh),
68  preallocated_result_(preallocated_result),
69  preallocated_feedback_(preallocated_feedback)
70  {
71  if (!preallocated_result_)
72  preallocated_result_.reset(new Result);
73  if (!preallocated_feedback_)
74  preallocated_feedback_.reset(new Feedback);
75  }
76 
77  void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL))
78  {
79  if (!req_succeed_ && !req_abort_ && !req_cancel_)
80  {
81  req_result_ = result;
82  req_abort_ = true;
83  }
84  }
85 
86  void setCanceled(ResultConstPtr result = ResultConstPtr((Result*)NULL))
87  {
88  if (!req_succeed_ && !req_abort_ && !req_cancel_)
89  {
90  req_result_ = result;
91  req_cancel_ = true;
92  }
93  }
94 
95  void setSucceeded(ResultConstPtr result = ResultConstPtr((Result*)NULL))
96  {
97  if (!req_succeed_ && !req_abort_ && !req_cancel_)
98  {
99  req_result_ = result;
100  req_succeed_ = true;
101  }
102  }
103 
104  void setFeedback(FeedbackConstPtr feedback = FeedbackConstPtr((Feedback*)NULL))
105  {
106  req_feedback_ = feedback;
107  }
108 
109  bool valid()
110  {
111  return gh_.getGoal() != NULL;
112  }
113 
115  {
116  using namespace actionlib_msgs;
117  if (valid())
118  {
119  actionlib_msgs::GoalStatus gs = gh_.getGoalStatus();
120  if (req_abort_ && (gs.status == GoalStatus::ACTIVE ||
121  gs.status == GoalStatus::PREEMPTING))
122  {
123  if (req_result_)
124  gh_.setAborted(*req_result_);
125  else
126  gh_.setAborted();
127  }
128  else if (req_cancel_ && gs.status == GoalStatus::PREEMPTING)
129  {
130  if (req_result_)
131  gh_.setCanceled(*req_result_);
132  else
133  gh_.setCanceled();
134  }
135  else if (req_succeed_ && (gs.status == GoalStatus::ACTIVE ||
136  gs.status == GoalStatus::PREEMPTING))
137  {
138  if (req_result_)
139  gh_.setSucceeded(*req_result_);
140  else
141  gh_.setSucceeded();
142  }
143  if (req_feedback_ && gs.status == GoalStatus::ACTIVE)
144  {
145  gh_.publishFeedback(*req_feedback_);
146  }
147  }
148  }
149 };
150 
151 } // namespace
152 
153 #endif // header guard
void publishFeedback(const Feedback &feedback)
boost::shared_ptr< const Goal > getGoal() const
RealtimeServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result=ResultPtr((Result *) NULL), const FeedbackPtr &preallocated_feedback=FeedbackPtr((Feedback *) NULL))
actionlib_msgs::GoalStatus getGoalStatus() const
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
void setAborted(ResultConstPtr result=ResultConstPtr((Result *) NULL))
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
void setSucceeded(ResultConstPtr result=ResultConstPtr((Result *) NULL))
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
void setCanceled(ResultConstPtr result=ResultConstPtr((Result *) NULL))
actionlib::ServerGoalHandle< Action > GoalHandle
void setFeedback(FeedbackConstPtr feedback=FeedbackConstPtr((Feedback *) NULL))


realtime_tools
Author(s): Stuart Glaser
autogenerated on Mon Feb 28 2022 23:22:45