31 #ifndef REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H_
32 #define REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H_
36 #include "rclcpp/exceptions.hpp"
37 #include "rclcpp/logging.hpp"
38 #include "rclcpp_action/server_goal_handle.hpp"
42 template <
class Action>
46 using GoalHandle = rclcpp_action::ServerGoalHandle<Action>;
47 using ResultSharedPtr =
typename Action::Result::SharedPtr;
48 using FeedbackSharedPtr =
typename Action::Feedback::SharedPtr;
58 ResultSharedPtr req_result_;
59 FeedbackSharedPtr req_feedback_;
60 rclcpp::Logger logger_;
63 std::shared_ptr<GoalHandle> gh_;
64 ResultSharedPtr preallocated_result_;
65 FeedbackSharedPtr preallocated_feedback_;
68 std::shared_ptr<GoalHandle> & gh,
const ResultSharedPtr & preallocated_result =
nullptr,
69 const FeedbackSharedPtr & preallocated_feedback =
nullptr)
71 gh, preallocated_result, preallocated_feedback, rclcpp::get_logger(
"realtime_tools"))
76 std::shared_ptr<GoalHandle> & gh,
const ResultSharedPtr & preallocated_result,
77 const FeedbackSharedPtr & preallocated_feedback, rclcpp::Logger logger)
84 preallocated_result_(preallocated_result),
85 preallocated_feedback_(preallocated_feedback)
87 if (!preallocated_result_) {
88 preallocated_result_.reset(
new typename Action::Result);
90 if (!preallocated_feedback_) {
91 preallocated_feedback_.reset(
new typename Action::Feedback);
95 void setAborted(ResultSharedPtr result =
nullptr)
97 if (req_execute_ && !req_succeed_ && !req_abort_ && !req_cancel_) {
98 std::lock_guard<std::mutex> guard(mutex_);
100 req_result_ = result;
105 void setCanceled(ResultSharedPtr result =
nullptr)
107 if (req_execute_ && !req_succeed_ && !req_abort_ && !req_cancel_) {
108 std::lock_guard<std::mutex> guard(mutex_);
110 req_result_ = result;
115 void setSucceeded(ResultSharedPtr result =
nullptr)
117 if (req_execute_ && !req_succeed_ && !req_abort_ && !req_cancel_) {
118 std::lock_guard<std::mutex> guard(mutex_);
120 req_result_ = result;
125 void setFeedback(FeedbackSharedPtr feedback =
nullptr)
127 std::lock_guard<std::mutex> guard(mutex_);
128 req_feedback_ = feedback;
133 if (!req_succeed_ && !req_abort_ && !req_cancel_) {
134 std::lock_guard<std::mutex> guard(mutex_);
139 bool valid() {
return nullptr != gh_.get(); }
141 void runNonRealtime()
147 std::lock_guard<std::mutex> guard(mutex_);
150 if (req_execute_ && !gh_->is_executing() && gh_->is_active() && !gh_->is_canceling()) {
153 if (req_abort_ && gh_->is_executing()) {
154 gh_->abort(req_result_);
157 if (req_cancel_ && gh_->is_active()) {
158 gh_->canceled(req_result_);
161 if (req_succeed_ && !gh_->is_canceling()) {
162 gh_->succeed(req_result_);
163 req_succeed_ =
false;
165 if (req_feedback_ && gh_->is_executing()) {
166 gh_->publish_feedback(req_feedback_);
168 }
catch (
const rclcpp::exceptions::RCLErrorBase & e) {
170 RCLCPP_WARN(logger_,
"%s", e.formatted_message.c_str());