Skip to content

Commit 736a0b3

Browse files
KalanaRatnayakeKalanaRatnayake
authored
Merge branch 'develop' into refactor/events/v1
2 parents a1649a9 + 7215b29 commit 736a0b3

17 files changed

Lines changed: 625 additions & 2 deletions

File tree

‎AUTHORS‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
11
Michael Pritchard
22
Thomas Muller-dardelin
3-
Kalana Ratnayake
4-
Buddhi Gamage
3+
Kalana Rathnayake
4+
Buddhi Gamage

‎capabilities2_capabilites/.clang-format‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
2+
BasedOnStyle: Google
3+
AccessModifierOffset: -2
4+
ConstructorInitializerIndentWidth: 2
5+
AlignEscapedNewlinesLeft: false
6+
AlignTrailingComments: true
7+
AllowAllParametersOfDeclarationOnNextLine: false
8+
AllowShortIfStatementsOnASingleLine: false
9+
AllowShortLoopsOnASingleLine: false
10+
AllowShortFunctionsOnASingleLine: None
11+
AlwaysBreakTemplateDeclarations: true
12+
AlwaysBreakBeforeMultilineStrings: false
13+
BreakBeforeBinaryOperators: false
14+
BreakBeforeTernaryOperators: false
15+
BreakConstructorInitializersBeforeComma: true
16+
BinPackParameters: true
17+
ColumnLimit: 120
18+
ConstructorInitializerAllOnOneLineOrOnePerLine: true
19+
DerivePointerBinding: false
20+
PointerBindsToType: true
21+
ExperimentalAutoDetectBinPacking: false
22+
IndentCaseLabels: true
23+
MaxEmptyLinesToKeep: 1
24+
NamespaceIndentation: None
25+
ObjCSpaceBeforeProtocolList: true
26+
PenaltyBreakBeforeFirstCallParameter: 19
27+
PenaltyBreakComment: 60
28+
PenaltyBreakString: 1
29+
PenaltyBreakFirstLessLess: 1000
30+
PenaltyExcessCharacter: 1000
31+
PenaltyReturnTypeOnItsOwnLine: 90
32+
SpacesBeforeTrailingComments: 2
33+
Cpp11BracedListStyle: false
34+
Standard: Auto
35+
IndentWidth: 2
36+
TabWidth: 2
37+
UseTab: Never
38+
IndentFunctionDeclarationAfterType: false
39+
SpacesInParentheses: false
40+
SpacesInAngles: false
41+
SpaceInEmptyParentheses: false
42+
SpacesInCStyleCastParentheses: false
43+
SpaceAfterControlStatementKeyword: true
44+
SpaceBeforeAssignmentOperators: true
45+
ContinuationIndentWidth: 4
46+
SortIncludes: false
47+
SpaceAfterCStyleCast: false
48+
49+
# Configure each individual brace in BraceWrapping
50+
BreakBeforeBraces: Custom
51+
52+
# Control of individual brace wrapping cases
53+
BraceWrapping: {
54+
AfterClass: 'true'
55+
AfterControlStatement: 'true'
56+
AfterEnum : 'true'
57+
AfterFunction : 'true'
58+
AfterNamespace : 'true'
59+
AfterStruct : 'true'
60+
AfterUnion : 'true'
61+
BeforeCatch : 'true'
62+
BeforeElse : 'true'
63+
IndentBraces : 'false'
64+
}

‎capabilities2_capabilites/CMakeLists.txt‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(capabilities2_capabilites)
3+
4+
# Default to C++17
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 17)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra -Wpedantic)
11+
endif()
12+
13+
find_package(ament_cmake REQUIRED)
14+
find_package(rclcpp REQUIRED)
15+
find_package(rclcpp_action REQUIRED)
16+
find_package(pluginlib REQUIRED)
17+
find_package(capabilities2_msgs REQUIRED)
18+
find_package(capabilities2_runner REQUIRED)
19+
find_package(capabilities2_utils REQUIRED)
20+
find_package(event_logger REQUIRED)
21+
find_package(event_logger_msgs REQUIRED)
22+
find_package(tinyxml2_vendor REQUIRED)
23+
find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor
24+
25+
include_directories(
26+
include
27+
)
28+
29+
add_library(${PROJECT_NAME} SHARED
30+
src/capabilities2_runner.cpp
31+
)
32+
33+
ament_target_dependencies(${PROJECT_NAME}
34+
rclcpp
35+
rclcpp_action
36+
pluginlib
37+
capabilities2_msgs
38+
capabilities2_runner
39+
capabilities2_utils
40+
event_logger
41+
event_logger_msgs
42+
TinyXML2
43+
)
44+
45+
pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml)
46+
47+
install(TARGETS ${PROJECT_NAME}
48+
EXPORT ${PROJECT_NAME}
49+
ARCHIVE DESTINATION lib
50+
LIBRARY DESTINATION lib
51+
RUNTIME DESTINATION bin
52+
)
53+
54+
install(DIRECTORY include/
55+
DESTINATION include
56+
)
57+
58+
install(DIRECTORY interfaces
59+
DESTINATION share/${PROJECT_NAME}
60+
)
61+
62+
install(DIRECTORY providers
63+
DESTINATION share/${PROJECT_NAME}
64+
)
65+
66+
ament_export_include_directories(include)
67+
ament_export_libraries(${PROJECT_NAME})
68+
69+
ament_package()

‎capabilities2_capabilites/include/capabilities2_capabilites/get_runner.hpp‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
#pragma once
2+
3+
#include <thread>
4+
5+
#include <tinyxml2.h>
6+
#include <rclcpp/rclcpp.hpp>
7+
#include <rclcpp_action/rclcpp_action.hpp>
8+
9+
#include <capabilities2_runner/service_runner.hpp>
10+
#include <capabilities2_msgs/srv/get_capability_specs.hpp>
11+
12+
namespace capabilities2_runner
13+
{
14+
15+
/**
16+
* @brief Executor runner class
17+
*
18+
* Class to run capabilities2 executor action based capability
19+
*
20+
*/
21+
class CapabilityGetRunner : public ServiceRunner<capabilities2_msgs::srv::GetCapabilitySpecs>
22+
{
23+
public:
24+
CapabilityGetRunner() : ServiceRunner()
25+
{
26+
}
27+
28+
/**
29+
* @brief Starter function for starting the action runner
30+
*
31+
* @param node shared pointer to the capabilities node. Allows to use ros node related functionalities
32+
* @param run_config runner configuration loaded from the yaml file
33+
*/
34+
virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override
35+
{
36+
init_service(node, run_config, "/capabilities/get_capability_specs");
37+
}
38+
39+
protected:
40+
/**
41+
* @brief This generate goal function overrides the generate_goal() function from ActionRunner()
42+
* @param parameters XMLElement that contains parameters in the format
43+
'<Event name=follow_waypoints provider=WaypointRunner x='$value' y='$value' />'
44+
* @return ActionT::Goal the generated goal
45+
*/
46+
virtual capabilities2_msgs::srv::GetCapabilitySpecs::Request generate_request(tinyxml2::XMLElement* parameters,
47+
int id) override
48+
{
49+
capabilities2_msgs::srv::GetCapabilitySpecs::Request request;
50+
return request;
51+
}
52+
53+
/**
54+
* @brief Update on_success event parameters with new data from an CapabilitySpecs message if avaible.
55+
*
56+
* This function is used to inject new data into the XMLElement containing
57+
* parameters related to the on_success trigger event
58+
*
59+
* <CapabilitySpecs>
60+
* <CapabilitySpec package="robot_navigation" type="capability_interface"
61+
* default_provider="robot_navigation/navigation_provider"> <content> name: Navigation dependencies:
62+
* - MapServer
63+
* - PathPlanner
64+
* </content>
65+
* </CapabilitySpec>
66+
* <CapabilitySpec package="robot_perception" type="capability_interface">
67+
* <content>
68+
* name: ObjectDetection
69+
* dependencies:
70+
* - Camera
71+
* - ImageProcessor
72+
* </content>
73+
* </CapabilitySpec>
74+
* </CapabilitySpecs>
75+
*
76+
* @param parameters
77+
* @return std::string
78+
*/
79+
80+
virtual std::string update_on_success(std::string& parameters)
81+
{
82+
tinyxml2::XMLElement* element = convert_to_xml(parameters);
83+
84+
// Create the OccupancyGrid element as a child of the existing parameters element
85+
tinyxml2::XMLElement* capabilitySpecsElement = element->GetDocument()->NewElement("CapabilitySpecs");
86+
element->InsertEndChild(capabilitySpecsElement);
87+
88+
for (const auto& spec : response_->capability_specs)
89+
{
90+
// Create a <CapabilitySpec> element
91+
tinyxml2::XMLElement* specElement = element->GetDocument()->NewElement("CapabilitySpec");
92+
93+
// Set attributes
94+
specElement->SetAttribute("package", spec.package.c_str());
95+
specElement->SetAttribute("type", spec.type.c_str());
96+
97+
if (!spec.default_provider.empty())
98+
{
99+
specElement->SetAttribute("default_provider", spec.default_provider.c_str());
100+
}
101+
102+
// Add content as a child element inside <CapabilitySpec>
103+
tinyxml2::XMLElement* contentElement = element->GetDocument()->NewElement("content");
104+
contentElement->SetText(spec.content.c_str());
105+
specElement->InsertEndChild(contentElement);
106+
107+
// Append <CapabilitySpec> to <CapabilitySpecs>
108+
capabilitySpecsElement->InsertEndChild(specElement);
109+
}
110+
111+
// Convert updated XML back to string and return
112+
std::string result = convert_to_string(element);
113+
return result;
114+
}
115+
};
116+
117+
} // namespace capabilities2_runner

‎capabilities2_capabilites/include/capabilities2_capabilites/input_multiplex_all_runner.hpp‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
#pragma once
2+
3+
#include <capabilities2_runner/multiplex_base_runner.hpp>
4+
5+
namespace capabilities2_runner
6+
{
7+
class InputMultiplexAllRunner : public MultiplexBaseRunner
8+
{
9+
public:
10+
/**
11+
* @brief Constructor which needs to be empty due to plugin semantics
12+
*/
13+
InputMultiplexAllRunner() : MultiplexBaseRunner()
14+
{
15+
}
16+
17+
/**
18+
* @brief trigger function to handle multiplexing of all inputs based on ALL condition
19+
*
20+
* @param parameters not used in this runner
21+
*/
22+
virtual void trigger(const std::string& parameters) override
23+
{
24+
tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters);
25+
26+
int input_count = 0;
27+
28+
parameters_->QueryIntAttribute("input_count", &input_count);
29+
parameters_->QueryIntAttribute("id", &runner_id);
30+
31+
if (input_count_tracker.find(runner_id) == input_count_tracker.end())
32+
{
33+
input_count_tracker[runner_id] = 1;
34+
expected_input_count[runner_id] = input_count;
35+
36+
info_("has started the All condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs.");
37+
}
38+
else
39+
{
40+
input_count_tracker[runner_id] += 1;
41+
42+
info_("has received " + std::to_string(input_count_tracker[runner_id]) + "/" +
43+
std::to_string(expected_input_count[runner_id]) + " inputs for ALL condition.");
44+
}
45+
46+
if (input_count_tracker[runner_id] == expected_input_count[runner_id])
47+
{
48+
info_("has fullfilled the All condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs.");
49+
50+
executionThreadPool[runner_id] = std::thread(&InputMultiplexAllRunner::execution, this, runner_id);
51+
}
52+
}
53+
};
54+
55+
} // namespace capabilities2_runner

‎capabilities2_capabilites/include/capabilities2_capabilites/input_multiplex_any_runner.hpp‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
#pragma once
2+
3+
#include <capabilities2_runner/multiplex_base_runner.hpp>
4+
5+
namespace capabilities2_runner
6+
{
7+
class InputMultiplexAnyRunner : public MultiplexBaseRunner
8+
{
9+
public:
10+
/**
11+
* @brief Constructor which needs to be empty due to plugin semantics
12+
*/
13+
InputMultiplexAnyRunner() : MultiplexBaseRunner()
14+
{
15+
}
16+
17+
/**
18+
* @brief trigger function to handle multiplexing of all inputs based on ANY condition
19+
*
20+
* @param parameters not used in this runner
21+
*/
22+
virtual void trigger(const std::string& parameters) override
23+
{
24+
info_("received new parameters for InputMultiplexAnyRunner : " + parameters);
25+
26+
tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters);
27+
28+
int input_count = -1;
29+
30+
parameters_->QueryIntAttribute("input_count", &input_count);
31+
parameters_->QueryIntAttribute("id", &runner_id);
32+
33+
if (runner_id < 0 || input_count < 0)
34+
{
35+
throw runner_exception("UID or input_count not found in parameters");
36+
}
37+
else
38+
{
39+
info_("triggered with UID: " + std::to_string(runner_id) + " and input_count: " + std::to_string(input_count));
40+
}
41+
42+
if (input_count_tracker.find(runner_id) == input_count_tracker.end())
43+
{
44+
input_count_tracker[runner_id] = 1;
45+
expected_input_count[runner_id] = input_count;
46+
47+
info_("has started the ANY condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs.");
48+
}
49+
else
50+
{
51+
input_count_tracker[runner_id] += 1;
52+
53+
info_("has received " + std::to_string(input_count_tracker[runner_id]) + "/" +
54+
std::to_string(expected_input_count[runner_id]) + " inputs for ANY condition.");
55+
}
56+
57+
if (input_count_tracker[runner_id] > 0)
58+
{
59+
info_("has fullfilled the ANY condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs.");
60+
61+
executionThreadPool[runner_id] = std::thread(&InputMultiplexAnyRunner::execution, this, runner_id);
62+
}
63+
}
64+
};
65+
66+
} // namespace capabilities2_runner

0 commit comments

Comments
 (0)