Merge pull request #44 from ichiro-its/hotfix/load-config-after-save

[Hotfix] - Load Config After Save
ichiro-its · Jun 9, 2024 · 8906cde · 8906cde
2 parents e8ac74c + d80eb49
commit 8906cde
Show file tree

Hide file tree

Showing 9 changed files with 24 additions and 15 deletions.
diff --git 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<ActionManager>& action_manager);
 
 protected:
   void AddNextToCompletionQueue() override;
   void WaitForRequest() override;
   void HandleRequest() override;
+  std::shared_ptr<ActionManager> action_manager_;
 };
 }  // namespace akushon
 

diff --git a/include/akushon/config/grpc/config.hpp b/include/akushon/config/grpc/config.hpp
@@ -24,6 +24,7 @@
 #include <memory>
 #include <thread>
 
+#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<akushon::ActionManager> & action_manager);
 
 private:
   std::string path;
@@ -50,6 +52,7 @@ class ConfigGrpc
   static inline std::unique_ptr<grpc::Server> server_;
   std::shared_ptr<std::thread> thread_;
   akushon_interfaces::proto::Config::AsyncService service_;
+  std::shared_ptr<akushon::ActionManager> action_manager_;
 
   std::thread async_server;
 };

diff --git 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<ActionManager> & 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
@@ -42,7 +42,7 @@ class AkushonNode
 
   void run_action_manager(std::shared_ptr<ActionManager> action_manager);
 
-  void run_config_service(const std::string & path);
+  void run_config_service(const std::string & path, const std::shared_ptr<ActionManager> & 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
@@ -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<ActionManager>& 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
@@ -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<ActionManager>& 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
@@ -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<ActionManager> & action_manager)
 : config_util(path)
 {
   get_actions_service = node->create_service<GetActions>(
@@ -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
@@ -55,9 +55,9 @@ void AkushonNode::run_action_manager(std::shared_ptr<ActionManager> action_manag
   action_node = std::make_shared<ActionNode>(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<ActionManager> & action_manager)
 {
-  config_node = std::make_shared<ConfigNode>(node, path);
+  config_node = std::make_shared<ConfigNode>(node, path, action_manager);
 }
 
 }  // namespace akushon
diff --git 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();