diff --git a/CMakeLists.txt b/CMakeLists.txt index 16267be..efd09bd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,9 +22,17 @@ find_package(std_msgs REQUIRED) find_package(tachimawari REQUIRED) find_package(tachimawari_interfaces REQUIRED) +find_package(Protobuf CONFIG REQUIRED) +message(STATUS "Using protobuf ${Protobuf_VERSION}") + +# Find gRPC installation +# Looks for gRPCConfig.cmake file installed by gRPC's cmake installation. +find_package(gRPC CONFIG REQUIRED) +message(STATUS "Using gRPC ${gRPC_VERSION}") + add_library(${PROJECT_NAME} SHARED "src/${PROJECT_NAME}/action/model/action_name.cpp" - "src/${PROJECT_NAME}/action/model/action.cpp" + "src/${PROJECT_NAME}/action/model/action.cpp" "src/${PROJECT_NAME}/action/model/pose.cpp" "src/${PROJECT_NAME}/action/node/action_manager.cpp" "src/${PROJECT_NAME}/action/node/action_node.cpp" @@ -32,9 +40,18 @@ add_library(${PROJECT_NAME} SHARED "src/${PROJECT_NAME}/action/process/joint_process.cpp" "src/${PROJECT_NAME}/config/node/config_node.cpp" "src/${PROJECT_NAME}/config/utils/config.cpp" - "src/${PROJECT_NAME}/node/akushon_node.cpp") + "src/${PROJECT_NAME}/config/grpc/config.cpp" + "src/${PROJECT_NAME}/config/grpc/call_data_base.cpp" + "src/${PROJECT_NAME}/config/grpc/call_data_get_config.cpp" + "src/${PROJECT_NAME}/config/grpc/call_data_save_config.cpp" + "src/${PROJECT_NAME}/config/grpc/call_data_publish_set_joints.cpp" + "src/${PROJECT_NAME}/config/grpc/call_data_publish_set_torques.cpp" + "src/${PROJECT_NAME}/config/grpc/call_data_subscribe_current_joints.cpp" + "src/${PROJECT_NAME}/config/grpc/call_data_run_action.cpp" + "src/${PROJECT_NAME}/node/akushon_node.cpp" + ) -target_include_directories(${PROJECT_NAME} PUBLIC +target_include_directories(${PROJECT_NAME} PUBLIC $ $) @@ -45,7 +62,13 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action std_msgs tachimawari - tachimawari_interfaces) + tachimawari_interfaces + gRPC) + +target_link_libraries(${PROJECT_NAME} + gRPC::grpc++_reflection + gRPC::grpc++ +) install(DIRECTORY "include" DESTINATION ".") @@ -53,7 +76,10 @@ install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION "lib" LIBRARY DESTINATION "lib" - RUNTIME DESTINATION "bin") + RUNTIME DESTINATION "bin" + INCLUDES DESTINATION "include") + +target_compile_options(${PROJECT_NAME} PRIVATE -fPIC) add_executable(action "src/action_main.cpp") target_include_directories(action PUBLIC diff --git a/include/akushon/config/grpc/call_data.hpp b/include/akushon/config/grpc/call_data.hpp new file mode 100644 index 0000000..0ce97ee --- /dev/null +++ b/include/akushon/config/grpc/call_data.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2024 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef AKUSHON__CONFIG__GRPC__CALL_DATA_HPP_ +#define AKUSHON__CONFIG__GRPC__CALL_DATA_HPP_ + +#include "akushon/config/grpc/call_data_base.hpp" +#include "akushon_interfaces/akushon.grpc.pb.h" +#include "akushon_interfaces/akushon.pb.h" +#include "grpc/support/log.h" +#include "grpcpp/grpcpp.h" + +namespace akushon +{ +template +class CallData : CallDataBase +{ +public: + CallData( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string & path); + void Proceed() override; + +protected: + virtual void AddNextToCompletionQueue() = 0; + + enum class CallStatus { CREATE, PROCESS, FINISH }; + + CallStatus status_; // The current serving state. + + akushon_interfaces::proto::Config::AsyncService * service_; + + const std::string & path_; + + grpc::ServerCompletionQueue * cq_; + grpc::ServerContext ctx_; + ConfigRequest request_; + ConfigReply reply_; + grpc::ServerAsyncResponseWriter responder_; +}; + +template +CallData::CallData( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string & path) +: status_(CallStatus::CREATE), service_(service), cq_(cq), responder_(&ctx_), path_(path) +{ +} + +template +void CallData::Proceed() +{ + switch (status_) { + case CallStatus::CREATE: + status_ = CallStatus::PROCESS; + WaitForRequest(); + break; + case CallStatus::PROCESS: + AddNextToCompletionQueue(); + HandleRequest(); + status_ = CallStatus::FINISH; + responder_.Finish(reply_, grpc::Status::OK, this); + break; + default: + delete this; + break; + } +} + +} // namespace akushon + +#endif // AKUSHON__CONFIG__GRPC__CALL_DATA_HPP_ diff --git a/include/akushon/config/grpc/call_data_base.hpp b/include/akushon/config/grpc/call_data_base.hpp new file mode 100644 index 0000000..cf9b317 --- /dev/null +++ b/include/akushon/config/grpc/call_data_base.hpp @@ -0,0 +1,38 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef __AKUSHON__CONFIG__GRPC__CALL_DATA_BASE_HPP__ +#define __AKUSHON__CONFIG__GRPC__CALL_DATA_BASE_HPP__ +namespace akushon +{ +class CallDataBase +{ +public: + CallDataBase(); + + virtual void Proceed() = 0; + +protected: + virtual void WaitForRequest() = 0; + virtual void HandleRequest() = 0; +}; +} // namespace akushon + +#endif // __AKUSHON__CONFIG__GRPC__CALL_DATA_BASE_HPP__ diff --git a/include/akushon/config/grpc/call_data_get_config.hpp b/include/akushon/config/grpc/call_data_get_config.hpp new file mode 100644 index 0000000..0c77422 --- /dev/null +++ b/include/akushon/config/grpc/call_data_get_config.hpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef AKUSHON__CONFIG__GRPC__CALL_DATA_GET_CONFIG_HPP_ +#define AKUSHON__CONFIG__GRPC__CALL_DATA_GET_CONFIG_HPP_ + +#include "akushon/config/grpc/call_data.hpp" + +namespace akushon +{ +class CallDataGetConfig +: CallData +{ +public: + CallDataGetConfig( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path); + +protected: + void AddNextToCompletionQueue() override; + void WaitForRequest() override; + void HandleRequest() override; +}; +} // namespace akushon + +#endif // AKUSHON__CONFIG__GRPC__CALL_DATA_GET_CONFIG_HPP_ \ No newline at end of file diff --git a/include/akushon/config/grpc/call_data_publish_set_joints.hpp b/include/akushon/config/grpc/call_data_publish_set_joints.hpp new file mode 100644 index 0000000..3622f9e --- /dev/null +++ b/include/akushon/config/grpc/call_data_publish_set_joints.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef AKUSHON__CONFIG__GRPC__CALL_DATA_PUBLISH_SET_JOINTS_HPP_ +#define AKUSHON__CONFIG__GRPC__CALL_DATA_PUBLISH_SET_JOINTS_HPP_ + +#include "akushon/config/grpc/call_data.hpp" +#include "tachimawari_interfaces/msg/set_joints.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace akushon +{ +class CallDataPublishSetJoints +: CallData +{ +public: + CallDataPublishSetJoints( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path, rclcpp::Node::SharedPtr& node); + +protected: + void AddNextToCompletionQueue() override; + void WaitForRequest() override; + void HandleRequest() override; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr set_joints_publisher_; +}; +} // namespace akushon + +#endif // AKUSHON__CONFIG__GRPC__CALL_DATA_PUBLISH_SET_JOINTS_HPP_ \ No newline at end of file diff --git a/include/akushon/config/grpc/call_data_publish_set_torques.hpp b/include/akushon/config/grpc/call_data_publish_set_torques.hpp new file mode 100644 index 0000000..1cba37e --- /dev/null +++ b/include/akushon/config/grpc/call_data_publish_set_torques.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef AKUSHON__CONFIG__GRPC__CALL_DATA_PUBLISH_SET_TORQUES_HPP_ +#define AKUSHON__CONFIG__GRPC__CALL_DATA_PUBLISH_SET_TORQUES_HPP_ + +#include "akushon/config/grpc/call_data.hpp" +#include "tachimawari_interfaces/msg/set_torques.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace akushon +{ +class CallDataPublishSetTorques +: CallData +{ +public: + CallDataPublishSetTorques( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path, rclcpp::Node::SharedPtr& node); + +protected: + virtual void AddNextToCompletionQueue() override; + virtual void WaitForRequest() override; + virtual void HandleRequest() override; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr set_torque_publisher_; +}; +} // namespace akushon + +#endif // AKUSHON__CONFIG__GRPC__CALL_DATA_PUBLISH_SET_TORQUES_HPP_ diff --git a/include/akushon/config/grpc/call_data_run_action.hpp b/include/akushon/config/grpc/call_data_run_action.hpp new file mode 100644 index 0000000..026051b --- /dev/null +++ b/include/akushon/config/grpc/call_data_run_action.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef AKUSHON__CONFIG__GRPC__CALL_DATA_RUN_ACTION_HPP_ +#define AKUSHON__CONFIG__GRPC__CALL_DATA_RUN_ACTION_HPP_ + +#include "akushon/config/grpc/call_data.hpp" +#include "akushon_interfaces/msg/run_action.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace akushon +{ +class CallDataRunAction +: CallData +{ +public: + CallDataRunAction( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path, rclcpp::Node::SharedPtr& node); + +protected: + void AddNextToCompletionQueue() override; + void WaitForRequest() override; + void HandleRequest() override; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr run_action_publisher_; +}; +} // namespace akushon + +#endif // AKUSHON__CONFIG__GRPC__CALL_DATA_RUN_ACTION_HPP_ \ No newline at end of file diff --git a/include/akushon/config/grpc/call_data_save_config.hpp b/include/akushon/config/grpc/call_data_save_config.hpp new file mode 100644 index 0000000..9643848 --- /dev/null +++ b/include/akushon/config/grpc/call_data_save_config.hpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef AKUSHON__CONFIG__GRPC__SAVE_CONFIG_HPP_ +#define AKUSHON__CONFIG__GRPC__SAVE_CONFIG_HPP_ + +#include "akushon/config/grpc/call_data.hpp" + +namespace akushon +{ +class CallDataSaveConfig +: CallData +{ +public: + CallDataSaveConfig( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path); + +protected: + void AddNextToCompletionQueue() override; + void WaitForRequest() override; + void HandleRequest() override; +}; +} // namespace akushon + +#endif // AKUSHON__CONFIG__GRPC__SAVE_CONFIG_HPP_ diff --git a/include/akushon/config/grpc/call_data_subscribe_current_joints.hpp b/include/akushon/config/grpc/call_data_subscribe_current_joints.hpp new file mode 100644 index 0000000..4e00f1e --- /dev/null +++ b/include/akushon/config/grpc/call_data_subscribe_current_joints.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef AKUSHON__CONFIG__GRPC__CALL_DATA_SUBSCRIBE_CURRENT_JOINTS_HPP_ +#define AKUSHON__CONFIG__GRPC__CALL_DATA_SUBSCRIBE_CURRENT_JOINTS_HPP_ + +#include "akushon/config/grpc/call_data.hpp" +#include "akushon_interfaces/msg/run_action.hpp" +#include "tachimawari_interfaces/msg/current_joints.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace akushon +{ +class CallDataSubscribeCurrentJoints +: CallData +{ +public: + CallDataSubscribeCurrentJoints( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path, rclcpp::Node::SharedPtr& node); + +protected: + void AddNextToCompletionQueue() override; + void WaitForRequest() override; + void HandleRequest() override; + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr + current_joint_subscription_; + tachimawari_interfaces::msg::CurrentJoints curr_joints_; +}; +} // namespace akushon + +#endif // AKUSHON__CONFIG__GRPC__CALL_DATA_SUBSCRIBE_CURRENT_JOINTS_HPP_ diff --git a/include/akushon/config/grpc/config.hpp b/include/akushon/config/grpc/config.hpp new file mode 100644 index 0000000..c254e49 --- /dev/null +++ b/include/akushon/config/grpc/config.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2024 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef AKUSHON__CONFIG__GRPC__CONFIG_HPP_ +#define AKUSHON__CONFIG__GRPC__CONFIG_HPP_ + +#include +#include + +#include "akushon_interfaces/akushon.grpc.pb.h" +#include "akushon_interfaces/akushon.pb.h" +#include "grpcpp/grpcpp.h" +#include "rclcpp/rclcpp.hpp" + +using akushon_interfaces::proto::Config; + +namespace akushon +{ +class ConfigGrpc +{ +public: + explicit ConfigGrpc(); + explicit ConfigGrpc(const std::string & path); + + ~ConfigGrpc(); + + void Run(uint16_t port, const std::string & path, rclcpp::Node::SharedPtr & node); + +private: + std::string path; + + static inline std::unique_ptr cq_; + static inline std::unique_ptr server_; + std::shared_ptr thread_; + akushon_interfaces::proto::Config::AsyncService service_; + + std::thread async_server; +}; + +} // namespace akushon + +#endif // AKUSHON__CONFIG__GRPC__CONFIG_HPP_ diff --git a/include/akushon/config/node/config_node.hpp b/include/akushon/config/node/config_node.hpp index f97c804..ad62aa2 100644 --- a/include/akushon/config/node/config_node.hpp +++ b/include/akushon/config/node/config_node.hpp @@ -25,6 +25,7 @@ #include #include "akushon/config/utils/config.hpp" +#include "akushon/config/grpc/config.hpp" #include "akushon_interfaces/srv/save_actions.hpp" #include "akushon_interfaces/srv/get_actions.hpp" #include "rclcpp/rclcpp.hpp" @@ -44,6 +45,7 @@ class ConfigNode std::string get_node_prefix() const; Config config_util; + ConfigGrpc config_grpc; rclcpp::Service::SharedPtr save_actions_service; rclcpp::Service::SharedPtr get_actions_service; diff --git a/src/akushon/config/grpc/call_data_base.cpp b/src/akushon/config/grpc/call_data_base.cpp new file mode 100644 index 0000000..0d7fc35 --- /dev/null +++ b/src/akushon/config/grpc/call_data_base.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "akushon/config/grpc/call_data_base.hpp" + +namespace akushon +{ +CallDataBase::CallDataBase() {} +} \ No newline at end of file diff --git a/src/akushon/config/grpc/call_data_get_config.cpp b/src/akushon/config/grpc/call_data_get_config.cpp new file mode 100644 index 0000000..485191c --- /dev/null +++ b/src/akushon/config/grpc/call_data_get_config.cpp @@ -0,0 +1,54 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "akushon/config/grpc/call_data_get_config.hpp" +#include "akushon/config/utils/config.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace akushon +{ + +CallDataGetConfig::CallDataGetConfig( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path) +: CallData(service, cq, path) +{ + Proceed(); +} + +void CallDataGetConfig::AddNextToCompletionQueue() +{ + new CallDataGetConfig(service_, cq_, path_); +} + +void CallDataGetConfig::WaitForRequest() +{ + service_->RequestGetConfig(&ctx_, &request_, &responder_, cq_, cq_, this); +} + +void CallDataGetConfig::HandleRequest() +{ + Config config(path_); + + reply_.set_json_actions(config.get_config()); + RCLCPP_INFO(rclcpp::get_logger("GetConfig"), "config has been sent!"); +} + +} // namespace akushon diff --git a/src/akushon/config/grpc/call_data_publish_set_joints.cpp b/src/akushon/config/grpc/call_data_publish_set_joints.cpp new file mode 100644 index 0000000..2d89890 --- /dev/null +++ b/src/akushon/config/grpc/call_data_publish_set_joints.cpp @@ -0,0 +1,69 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "akushon/config/grpc/call_data_publish_set_joints.hpp" +#include "akushon/config/utils/config.hpp" +#include "nlohmann/json.hpp" + +namespace akushon +{ +CallDataPublishSetJoints::CallDataPublishSetJoints( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path, rclcpp::Node::SharedPtr& node) +: CallData(service, cq, path), node_(node) +{ + set_joints_publisher_ = + node_->create_publisher("/joint/set_joints", 10); + Proceed(); +} + +void CallDataPublishSetJoints::AddNextToCompletionQueue() +{ + new CallDataPublishSetJoints(service_, cq_, path_, node_); +} + +void CallDataPublishSetJoints::WaitForRequest() +{ + service_->RequestPublishSetJoints(&ctx_, &request_, &responder_, cq_, cq_, this); +} + +void CallDataPublishSetJoints::HandleRequest() +{ + try { + int control_type = request_.control_type(); + tachimawari_interfaces::msg::SetJoints set_joints_; + set_joints_.control_type = control_type; + // set_joints_.joints.push_back(joint_actions); + nlohmann::json publish_set_joint = nlohmann::json::parse(request_.joints_actions()); + for (const auto & items_json_ : publish_set_joint["joints"].items()) { + nlohmann::json obj = items_json_.value(); + tachimawari_interfaces::msg::Joint joint; + joint.id = (uint8_t)obj.at("id").get(); + joint.position = (float)obj.at("position").get(); + set_joints_.joints.push_back(joint); + } + set_joints_publisher_->publish(set_joints_); + + RCLCPP_INFO(rclcpp::get_logger("PublishSetJoints"), "set joints has been published!"); + } catch (nlohmann::json::exception e) { + RCLCPP_ERROR(rclcpp::get_logger("PublishSetJoints"), e.what()); + } +} +} // namespace akushon diff --git a/src/akushon/config/grpc/call_data_publish_set_torques.cpp b/src/akushon/config/grpc/call_data_publish_set_torques.cpp new file mode 100644 index 0000000..2f9f79f --- /dev/null +++ b/src/akushon/config/grpc/call_data_publish_set_torques.cpp @@ -0,0 +1,66 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "akushon/config/grpc/call_data_publish_set_torques.hpp" +#include "akushon/config/utils/config.hpp" +#include "rclcpp/rclcpp.hpp" +#include "nlohmann/json.hpp" + +namespace akushon +{ +CallDataPublishSetTorques::CallDataPublishSetTorques( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path, rclcpp::Node::SharedPtr& node) +: CallData(service, cq, path), node_(node) +{ + set_torque_publisher_ = + node_->create_publisher("/joint/set_torques", 10); + Proceed(); +} + +void CallDataPublishSetTorques::AddNextToCompletionQueue() +{ + new CallDataPublishSetTorques(service_, cq_, path_, node_); +} + +void CallDataPublishSetTorques::WaitForRequest() +{ + service_->RequestSetTorques(&ctx_, &request_, &responder_, cq_, cq_, this); +} + +void CallDataPublishSetTorques::HandleRequest() +{ + try { + tachimawari_interfaces::msg::SetTorques set_torque; + set_torque.torque_enable = request_.torque_enable(); + + nlohmann::json publish_set_torque = nlohmann::json::parse(request_.ids()); + + for (const auto & items : publish_set_torque["id"].items()) { + set_torque.ids.push_back((uint8_t)items.value().get()); + } + set_torque_publisher_->publish(set_torque); + RCLCPP_INFO(rclcpp::get_logger("PublishSetTorques"), "set torques has been published!"); + + } catch (nlohmann::json::exception e) { + RCLCPP_ERROR(rclcpp::get_logger("PublishSetTorques"), e.what()); + } +} +} // namespace akushon diff --git a/src/akushon/config/grpc/call_data_run_action.cpp b/src/akushon/config/grpc/call_data_run_action.cpp new file mode 100644 index 0000000..a6d5510 --- /dev/null +++ b/src/akushon/config/grpc/call_data_run_action.cpp @@ -0,0 +1,56 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "akushon/config/grpc/call_data_run_action.hpp" +#include "akushon/config/utils/config.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace akushon +{ +CallDataRunAction::CallDataRunAction( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path, rclcpp::Node::SharedPtr& node) +: CallData(service, cq, path), node_(node) +{ + run_action_publisher_ = + node_->create_publisher("/action/run_action", 10); + Proceed(); +} + +void CallDataRunAction::AddNextToCompletionQueue() +{ + new CallDataRunAction(service_, cq_, path_, node_); +} + +void CallDataRunAction::WaitForRequest() +{ + service_->RequestRunAction(&ctx_, &request_, &responder_, cq_, cq_, this); +} + +void CallDataRunAction::HandleRequest() +{ + akushon_interfaces::msg::RunAction run_action; + run_action.control_type = request_.control_type(); + run_action.action_name = request_.action_name(); + run_action.json = request_.json_action(); + run_action_publisher_->publish(run_action); + RCLCPP_INFO(rclcpp::get_logger("PublishSetJoints"), "run action config has been published!"); +} +} // namespace akushon diff --git a/src/akushon/config/grpc/call_data_save_config.cpp b/src/akushon/config/grpc/call_data_save_config.cpp new file mode 100644 index 0000000..7e54d06 --- /dev/null +++ b/src/akushon/config/grpc/call_data_save_config.cpp @@ -0,0 +1,59 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "akushon/config/grpc/call_data_save_config.hpp" +#include "akushon/config/utils/config.hpp" +#include "rclcpp/rclcpp.hpp" +#include "nlohmann/json.hpp" + +namespace akushon +{ + +CallDataSaveConfig::CallDataSaveConfig( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path) +: CallData(service, cq, path) +{ + Proceed(); +} + +void CallDataSaveConfig::AddNextToCompletionQueue() +{ + new CallDataSaveConfig(service_, cq_, path_); +} + +void CallDataSaveConfig::WaitForRequest() +{ + service_->RequestSaveConfig(&ctx_, &request_, &responder_, cq_, cq_, this); +} + +void CallDataSaveConfig::HandleRequest() +{ + Config config(path_); + try { + nlohmann::json akushon_data = nlohmann::json::parse(request_.json_actions()); + config.save_config(akushon_data); + RCLCPP_INFO(rclcpp::get_logger("SaveConfig"), "config has been saved!"); + } catch (nlohmann::json::exception e) { + RCLCPP_ERROR(rclcpp::get_logger("SaveConfig"), e.what()); + } +} + +} // namespace akushon diff --git a/src/akushon/config/grpc/call_data_subscribe_current_joints.cpp b/src/akushon/config/grpc/call_data_subscribe_current_joints.cpp new file mode 100644 index 0000000..8c564a1 --- /dev/null +++ b/src/akushon/config/grpc/call_data_subscribe_current_joints.cpp @@ -0,0 +1,73 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "akushon/config/grpc/call_data_subscribe_current_joints.hpp" + +#include +#include + +#include "akushon/config/utils/config.hpp" +#include "nlohmann/json.hpp" + +namespace akushon +{ +CallDataSubscribeCurrentJoints::CallDataSubscribeCurrentJoints( + akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, + const std::string& path, rclcpp::Node::SharedPtr& node) +: CallData(service, cq, path), node_(node) +{ + Proceed(); +} // namespace akushon + +void CallDataSubscribeCurrentJoints::AddNextToCompletionQueue() +{ + new CallDataSubscribeCurrentJoints(service_, cq_, path_, node_); +} + +void CallDataSubscribeCurrentJoints::WaitForRequest() +{ + service_->RequestSubscribeCurrentJoints(&ctx_, &request_, &responder_, cq_, cq_, this); +} + +void CallDataSubscribeCurrentJoints::HandleRequest() +{ + try { + current_joint_subscription_ = + node_->create_subscription( + "/joint/current_joints", 10, + [this](const tachimawari_interfaces::msg::CurrentJoints::SharedPtr curr_joints) { + curr_joints_.joints = curr_joints->joints; + }); + nlohmann::json curr_joints; + + for (const auto & items : curr_joints_.joints) { + nlohmann::json obj; + obj["id"] = std::to_string(items.id); + obj["position"] = std::to_string(items.position); + curr_joints.push_back(obj); + } + reply_.set_msg_joints(curr_joints.dump()); + RCLCPP_INFO(rclcpp::get_logger("PublishSetTorques"), "curr joints has been sended!"); + current_joint_subscription_.reset(); + } catch (nlohmann::json::exception e) { + RCLCPP_ERROR(rclcpp::get_logger("PublishSetTorques"), e.what()); + } +} +} // namespace akushon diff --git a/src/akushon/config/grpc/config.cpp b/src/akushon/config/grpc/config.cpp new file mode 100644 index 0000000..b3589d5 --- /dev/null +++ b/src/akushon/config/grpc/config.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2023 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "akushon/config/grpc/config.hpp" + +#include +#include +#include +#include + +#include "akushon/config/utils/config.hpp" +#include "rclcpp/rclcpp.hpp" +#include "akushon/config/grpc/call_data_get_config.hpp" +#include "akushon/config/grpc/call_data_save_config.hpp" +#include "akushon/config/grpc/call_data_publish_set_joints.hpp" +#include "akushon/config/grpc/call_data_publish_set_torques.hpp" +#include "akushon/config/grpc/call_data_subscribe_current_joints.hpp" +#include "akushon/config/grpc/call_data_run_action.hpp" + +using grpc::ServerBuilder; +using namespace std::chrono_literals; + +namespace akushon +{ +ConfigGrpc::ConfigGrpc() {} +ConfigGrpc::ConfigGrpc(const std::string & path) : path(path) {} + +ConfigGrpc::~ConfigGrpc() +{ + server_->Shutdown(); + cq_->Shutdown(); +} + +void ConfigGrpc::Run(uint16_t port, const std::string& path, rclcpp::Node::SharedPtr& node) +{ + std::string server_address = absl::StrFormat("0.0.0.0:%d", port); + + ServerBuilder builder; + builder.AddListeningPort(server_address, grpc::InsecureServerCredentials()); + builder.RegisterService(&service_); + + cq_ = builder.AddCompletionQueue(); + server_ = builder.BuildAndStart(); + std::cout << "Server listening on " << server_address << std::endl; + + std::signal(SIGINT, [](int signum) { + server_->Shutdown(); + cq_->Shutdown(); + exit(signum); + }); + async_server = std::thread([path, this, &node]() { + new CallDataGetConfig(&service_, cq_.get(), path); + new CallDataSaveConfig(&service_, cq_.get(), path); + new CallDataPublishSetJoints(&service_, cq_.get(), path, node); + new CallDataPublishSetTorques(&service_, cq_.get(), path, node); + new CallDataRunAction(&service_, cq_.get(), path, node); + new CallDataSubscribeCurrentJoints(&service_, cq_.get(), path, node); + void * tag; // uniquely identifies a request. + bool ok = true; + while (true) { + this->cq_->Next(&tag, &ok); + if (ok) { + static_cast(tag)->Proceed(); + } + } + }); + std::this_thread::sleep_for(200ms); +} + +} // namespace akushon diff --git a/src/akushon/config/node/config_node.cpp b/src/akushon/config/node/config_node.cpp index 7baa907..ebc73b2 100644 --- a/src/akushon/config/node/config_node.cpp +++ b/src/akushon/config/node/config_node.cpp @@ -50,6 +50,8 @@ ConfigNode::ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path) response->status = "SAVED"; } ); + config_grpc.Run(5060, path, node); + RCLCPP_INFO(rclcpp::get_logger("GrpcServers"), "grpc running"); } std::string ConfigNode::get_node_prefix() const