SMACC2
Loading...
Searching...
No Matches
smacc_action_client_base.hpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20
21#pragma once
22
26
27#include <optional>
28#include <rclcpp_action/rclcpp_action.hpp>
29
30namespace smacc2
31{
32namespace client_bases
33{
34using namespace smacc2::default_events;
35
36template <typename ActionType>
38{
39public:
40 // Inside this macro you can find the typedefs for Goal and other types
41 // ACTION_DEFINITION(ActionType);
42 typedef rclcpp_action::Client<ActionType> ActionClient;
43 // typedef actionlib::SimpleActionClient<ActionType> GoalHandle;
44
45 // typedef typename ActionClient::SimpleDoneCallback SimpleDoneCallback;
46 // typedef typename ActionClient::SimpleActiveCallback SimpleActiveCallback;
47 // typedef typename ActionClient::SimpleFeedbackCallback SimpleFeedbackCallback;
48
49 using Goal = typename ActionClient::Goal;
50 using Feedback = typename ActionClient::Feedback;
51 using GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
52 typedef typename GoalHandle::WrappedResult WrappedResult;
53
54 using SendGoalOptions = typename ActionClient::SendGoalOptions;
56 std::function<void(std::shared_future<typename GoalHandle::SharedPtr>)>;
57 using FeedbackCallback = typename GoalHandle::FeedbackCallback;
58 using ResultCallback = typename GoalHandle::ResultCallback;
59 using CancelRequest = typename ActionType::Impl::CancelGoalService::Request;
60 using CancelResponse = typename ActionType::Impl::CancelGoalService::Response;
61 using CancelCallback = std::function<void(typename CancelResponse::SharedPtr)>;
62
64
65 std::string action_endpoint_;
70
72
74
75 virtual std::shared_ptr<rclcpp_action::ClientBase> getClientBase() override { return client_; }
76
77 void onInitialize() override
78 {
79 if (name_ == "") name_ = smacc2::demangleSymbol(typeid(*this).name());
80 this->client_ = rclcpp_action::create_client<ActionType>(getNode(), action_endpoint_);
81 // RCLCPP_INFO_STREAM(
82 // this->getLogger(),
83 // "Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>());
84 //client_->wait_for_action_server();
85 }
86
88 {
90 this->getLogger(),
91 "Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>());
92 client_->wait_for_action_server();
93 }
94
95 static std::string getEventLabel()
96 {
97 auto type = TypeInfo::getTypeInfoFromType<ActionType>();
98 return type->getNonTemplatedTypeName();
99 }
100
101 std::optional<std::shared_future<typename GoalHandle::SharedPtr>> lastRequest_;
102 // typename GoalHandle::SharedPtr goalHandle_;
103 std::optional<std::shared_future<typename CancelResponse::SharedPtr>> lastCancelResponse_;
104
107 // SmaccActionResultSignal onPreempted_;
108 // SmaccActionResultSignal onRejected_;
110
111 // event creation/posting factory functions
115 // std::function<void(WrappedResult)> postPreemptedEvent;
116 // std::function<void(WrappedResult)> postRejectedEvent;
117
118 std::function<void(const Feedback &)> postFeedbackEvent;
119
121
122 template <typename EvType>
124 {
125 auto * ev = new EvType();
126 // ev->client = this;
127 // ev->resultMessage = *result;
129 getLogger(), "Action client Posting EVENT %s", demangleSymbol(typeid(ev).name()).c_str());
130 this->postEvent(ev);
131 }
132
133 template <typename TOrthogonal, typename TSourceObject>
135 {
136 // we create here all the event factory functions capturing the TOrthogonal
137 postSuccessEvent = [this](auto msg)
139 postAbortedEvent = [this](auto msg)
141
142 postCancelledEvent = [this](auto msg)
144
145 postFeedbackEvent = [this](auto msg)
146 {
148 actionFeedbackEvent->client = this;
149 actionFeedbackEvent->feedbackMessage = msg;
151 RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", demangleType(typeid(*this)).c_str());
152 };
153
154 feedback_cb = [this](auto client, auto feedback) { this->onFeedback(client, feedback); };
155 }
156
157 template <typename T>
158 boost::signals2::connection onSucceeded(void (T::*callback)(WrappedResult &), T * object)
159 {
160 return this->getStateMachine()->createSignalConnection(onSucceeded_, callback, object);
161 }
162
163 template <typename T>
164 boost::signals2::connection onSucceeded(std::function<void(WrappedResult &)> callback)
165 {
166 return this->getStateMachine()->createSignalConnection(onSucceeded_, callback);
167 }
168
169 template <typename T>
170 boost::signals2::connection onAborted(void (T::*callback)(WrappedResult &), T * object)
171 {
172 return this->getStateMachine()->createSignalConnection(onAborted_, callback, object);
173 }
174
175 template <typename T>
176 boost::signals2::connection onAborted(std::function<void(WrappedResult &)> callback)
177 {
178 return this->getStateMachine()->createSignalConnection(onAborted_, callback);
179 }
180
181 template <typename T>
182 boost::signals2::connection onCancelled(void (T::*callback)(WrappedResult &), T * object)
183 {
184 return this->getStateMachine()->createSignalConnection(onCancelled_, callback, object);
185 }
186
187 template <typename T>
188 boost::signals2::connection onCancelled(std::function<void(WrappedResult &)> callback)
189 {
190 return this->getStateMachine()->createSignalConnection(onCancelled_, callback);
191 }
192
193 /*
194 template <typename T>
195 boost::signals2::connection onPreempted(void (T::*callback)(WrappedResult &), T *object)
196 {
197 return this->getStateMachine()->createSignalConnection(onPreempted_, callback, object);
198 }
199
200 template <typename T>
201 boost::signals2::connection onPreempted(std::function<void(WrappedResult &)> callback)
202 {
203 return this->getStateMachine()->createSignalConnection(onPreempted_, callback);
204 }
205
206 template <typename T>
207 boost::signals2::connection onRejected(void (T::*callback)(WrappedResult &), T *object)
208 {
209 return this->getStateMachine()->createSignalConnection(onRejected_, callback, object);
210 }
211
212 template <typename T>
213 boost::signals2::connection onRejected(std::function<void(WrappedResult &)> callback)
214 {
215 return this->getStateMachine()->createSignalConnection(onRejected_, callback);
216 }
217 */
218
219 virtual bool cancelGoal() override
220 {
221 lastCancelResponse_ = this->client_->async_cancel_all_goals();
222
223 // if (lastRequest_ && lastRequest_->valid())
224 // {
225
226 // // rclcpp::spin_until_future_complete(getNode(), *lastRequest_);
227 // auto req = lastRequest_->get();
228 // RCLCPP_INFO_STREAM(
229 // getLogger(), "[" << getName() << "] Cancelling goal. req id: "
230 // << rclcpp_action::to_string(req->get_goal_id()));
231 // auto cancelresult = client_->async_cancel_goal(req);
232
233 // // wait actively
234 // // rclcpp::spin_until_future_complete(getNode(), cancelresult);
235 // //lastRequest_.reset();
236 // return true;
237 // }
238 // else
239 // {
240 // RCLCPP_ERROR(
241 // getLogger(), "%s [at %s]: not connected with actionserver, skipping cancel goal ...",
242 // getName().c_str(), getNamespace().c_str());
243 // return false;
244 // }
245
246 return true;
247 }
248
249 std::shared_future<typename GoalHandle::SharedPtr> sendGoal(
252 //ResultCallback resultCallback = nullptr) // bug related with the cancel action and the issue
253 {
254 // client_->sendGoal(goal, result_cb, active_cb, feedback_cb);
255 // std::shared_future<typename GoalHandle::SharedPtr>
256
257 SendGoalOptions options;
258
259 // GoalResponseCallback
260 // options.goal_response_callback;
261
263 // FeedbackCallback
264 options.feedback_callback = feedback_cb;
265
267 // ResultCallback result_callback;
268 // options.result_callback = result_cb;
269
270 options.result_callback =
271 [this, resultCallback](
272 const typename rclcpp_action::ClientGoalHandle<ActionType>::WrappedResult & result)
273 {
274 // TODO(#1652): a work around until rcl_action interface is updated
275 // if goal ids are not matched, the older goa call this callback so ignore the result
276 // if matched, it must be processed (including aborted)
277
279 getLogger(), "[" << getName() << "] Action result callback, getting shared future");
280 // auto goalHandle = result->get();
281 // goalHandle_ = lastRequest_->get();
283 getLogger(), "[" << getName() << "] Action client Result goal id: "
284 << rclcpp_action::to_string(result.goal_id));
285
286 // if (goalHandle_->get_goal_id() == result.goal_id)
287 // {
288 // // goal_result_available_ = true;
289 // // result_ = result;
290 // RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Result CB Goal id matches with last request");
291
292 auto resultCallbackPtr = resultCallback.lock();
293
294 if (resultCallbackPtr != nullptr)
295 {
297 getLogger(), "[" << getName() << "] Result CB calling user callback:"
298 << demangleSymbol(typeid(*resultCallbackPtr).name()));
299 (*resultCallbackPtr)(result);
300 }
301 else
302 {
304 getLogger(), "[" << getName() << "] Result CB calling default callback");
305 this->onResult(result);
306 }
307
308 // }
309 // else
310 // {
311 // RCLCPP_ERROR_STREAM(getLogger(), "[" << getName() << "] Result CB Goal id DOES NOT match with last request. Skipping, incorrect behavior.");
312 // }
313 };
314
315 // if (lastRequest_ && lastRequest_->valid())
316 // {
317 // RCLCPP_INFO_STREAM(getLogger(), getName() << ": checking previous request is really finished.");
318 // auto res = this->lastRequest_->get();
319 // RCLCPP_INFO_STREAM(getLogger(), getName() << ": okay");
320 // }
321 // else
322 // {
323 // RCLCPP_INFO_STREAM(getLogger(), getName() << ": no previous request.");
324 // }
325
327 getLogger(), "[" << getName() << "] client ready clients: "
328 << this->client_->get_number_of_ready_clients());
330 getLogger(),
331 "[" << getName() << "] Waiting it is ready? " << client_->action_server_is_ready());
332
333 RCLCPP_INFO_STREAM(getLogger(), getName() << ": async send goal.");
334 auto lastRequest = this->client_->async_send_goal(goal, options);
335 this->lastRequest_ = lastRequest;
336
338 getLogger(), "[" << getName() << "] Action request "
339 // << rclcpp_action::to_string(this->goalHandle_->get_goal_id()) <<". Goal sent to " << this->action_endpoint_
340 // << "\": " << std::endl
341 // << goal
342 );
343
344 // if (client_->isServerConnected())
345 // {
346 // RCLCPP_INFO_STREAM(getLogger(), getName() << ": Goal sent:" << goal);
347
348 // RCLCPP_INFO_STREAM(getLogger(), getName() << ": Goal Id: " <<
349 // rclcpp_action::to_string(lastRequest_->get()->get_goal_id()));
350
351 // for (auto& gh: this->goal_handles_)
352 // {
353
354 // }
355
356 // RCLCPP_INFO_STREAM(getLogger(), getName() << ": spinning until completed");
357 // if (rclcpp::spin_until_future_complete(this->getNode(), lastRequest_, std::chrono::seconds(2))
358 // !=rclcpp::executor::FutureReturnCode::SUCCESS)
359 // {
360 // throw std::runtime_error("send_goal failed");
361 // }
362
363 // goalHandle_ = lastRequest_->get();
364 // if (!goalHandle_) {
365 // throw std::runtime_error("Goal was rejected by the action server");
366 // }
367
368 // }
369 // else
370 // {
371 // RCLCPP_ERROR(getLogger(),"%s [at %s]: not connected with actionserver, skipping goal request
372 // ...", getName().c_str(), getNamespace().c_str());
373 // //client_->waitForServer();
374 // }
375
376 return lastRequest;
377 }
378
379protected:
380 typename ActionClient::SharedPtr client_;
381
383 typename GoalHandle::SharedPtr /*goalhandle*/,
384 const std::shared_ptr<const Feedback> feedback_msg)
385 {
387 }
388
390 {
391 // auto *actionResultEvent = new EvActionResult<TDerived>();
392 // actionResultEvent->client = this;
393 // actionResultEvent->resultMessage = *result_msg;
394
395 // const auto &resultType = this->getState();
396 const auto & resultType = result_msg.code;
397
399 getLogger(), "[" << this->getName() << "] response result ["
400 << rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
401
402 if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
403 {
404 RCLCPP_INFO(getLogger(), "[%s] request result: Success", this->getName().c_str());
407 }
408 else if (resultType == rclcpp_action::ResultCode::ABORTED)
409 {
410 RCLCPP_INFO(getLogger(), "[%s] request result: Aborted", this->getName().c_str());
413 }
414 else if (resultType == rclcpp_action::ResultCode::CANCELED)
415 {
416 RCLCPP_INFO(getLogger(), "[%s] request result: Cancelled", this->getName().c_str());
419 }
420 /*
421 else if (resultType == actionlib::SimpleClientGoalState::REJECTED)
422 {
423 RCLCPP_INFO(getLogger(),"[%s] request result: Rejected", this->getName().c_str());
424 onRejected_(result_msg);
425 postRejectedEvent(result_msg);
426 }
427 else if (resultType == actionlib::SimpleClientGoalState::PREEMPTED)
428 {
429 RCLCPP_INFO(getLogger(),"[%s] request result: Preempted", this->getName().c_str());
430 onPreempted_(result_msg);
431 postPreemptedEvent(result_msg);
432 }*/
433 else
434 {
436 getLogger(), "[%s] request result: NOT HANDLED TYPE: %d", this->getName().c_str(),
437 (int)resultType);
438 }
439 }
440};
441
442} // namespace client_bases
443
444} // namespace smacc2
rclcpp::Node::SharedPtr getNode()
Definition client.cpp:60
ISmaccStateMachine * getStateMachine()
rclcpp::Logger getLogger()
boost::signals2::connection createSignalConnection(TSmaccSignal &signal, TMemberFunctionPrototype callback, TSmaccObjectType *object)
std::weak_ptr< SmaccSignal< void(const WrappedResult &), optional_last_value< typename boost::function_traits< void(const WrappedResult &) >::result_type >, int, std::less< int >, function< void(const WrappedResult &) >, typename extended_signature< function_traits< void(const WrappedResult &) >::arity, void(const WrappedResult &) >::function_type, boost::signals2::mutex > > WeakPtr
std::optional< std::shared_future< typename CancelResponse::SharedPtr > > lastCancelResponse_
typename ActionType::Impl::CancelGoalService::Response CancelResponse
smacc2::SmaccSignal< void(const WrappedResult &)> SmaccActionResultSignal
typename ActionType::Impl::CancelGoalService::Request CancelRequest
boost::signals2::connection onAborted(void(T::*callback)(WrappedResult &), T *object)
std::function< void(std::shared_future< typename GoalHandle::SharedPtr >)> GoalResponseCallback
std::function< void(typename CancelResponse::SharedPtr)> CancelCallback
typename ActionClient::SendGoalOptions SendGoalOptions
void onFeedback(typename GoalHandle::SharedPtr, const std::shared_ptr< const Feedback > feedback_msg)
virtual std::shared_ptr< rclcpp_action::ClientBase > getClientBase() override
rclcpp_action::Client< ActionType > ActionClient
std::function< void(WrappedResult)> postCancelledEvent
std::function< void(WrappedResult)> postSuccessEvent
boost::signals2::connection onSucceeded(std::function< void(WrappedResult &)> callback)
boost::signals2::connection onSucceeded(void(T::*callback)(WrappedResult &), T *object)
boost::signals2::connection onCancelled(std::function< void(WrappedResult &)> callback)
rclcpp_action::ClientGoalHandle< ActionType > GoalHandle
std::function< void(WrappedResult)> postAbortedEvent
void onResult(const WrappedResult &result_msg)
std::function< void(const Feedback &)> postFeedbackEvent
boost::signals2::connection onCancelled(void(T::*callback)(WrappedResult &), T *object)
std::shared_future< typename GoalHandle::SharedPtr > sendGoal(Goal &goal, typename SmaccActionResultSignal::WeakPtr resultCallback=typename SmaccActionResultSignal::WeakPtr())
typename GoalHandle::ResultCallback ResultCallback
boost::signals2::connection onAborted(std::function< void(WrappedResult &)> callback)
std::optional< std::shared_future< typename GoalHandle::SharedPtr > > lastRequest_
typename GoalHandle::FeedbackCallback FeedbackCallback
std::string demangleSymbol()
std::string demangleType(const std::type_info *tinfo)