Merge branch 'develop' into refactor/events/v1

Author: KalanaRatnayake

AUTHORS

@@ -1,4 +1,4 @@
 Michael Pritchard
 Thomas Muller-dardelin
-Kalana Ratnayake
-Buddhi Gamage
+Kalana Rathnayake
+Buddhi Gamage

capabilities2_capabilites/.clang-format

@@ -0,0 +1,64 @@
+
+BasedOnStyle: Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+BinPackParameters: true
+ColumnLimit: 120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: false
+PointerBindsToType: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakString: 1
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: false
+Standard: Auto
+IndentWidth: 2
+TabWidth: 2
+UseTab: Never
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles: false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+SortIncludes: false
+SpaceAfterCStyleCast: false
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}

capabilities2_capabilites/CMakeLists.txt

@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.8)
+project(capabilities2_capabilites)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(capabilities2_msgs REQUIRED)
+find_package(capabilities2_runner REQUIRED)
+find_package(capabilities2_utils REQUIRED)
+find_package(event_logger REQUIRED)
+find_package(event_logger_msgs REQUIRED)
+find_package(tinyxml2_vendor REQUIRED)
+find_package(TinyXML2 REQUIRED)  # provided by tinyxml2 upstream, or tinyxml2_vendor
+
+include_directories(
+  include
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/capabilities2_runner.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  rclcpp_action
+  pluginlib
+  capabilities2_msgs
+  capabilities2_runner
+  capabilities2_utils
+  event_logger
+  event_logger_msgs
+  TinyXML2
+)
+
+pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml)
+
+install(TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY interfaces
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(DIRECTORY providers
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_package()

capabilities2_capabilites/include/capabilities2_capabilites/get_runner.hpp

@@ -0,0 +1,117 @@
+#pragma once
+
+#include <thread>
+
+#include <tinyxml2.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include <capabilities2_runner/service_runner.hpp>
+#include <capabilities2_msgs/srv/get_capability_specs.hpp>
+
+namespace capabilities2_runner
+{
+
+/**
+ * @brief Executor runner class
+ *
+ * Class to run capabilities2 executor action based capability
+ *
+ */
+class CapabilityGetRunner : public ServiceRunner<capabilities2_msgs::srv::GetCapabilitySpecs>
+{
+public:
+  CapabilityGetRunner() : ServiceRunner()
+  {
+  }
+
+  /**
+   * @brief Starter function for starting the action runner
+   *
+   * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities
+   * @param run_config runner configuration loaded from the yaml file
+   */
+  virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override
+  {
+    init_service(node, run_config, "/capabilities/get_capability_specs");
+  }
+
+protected:
+  /**
+   * @brief This generate goal function overrides the generate_goal() function from ActionRunner()
+   * @param parameters XMLElement that contains parameters in the format
+   '<Event name=follow_waypoints provider=WaypointRunner x='$value' y='$value' />'
+   * @return ActionT::Goal the generated goal
+   */
+  virtual capabilities2_msgs::srv::GetCapabilitySpecs::Request generate_request(tinyxml2::XMLElement* parameters,
+                                                                                int id) override
+  {
+    capabilities2_msgs::srv::GetCapabilitySpecs::Request request;
+    return request;
+  }
+
+  /**
+   * @brief Update on_success event parameters with new data from an CapabilitySpecs message if avaible.
+   *
+   * This function is used to inject new data into the XMLElement containing
+   * parameters related to the on_success trigger event
+   *
+   *  <CapabilitySpecs>
+   *      <CapabilitySpec package="robot_navigation" type="capability_interface"
+   * default_provider="robot_navigation/navigation_provider"> <content> name: Navigation dependencies:
+   *                  - MapServer
+   *                  - PathPlanner
+   *          </content>
+   *      </CapabilitySpec>
+   *      <CapabilitySpec package="robot_perception" type="capability_interface">
+   *          <content>
+   *              name: ObjectDetection
+   *              dependencies:
+   *                  - Camera
+   *                  - ImageProcessor
+   *          </content>
+   *      </CapabilitySpec>
+   *  </CapabilitySpecs>
+   *
+   * @param parameters
+   * @return std::string
+   */
+
+  virtual std::string update_on_success(std::string& parameters)
+  {
+    tinyxml2::XMLElement* element = convert_to_xml(parameters);
+
+    // Create the OccupancyGrid element as a child of the existing parameters element
+    tinyxml2::XMLElement* capabilitySpecsElement = element->GetDocument()->NewElement("CapabilitySpecs");
+    element->InsertEndChild(capabilitySpecsElement);
+
+    for (const auto& spec : response_->capability_specs)
+    {
+      // Create a <CapabilitySpec> element
+      tinyxml2::XMLElement* specElement = element->GetDocument()->NewElement("CapabilitySpec");
+
+      // Set attributes
+      specElement->SetAttribute("package", spec.package.c_str());
+      specElement->SetAttribute("type", spec.type.c_str());
+
+      if (!spec.default_provider.empty())
+      {
+        specElement->SetAttribute("default_provider", spec.default_provider.c_str());
+      }
+
+      // Add content as a child element inside <CapabilitySpec>
+      tinyxml2::XMLElement* contentElement = element->GetDocument()->NewElement("content");
+      contentElement->SetText(spec.content.c_str());
+      specElement->InsertEndChild(contentElement);
+
+      // Append <CapabilitySpec> to <CapabilitySpecs>
+      capabilitySpecsElement->InsertEndChild(specElement);
+    }
+
+    // Convert updated XML back to string and return
+    std::string result = convert_to_string(element);
+    return result;
+  }
+};
+
+}  // namespace capabilities2_runner

capabilities2_capabilites/include/capabilities2_capabilites/input_multiplex_all_runner.hpp

@@ -0,0 +1,55 @@
+#pragma once
+
+#include <capabilities2_runner/multiplex_base_runner.hpp>
+
+namespace capabilities2_runner
+{
+class InputMultiplexAllRunner : public MultiplexBaseRunner
+{
+public:
+  /**
+   * @brief Constructor which needs to be empty due to plugin semantics
+   */
+  InputMultiplexAllRunner() : MultiplexBaseRunner()
+  {
+  }
+
+  /**
+   * @brief trigger function to handle multiplexing of all inputs based on ALL condition
+   *
+   * @param parameters not used in this runner
+   */
+  virtual void trigger(const std::string& parameters) override
+  {
+    tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters);
+
+    int input_count = 0;
+
+    parameters_->QueryIntAttribute("input_count", &input_count);
+    parameters_->QueryIntAttribute("id", &runner_id);
+
+    if (input_count_tracker.find(runner_id) == input_count_tracker.end())
+    {
+      input_count_tracker[runner_id] = 1;
+      expected_input_count[runner_id] = input_count;
+
+      info_("has started the All condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs.");
+    }
+    else
+    {
+      input_count_tracker[runner_id] += 1;
+
+      info_("has received " + std::to_string(input_count_tracker[runner_id]) + "/" +
+            std::to_string(expected_input_count[runner_id]) + " inputs for ALL condition.");
+    }
+
+    if (input_count_tracker[runner_id] == expected_input_count[runner_id])
+    {
+      info_("has fullfilled the All condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs.");
+
+      executionThreadPool[runner_id] = std::thread(&InputMultiplexAllRunner::execution, this, runner_id);
+    }
+  }
+};
+
+}  // namespace capabilities2_runner

capabilities2_capabilites/include/capabilities2_capabilites/input_multiplex_any_runner.hpp

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <capabilities2_runner/multiplex_base_runner.hpp>
+
+namespace capabilities2_runner
+{
+class InputMultiplexAnyRunner : public MultiplexBaseRunner
+{
+public:
+  /**
+   * @brief Constructor which needs to be empty due to plugin semantics
+   */
+  InputMultiplexAnyRunner() : MultiplexBaseRunner()
+  {
+  }
+
+  /**
+   * @brief trigger function to handle multiplexing of all inputs based on ANY condition
+   *
+   * @param parameters not used in this runner
+   */
+  virtual void trigger(const std::string& parameters) override
+  {
+    info_("received new parameters for InputMultiplexAnyRunner : " + parameters);
+
+    tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters);
+
+    int input_count = -1;
+
+    parameters_->QueryIntAttribute("input_count", &input_count);
+    parameters_->QueryIntAttribute("id", &runner_id);
+
+    if (runner_id < 0 || input_count < 0)
+    {
+      throw runner_exception("UID or input_count not found in parameters");
+    }
+    else
+    {
+      info_("triggered with UID: " + std::to_string(runner_id) + " and input_count: " + std::to_string(input_count));
+    }
+
+    if (input_count_tracker.find(runner_id) == input_count_tracker.end())
+    {
+      input_count_tracker[runner_id] = 1;
+      expected_input_count[runner_id] = input_count;
+
+      info_("has started the ANY condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs.");
+    }
+    else
+    {
+      input_count_tracker[runner_id] += 1;
+
+      info_("has received " + std::to_string(input_count_tracker[runner_id]) + "/" +
+            std::to_string(expected_input_count[runner_id]) + " inputs for ANY condition.");
+    }
+
+    if (input_count_tracker[runner_id] > 0)
+    {
+      info_("has fullfilled the ANY condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs.");
+
+      executionThreadPool[runner_id] = std::thread(&InputMultiplexAnyRunner::execution, this, runner_id);
+    }
+  }
+};
+
+}  // namespace capabilities2_runner