Added registerClientCallback and registerDependency methods

CMakeLists.txt

@@ -42,19 +42,19 @@ include_directories(
 find_package(rosidl_default_generators REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rr_interfaces REQUIRED)        # CHANGE
+find_package(rr_interfaces REQUIRED)
 
 add_executable(consumer src/ResourceConsumer.cpp)
 target_link_libraries(consumer rr_core)
-ament_target_dependencies(consumer rclcpp rr_interfaces Boost)     # CHANGE
+ament_target_dependencies(consumer rclcpp rr_interfaces Boost)
 
 add_executable(agent src/ResourceAgent.cpp)
 target_link_libraries(agent rr_core)
-ament_target_dependencies(agent rclcpp rr_interfaces Boost)     # CHANGE
+ament_target_dependencies(agent rclcpp rr_interfaces Boost)
 
 add_executable(producer src/ResourceProducer.cpp)
 target_link_libraries(producer rr_core)
-ament_target_dependencies(producer rclcpp rr_interfaces Boost)     # CHANGE
+ament_target_dependencies(producer rclcpp rr_interfaces Boost)
 
 install(TARGETS
   consumer
@@ -79,6 +79,12 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+if(NOT EXISTS "$ENV{HOME}/.temoto")
+  execute_process(
+    COMMAND ${CMAKE_COMMAND} -E make_directory "$ENV{HOME}/.temoto"
+  )
+endif()
+
 ament_export_include_directories(include)
 
 ament_package()

README.md

@@ -41,7 +41,7 @@ These nodes to the following:
 
 To run the nodes following commands can be used for the producer, agend and consumer:
 ```
-ros2 run temoto_resource_registrar producer;
-ros2 run temoto_resource_registrar agent;
-ros2 run temoto_resource_registrar consumer;
+ros2 run temoto_resource_registrar producer
+ros2 run temoto_resource_registrar agent
+ros2 run temoto_resource_registrar consumer
 ```

include/rr/ros2_resource_registrar.h

@@ -38,24 +38,25 @@ namespace temoto_resource_registrar
     {
     }
 
-    rclcpp::Node::SharedPtr getResourceRegistrarNode() {
+    rclcpp::Node::SharedPtr getRRNode() {
         return shared_from_this();
     }
 
     ~ResourceRegistrarRos2()
     {
-      // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Destroying RR Ros");
-      // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "unloading clients");
+      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Destroying RR Ros");
+      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "unloading clients");
       for (const std::string &client_id : clients_.getIds())
       {
         try
         {
-          // RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "unloading client" << client_id);
+          RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "unloading client: " << client_id);
           unloadClient(client_id);
+          RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "finished unloading client: " << client_id);
         }
         catch (...)
         {
-          //ROS_WARN_STREAM("unloading error for client " << client_id);
+          RCLCPP_WARN_STREAM(rclcpp::get_logger("rclcpp"), "unloading error for client: " << client_id);
         }
       }
     }
@@ -154,13 +155,6 @@ namespace temoto_resource_registrar
         RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[inner service] callback finished");
       };
 
-      while (!unload_clients_[client_name]->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
-        if (!rclcpp::ok()) {
-          RCLCPP_ERROR(rclcpp::get_logger("ros2_r_r.h"), "Interrupted while waiting for the service. Exiting.");
-        }
-        RCLCPP_INFO(rclcpp::get_logger("ros2_r_r.h"), "service not available, waiting again...");
-      }
-
       auto result = unload_clients_[client_name]->async_send_request(request, inner_client_callback);
       RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "waiting for future");
 
@@ -199,6 +193,28 @@ namespace temoto_resource_registrar
       return queryPair;
     }
 
+    template <class ClientQueryType>
+    void registerClientCallback(const std::string& rr_name,
+                                const std::string& server_name,
+                                const std::string& query_id,
+                                std::function<void(const std::shared_ptr<typename ClientQueryType::Request>&,
+                                                 std::shared_ptr<typename ClientQueryType::Response>,
+                                                 const temoto_resource_registrar::Status &)> user_callback)
+    {
+      std::string client_id = createClient<Ros2Client<ClientQueryType>>(rr_name, server_name);
+      // storeClientQueryStatusCb<Ros2Client<ClientQueryType>, std::function<void(ClientQueryType, Status)>>(client_id, query_id, user_callback);
+      storeClientQueryStatusCb<Ros2Client<ClientQueryType>>(client_id, query_id, user_callback);
+    }
+
+
+    void registerDependency(const std::string &rr_name,
+                            const std::string &query_id,
+                            const std::string &parent_query_id)
+    {
+      rr_catalog_->storeDependency(parent_query_id, rr_name, query_id);
+      autoSaveCatalog();
+    }
+
     /**
  * @brief Get the Ros Child Queries object. Used to get executed queries of a dependency of the query
  * defined. Takes in an query ID and the serverName the dependency used.

include/rr/ros2_server.h

@@ -194,8 +194,7 @@ class Ros2Server : public temoto_resource_registrar::RrServerBase
                                 wrapped_query,
                                 serialized_request,
                                 sanitizeAndSerialize(res));
-
-        //ROS_INFO_STREAM("Stored!");
+        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "===== Stored! =====");
       }
       catch (const resource_registrar::TemotoErrorStack &e)
       {
@@ -206,7 +205,7 @@ class Ros2Server : public temoto_resource_registrar::RrServerBase
         res->temoto_metadata.metadata = Serializer::serialize<temoto_resource_registrar::ResponseMetadata>(wrapped_query.responseMetadata());
       }
 
-      //ROS_INFO("Executing query finished callback");
+      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Executing query finished callback");
       transaction_callback_ptr_(temoto_resource_registrar::TransactionInfo(200, wrapped_query));
     }
     else
@@ -220,6 +219,7 @@ class Ros2Server : public temoto_resource_registrar::RrServerBase
 
     rr_catalog_->saveCatalog();
     //ROS_WARN_STREAM("server call end " << res.temoto_metadata.request_id << " " << id());
+    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "server call end");
   }
 
   void triggerCallback(const temoto_resource_registrar::Status &status) const

rr_core/include/temoto_resource_registrar/rr_catalog.h

@@ -138,6 +138,11 @@ namespace temoto_resource_registrar
     {
       TEMOTO_DEBUG_("saving catalog to: %s", (configuration_.location()).c_str());
       std::ofstream ofs(configuration_.location());
+      if (!ofs)
+      {
+          std::cout << "\033[1;35m [rr_catalog] Failed to open file for writing:  \033[0m" << (configuration_.location()).c_str() << std::endl;
+          return;
+      }
       boost::archive::binary_oarchive oa(ofs);
       oa << *(this);
       ofs.close();

src/ResourceConsumer.cpp

@@ -58,7 +58,7 @@ int main(int argc, char **argv)
   exec.add_node(rr);
   exec.spin();
 
-  rclcpp::shutdown();
   RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "shutdown");
+  rclcpp::shutdown();
   return 0;
 }