cx_features: add generated features for navgraph interface communication

Author: snoato

cx_bringup/params/clips_features_manager.yaml

@@ -1,6 +1,6 @@
 clips_features_manager:
   ros__parameters:
-    clips_features_list: ["clips_pddl_parser", "skill_execution", "clips_protobuf",  "ros_topic_publisher", "ros_topic_subscriber", "ros_service_requester"]
+    clips_features_list: ["clips_pddl_parser", "skill_execution", "clips_protobuf",  "ros_topic_publisher", "ros_topic_subscriber", "ros_service_requester", "navgraph_compute", "navgraph_set_bounding_box"]
     clips_features:
       mock_feature:
         plugin: "cx::MockFeature"
@@ -16,6 +16,14 @@ clips_features_manager:
         plugin: "cx::RosTopicSubscriberFeature"
       ros_service_requester:
         plugin: "cx::RosServiceRequesterFeature"
+      navgraph_compute:
+        plugin: "cx:NavGraphCompute"
+      navgraph_set_bounding_box:
+        plugin: "cx:NavGraphSetBoundingBox"
+      navgraph_set_bounding_box:
+        plugin: "cx:NavGraphGenerateWaitzones"
+      navgraph_set_bounding_box:
+        plugin: "cx:NavGraphUpdateStationByTag"
       clips_protobuf:
         plugin: "cx::ClipsProtobufFeature"
         feature_parameters: ["protobuf_path"]

cx_features/CMakeLists.txt

@@ -95,6 +95,22 @@ add_library(ros_topic_subscriber_feature SHARED src/cx_features/ros_communicatio
 ament_target_dependencies(ros_topic_subscriber_feature ${dependencies})
 target_link_libraries(ros_topic_subscriber_feature PkgConfig::CLIPSMM)
 
+add_library(navgraph_compute SHARED src/cx_features/ros_communication_feature/NavGraphCompute.cpp)
+ament_target_dependencies(navgraph_compute ${dependencies})
+target_link_libraries(navgraph_compute PkgConfig::CLIPSMM)
+
+add_library(navgraph_set_bounding_box SHARED src/cx_features/ros_communication_feature/NavGraphSetBoundingBox.cpp)
+ament_target_dependencies(navgraph_set_bounding_box ${dependencies})
+target_link_libraries(navgraph_set_bounding_box PkgConfig::CLIPSMM)
+
+add_library(navgraph_generate_waitzones SHARED src/cx_features/ros_communication_feature/NavGraphGenerateWaitzones.cpp)
+ament_target_dependencies(navgraph_generate_waitzones ${dependencies})
+target_link_libraries(navgraph_generate_waitzones PkgConfig::CLIPSMM)
+
+add_library(navgraph_update_station_by_tag SHARED src/cx_features/ros_communication_feature/NavGraphUpdateStationByTag.cpp)
+ament_target_dependencies(navgraph_update_station_by_tag ${dependencies})
+target_link_libraries(navgraph_update_station_by_tag PkgConfig::CLIPSMM)
+
 
 # END OF PLUGINS REGISTRATION
 add_library(${PROJECT_NAME} SHARED ${CX_FEATURES_SOURCES})
@@ -128,6 +144,10 @@ install(TARGETS
   ros_service_requester_feature
   ros_topic_subscriber_feature
   ros_topic_publisher_feature
+  navgraph_compute
+  navgraph_set_bounding_box
+  navgraph_generate_waitzones
+  navgraph_update_station_by_tag
   clips_pddl_parser_feature
   clips_protobuf_feature
   ${PROJECT_NAME}

cx_features/features_plugin.xml

@@ -39,4 +39,24 @@
       <description>Clips Feature providing Protobuf-based communication</description>
     </class>
   </library>
+  <library path="navgraph_compute">
+    <class type="cx::NavGraphCompute" base_class_type="cx::ClipsFeature">
+      <description>Clips Feature providing access to NavGraphInterfaceCompute ROS2 services</description>
+    </class>
+  </library>
+  <library path="navgraph_set_boundingBox">
+    <class type="cx::NavGraphSetBoundingBox" base_class_type="cx::ClipsFeature">
+      <description>Clips Feature providing access to NavGraphInterfaceSetBoundingBox ROS2 services</description>
+    </class>
+  </library>
+  <library path="navgraph_generate_waitzones">
+    <class type="cx::NavGraphGenerateWaitzones" base_class_type="cx::ClipsFeature">
+      <description>Clips Feature providing access to NavGraphInterfaceGenerateWaitzones ROS2 services</description>
+    </class>
+  </library>
+  <library path="navgraph_update_station_by_tag">
+    <class type="cx::NavGraphUpdateStationByTag" base_class_type="cx::ClipsFeature">
+      <description>Clips Feature providing access to NavGraphInterfaceUpdateStationByTag ROS2 services</description>
+    </class>
+  </library>
 </class_libraries>

cx_features/include/cx_features/NavGraphCompute.hpp

@@ -0,0 +1,79 @@
+/***************************************************************************
+ *  NavGraphCompute.hpp
+ *
+ *  Automatically Generated: 2023-11-30 14:32:29
+ ****************************************************************************/
+
+/*  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  Read the full text in the LICENSE.GPL file in the doc directory.
+ */
+
+#ifndef CX_FEATURES__NAVGRAPHCOMPUTE_HPP_
+#define CX_FEATURES__NAVGRAPHCOMPUTE_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+
+#include "cx_core/ClipsFeature.hpp"
+#include "cx_utils/LockSharedPtr.hpp"
+#include "cx_utils/NodeThread.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "cx_msgs/srv/nav_graph_interface_compute.hpp"
+
+namespace cx {
+
+class NavGraphCompute : public ClipsFeature, public rclcpp::Node {
+public:
+  NavGraphCompute();
+  ~NavGraphCompute();
+
+  void initialise(const std::string &feature_name) override;
+
+  bool clips_context_init(const std::string &env_name,
+                          LockSharedPtr<CLIPS::Environment> &clips) override;
+  bool clips_context_destroyed(const std::string &env_name) override;
+
+  std::string getFeatureName() const;
+
+private:
+  std::map<std::string, LockSharedPtr<CLIPS::Environment>> envs_;
+  std::thread spin_thread_;
+  std::map<
+      std::string,
+      std::map<std::string, rclcpp::Client<cx_msgs::srv::NavGraphInterfaceCompute>::SharedPtr>>
+      request_clients_;
+  std::map<
+      std::string,
+      std::map<std::string, std::shared_ptr<cx_msgs::srv::NavGraphInterfaceCompute::Request>>>
+      requests_;
+  void create_request(const std::string &env_name,
+                      const std::string &service_name);
+
+  void set_field_request(const std::string &env_name,
+                         const std::string &service_name,
+                         const std::string &field_name, CLIPS::Value value);
+
+  void set_array_request(const std::string &env_name,
+                         const std::string &service_name,
+                         const std::string &field_name, CLIPS::Values values);
+
+  void request_from_node(const std::string &env_name,
+                         const std::string &service_name);
+
+  void service_callback(
+      rclcpp::Client<cx_msgs::srv::NavGraphInterfaceCompute>::SharedFuture response,
+      std::string service_name, std::string env_name);
+};
+} // namespace cx
+#endif // !CX_FEATURES__NAVGRAPHCOMPUTE_HPP_

cx_features/include/cx_features/NavGraphGenerateWaitzones.hpp

@@ -0,0 +1,79 @@
+/***************************************************************************
+ *  NavGraphGenerateWaitzones.hpp
+ *
+ *  Automatically Generated: 2023-11-30 14:38:37
+ ****************************************************************************/
+
+/*  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  Read the full text in the LICENSE.GPL file in the doc directory.
+ */
+
+#ifndef CX_FEATURES__NAVGRAPHGENERATEWAITZONES_HPP_
+#define CX_FEATURES__NAVGRAPHGENERATEWAITZONES_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+
+#include "cx_core/ClipsFeature.hpp"
+#include "cx_utils/LockSharedPtr.hpp"
+#include "cx_utils/NodeThread.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "cx_msgs/srv/nav_graph_interface_generate_waitzones.hpp"
+
+namespace cx {
+
+class NavGraphGenerateWaitzones : public ClipsFeature, public rclcpp::Node {
+public:
+  NavGraphGenerateWaitzones();
+  ~NavGraphGenerateWaitzones();
+
+  void initialise(const std::string &feature_name) override;
+
+  bool clips_context_init(const std::string &env_name,
+                          LockSharedPtr<CLIPS::Environment> &clips) override;
+  bool clips_context_destroyed(const std::string &env_name) override;
+
+  std::string getFeatureName() const;
+
+private:
+  std::map<std::string, LockSharedPtr<CLIPS::Environment>> envs_;
+  std::thread spin_thread_;
+  std::map<
+      std::string,
+      std::map<std::string, rclcpp::Client<cx_msgs::srv::NavGraphInterfaceGenerateWaitzones>::SharedPtr>>
+      request_clients_;
+  std::map<
+      std::string,
+      std::map<std::string, std::shared_ptr<cx_msgs::srv::NavGraphInterfaceGenerateWaitzones::Request>>>
+      requests_;
+  void create_request(const std::string &env_name,
+                      const std::string &service_name);
+
+  void set_field_request(const std::string &env_name,
+                         const std::string &service_name,
+                         const std::string &field_name, CLIPS::Value value);
+
+  void set_array_request(const std::string &env_name,
+                         const std::string &service_name,
+                         const std::string &field_name, CLIPS::Values values);
+
+  void request_from_node(const std::string &env_name,
+                         const std::string &service_name);
+
+  void service_callback(
+      rclcpp::Client<cx_msgs::srv::NavGraphInterfaceGenerateWaitzones>::SharedFuture response,
+      std::string service_name, std::string env_name);
+};
+} // namespace cx
+#endif // !CX_FEATURES__NAVGRAPHGENERATEWAITZONES_HPP_

cx_features/include/cx_features/NavGraphSetBoundingBox.hpp

@@ -0,0 +1,79 @@
+/***************************************************************************
+ *  NavGraphSetBoundingBox.hpp
+ *
+ *  Automatically Generated: 2023-11-30 14:34:21
+ ****************************************************************************/
+
+/*  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  Read the full text in the LICENSE.GPL file in the doc directory.
+ */
+
+#ifndef CX_FEATURES__NAVGRAPHSETBOUNDINGBOX_HPP_
+#define CX_FEATURES__NAVGRAPHSETBOUNDINGBOX_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+
+#include "cx_core/ClipsFeature.hpp"
+#include "cx_utils/LockSharedPtr.hpp"
+#include "cx_utils/NodeThread.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "cx_msgs/srv/nav_graph_interface_set_bounding_box.hpp"
+
+namespace cx {
+
+class NavGraphSetBoundingBox : public ClipsFeature, public rclcpp::Node {
+public:
+  NavGraphSetBoundingBox();
+  ~NavGraphSetBoundingBox();
+
+  void initialise(const std::string &feature_name) override;
+
+  bool clips_context_init(const std::string &env_name,
+                          LockSharedPtr<CLIPS::Environment> &clips) override;
+  bool clips_context_destroyed(const std::string &env_name) override;
+
+  std::string getFeatureName() const;
+
+private:
+  std::map<std::string, LockSharedPtr<CLIPS::Environment>> envs_;
+  std::thread spin_thread_;
+  std::map<
+      std::string,
+      std::map<std::string, rclcpp::Client<cx_msgs::srv::NavGraphInterfaceSetBoundingBox>::SharedPtr>>
+      request_clients_;
+  std::map<
+      std::string,
+      std::map<std::string, std::shared_ptr<cx_msgs::srv::NavGraphInterfaceSetBoundingBox::Request>>>
+      requests_;
+  void create_request(const std::string &env_name,
+                      const std::string &service_name);
+
+  void set_field_request(const std::string &env_name,
+                         const std::string &service_name,
+                         const std::string &field_name, CLIPS::Value value);
+
+  void set_array_request(const std::string &env_name,
+                         const std::string &service_name,
+                         const std::string &field_name, CLIPS::Values values);
+
+  void request_from_node(const std::string &env_name,
+                         const std::string &service_name);
+
+  void service_callback(
+      rclcpp::Client<cx_msgs::srv::NavGraphInterfaceSetBoundingBox>::SharedFuture response,
+      std::string service_name, std::string env_name);
+};
+} // namespace cx
+#endif // !CX_FEATURES__NAVGRAPHSETBOUNDINGBOX_HPP_