-
Notifications
You must be signed in to change notification settings - Fork 36
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* bug fix galactic mutex * testing undo motion and also improving action client * important refactoring smacc action client * fix * progress in smacc action client * progress in the smacc action client fork based on signals * more changes * more testing * more testing * more testing on abort * minor * adding smaccServiceerver client to galactic * Update cb_default_keyboard_behavior.hpp * testing more husky robot * progress in tests husky demo * testing abort forward and undo * finishing cancel and undo behavior tests
- Loading branch information
1 parent
cb7f8ec
commit 193a8a3
Showing
59 changed files
with
2,303 additions
and
150 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
100 changes: 100 additions & 0 deletions
100
smacc2/include/smacc2/client_bases/smacc_service_server_client.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,100 @@ | ||
| // Copyright 2021 RobosoftAI Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| /***************************************************************************************************************** | ||
| * | ||
| * Authors: Pablo Inigo Blasco, Brett Aldrich | ||
| * | ||
| ******************************************************************************************************************/ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <optional> | ||
| #include <smacc2/smacc_client.hpp> | ||
| #include <smacc2/smacc_signal.hpp> | ||
|
|
||
| namespace smacc2 | ||
| { | ||
| namespace client_bases | ||
| { | ||
| template <typename TService> | ||
| class SmaccServiceServerClient : public smacc2::ISmaccClient | ||
| { | ||
| using TServiceRequest = typename TService::Request; | ||
| using TServiceResponse = typename TService::Response; | ||
|
|
||
| public: | ||
| std::optional<std::string> serviceName_; | ||
| SmaccServiceServerClient() { initialized_ = false; } | ||
| SmaccServiceServerClient(std::string service_name) | ||
| { | ||
| serviceName_ = service_name; | ||
| initialized_ = false; | ||
| } | ||
|
|
||
| virtual ~SmaccServiceServerClient() {} | ||
|
|
||
| smacc2::SmaccSignal<void( | ||
| const std::shared_ptr<typename TService::Request>, | ||
| std::shared_ptr<typename TService::Response>)> | ||
| onServiceRequestReceived_; | ||
|
|
||
| template <typename T> | ||
| boost::signals2::connection onServiceRequestReceived( | ||
| void (T::*callback)( | ||
| const std::shared_ptr<typename TService::Request>, | ||
| std::shared_ptr<typename TService::Response>), | ||
| T * object) | ||
| { | ||
| return this->getStateMachine()->createSignalConnection( | ||
| onServiceRequestReceived_, callback, object); | ||
| } | ||
|
|
||
| void onInitialize() override | ||
| { | ||
| if (!initialized_) | ||
| { | ||
| if (!serviceName_) | ||
| { | ||
| RCLCPP_ERROR_STREAM( | ||
| getLogger(), | ||
| "[" << this->getName() << "] service server with no service name set. Skipping."); | ||
| } | ||
| else | ||
| { | ||
| RCLCPP_INFO_STREAM( | ||
| getLogger(), "[" << this->getName() << "] Client Service: " << *serviceName_); | ||
|
|
||
| server_ = getNode()->create_service<TService>( | ||
| *serviceName_, std::bind( | ||
| &SmaccServiceServerClient<TService>::serviceCallback, this, | ||
| std::placeholders::_1, std::placeholders::_2)); | ||
|
|
||
| this->initialized_ = true; | ||
| } | ||
| } | ||
| } | ||
|
|
||
| private: | ||
| void serviceCallback( | ||
| const std::shared_ptr<typename TService::Request> req, | ||
| std::shared_ptr<typename TService::Response> res) | ||
| { | ||
| onServiceRequestReceived_(req, res); | ||
| } | ||
| typename rclcpp::Service<TService>::SharedPtr server_; | ||
| bool initialized_; | ||
| }; | ||
| } // namespace client_bases | ||
| } // namespace smacc2 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
38 changes: 38 additions & 0 deletions
38
smacc2/include/smacc2/client_behaviors/cb_service_server_callback_base.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,38 @@ | ||
| // Copyright 2021 RobosoftAI Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #pragma once | ||
| #include <smacc2/smacc_client_behavior.hpp> | ||
|
|
||
| namespace smacc2 | ||
| { | ||
| template <typename TService> | ||
| class CbServiceServerCallbackBase : public smacc2::SmaccClientBehavior | ||
| { | ||
| public: | ||
| virtual void onEntry() override | ||
| { | ||
| this->requiresClient(attachedClient_); | ||
| attachedClient_->onServiceRequestReceived( | ||
| &CbServiceServerCallbackBase::onServiceRequestReceived, this); | ||
| } | ||
|
|
||
| virtual void onServiceRequestReceived( | ||
| const std::shared_ptr<typename TService::Request> req, | ||
| std::shared_ptr<typename TService::Response> res) = 0; | ||
|
|
||
| protected: | ||
| smacc2::client_bases::SmaccServiceServerClient<TService> * attachedClient_ = nullptr; | ||
| }; | ||
| } // namespace smacc2 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.