diff --git a/include/akushon/config/grpc/call_data_save_config.hpp b/include/akushon/config/grpc/call_data_save_config.hpp index 9643848..1a0af83 100644 --- a/include/akushon/config/grpc/call_data_save_config.hpp +++ b/include/akushon/config/grpc/call_data_save_config.hpp @@ -21,6 +21,7 @@ #ifndef AKUSHON__CONFIG__GRPC__SAVE_CONFIG_HPP_ #define AKUSHON__CONFIG__GRPC__SAVE_CONFIG_HPP_ +#include "akushon/action/node/action_manager.hpp" #include "akushon/config/grpc/call_data.hpp" namespace akushon @@ -31,12 +32,13 @@ class CallDataSaveConfig public: CallDataSaveConfig( akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, - const std::string& path); + const std::string& path, const std::shared_ptr& action_manager); protected: void AddNextToCompletionQueue() override; void WaitForRequest() override; void HandleRequest() override; + std::shared_ptr action_manager_; }; } // namespace akushon diff --git a/include/akushon/config/grpc/config.hpp b/include/akushon/config/grpc/config.hpp index c254e49..a0c8cc7 100644 --- a/include/akushon/config/grpc/config.hpp +++ b/include/akushon/config/grpc/config.hpp @@ -24,6 +24,7 @@ #include #include +#include "akushon/action/node/action_manager.hpp" #include "akushon_interfaces/akushon.grpc.pb.h" #include "akushon_interfaces/akushon.pb.h" #include "grpcpp/grpcpp.h" @@ -41,7 +42,8 @@ class ConfigGrpc ~ConfigGrpc(); - void Run(uint16_t port, const std::string & path, rclcpp::Node::SharedPtr & node); + void Run(uint16_t port, const std::string & path, rclcpp::Node::SharedPtr & node, + const std::shared_ptr & action_manager); private: std::string path; @@ -50,6 +52,7 @@ class ConfigGrpc static inline std::unique_ptr server_; std::shared_ptr thread_; akushon_interfaces::proto::Config::AsyncService service_; + std::shared_ptr action_manager_; std::thread async_server; }; diff --git a/include/akushon/config/node/config_node.hpp b/include/akushon/config/node/config_node.hpp index ad62aa2..6c96304 100644 --- a/include/akushon/config/node/config_node.hpp +++ b/include/akushon/config/node/config_node.hpp @@ -39,7 +39,8 @@ class ConfigNode using SaveActions = akushon_interfaces::srv::SaveActions; using GetActions = akushon_interfaces::srv::GetActions; - explicit ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path); + explicit ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path, + const std::shared_ptr & action_manager); private: std::string get_node_prefix() const; diff --git a/include/akushon/node/akushon_node.hpp b/include/akushon/node/akushon_node.hpp index 17e0820..3de34d9 100644 --- a/include/akushon/node/akushon_node.hpp +++ b/include/akushon/node/akushon_node.hpp @@ -42,7 +42,7 @@ class AkushonNode void run_action_manager(std::shared_ptr action_manager); - void run_config_service(const std::string & path); + void run_config_service(const std::string & path, const std::shared_ptr & action_manager); private: double start_time; diff --git a/src/akushon/config/grpc/call_data_save_config.cpp b/src/akushon/config/grpc/call_data_save_config.cpp index a6f4956..2bd261f 100644 --- a/src/akushon/config/grpc/call_data_save_config.cpp +++ b/src/akushon/config/grpc/call_data_save_config.cpp @@ -28,15 +28,15 @@ namespace akushon CallDataSaveConfig::CallDataSaveConfig( akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, - const std::string& path) -: CallData(service, cq, path) + const std::string& path, const std::shared_ptr& action_manager) +: CallData(service, cq, path), action_manager_(action_manager) { Proceed(); } void CallDataSaveConfig::AddNextToCompletionQueue() { - new CallDataSaveConfig(service_, cq_, path_); + new CallDataSaveConfig(service_, cq_, path_, action_manager_); } void CallDataSaveConfig::WaitForRequest() @@ -49,6 +49,7 @@ void CallDataSaveConfig::HandleRequest() Config config(path_); try { config.save_config(request_.json_actions()); + action_manager_->load_config(path_); RCLCPP_INFO(rclcpp::get_logger("SaveConfig"), "config has been saved!"); } catch (nlohmann::json::exception e) { RCLCPP_ERROR(rclcpp::get_logger("SaveConfig"), e.what()); diff --git a/src/akushon/config/grpc/config.cpp b/src/akushon/config/grpc/config.cpp index b3589d5..e83740d 100644 --- a/src/akushon/config/grpc/config.cpp +++ b/src/akushon/config/grpc/config.cpp @@ -48,7 +48,8 @@ ConfigGrpc::~ConfigGrpc() cq_->Shutdown(); } -void ConfigGrpc::Run(uint16_t port, const std::string& path, rclcpp::Node::SharedPtr& node) +void ConfigGrpc::Run(uint16_t port, const std::string& path, rclcpp::Node::SharedPtr& node, + const std::shared_ptr& action_manager) { std::string server_address = absl::StrFormat("0.0.0.0:%d", port); @@ -65,9 +66,9 @@ void ConfigGrpc::Run(uint16_t port, const std::string& path, rclcpp::Node::Share cq_->Shutdown(); exit(signum); }); - async_server = std::thread([path, this, &node]() { + async_server = std::thread([path, this, &node, &action_manager]() { new CallDataGetConfig(&service_, cq_.get(), path); - new CallDataSaveConfig(&service_, cq_.get(), path); + new CallDataSaveConfig(&service_, cq_.get(), path, action_manager); new CallDataPublishSetJoints(&service_, cq_.get(), path, node); new CallDataPublishSetTorques(&service_, cq_.get(), path, node); new CallDataRunAction(&service_, cq_.get(), path, node); diff --git a/src/akushon/config/node/config_node.cpp b/src/akushon/config/node/config_node.cpp index ebc73b2..553bfcc 100644 --- a/src/akushon/config/node/config_node.cpp +++ b/src/akushon/config/node/config_node.cpp @@ -31,7 +31,8 @@ namespace akushon { -ConfigNode::ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path) +ConfigNode::ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path, + const std::shared_ptr & action_manager) : config_util(path) { get_actions_service = node->create_service( @@ -50,7 +51,7 @@ ConfigNode::ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path) response->status = "SAVED"; } ); - config_grpc.Run(5060, path, node); + config_grpc.Run(5060, path, node, action_manager); RCLCPP_INFO(rclcpp::get_logger("GrpcServers"), "grpc running"); } diff --git a/src/akushon/node/akushon_node.cpp b/src/akushon/node/akushon_node.cpp index c23af71..cd99066 100644 --- a/src/akushon/node/akushon_node.cpp +++ b/src/akushon/node/akushon_node.cpp @@ -55,9 +55,9 @@ void AkushonNode::run_action_manager(std::shared_ptr action_manag action_node = std::make_shared(node, action_manager); } -void AkushonNode::run_config_service(const std::string & path) +void AkushonNode::run_config_service(const std::string & path, const std::shared_ptr & action_manager) { - config_node = std::make_shared(node, path); + config_node = std::make_shared(node, path, action_manager); } } // namespace akushon diff --git a/src/akushon_main.cpp b/src/akushon_main.cpp index 235997a..535d34b 100644 --- a/src/akushon_main.cpp +++ b/src/akushon_main.cpp @@ -45,7 +45,7 @@ int main(int argc, char * argv[]) action_manager->load_config(path); akushon_node->run_action_manager(action_manager); - akushon_node->run_config_service(path); + akushon_node->run_config_service(path, action_manager); rclcpp::spin(node); rclcpp::shutdown();