diff --git a/AUTHORS b/AUTHORS index 848e7ac..c86be4e 100644 --- a/AUTHORS +++ b/AUTHORS @@ -1,4 +1,4 @@ Michael Pritchard Thomas Muller-dardelin -Kalana Rathnayake +Kalana Ratnayake Buddhi Gamage diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt deleted file mode 100644 index 5e3a2d2..0000000 --- a/capabilities2_fabric/CMakeLists.txt +++ /dev/null @@ -1,159 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_fabric) - -# 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 dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(tinyxml2_vendor REQUIRED) -find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -find_package(bondcpp REQUIRED) -find_package(backward_ros REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_events REQUIRED) -find_package(capabilities2_utils REQUIRED) - -include_directories( - include -) - -############################################################################ -# capabilities2_fabric node executable -############################################################################ - -add_executable(${PROJECT_NAME} - src/capabilities_fabric_node.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - bondcpp - capabilities2_msgs - capabilities2_events - capabilities2_utils - TinyXML2 -) - -install(TARGETS ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} -) - -############################################################################ -# capabilities2_fabric component library -############################################################################ - -add_library(${PROJECT_NAME}_comp SHARED - src/capabilities_fabric_component.cpp -) - -ament_target_dependencies(${PROJECT_NAME}_comp - rclcpp - rclcpp_action - rclcpp_components - bondcpp - capabilities2_msgs - capabilities2_events - capabilities2_utils - TinyXML2 -) - -rclcpp_components_register_node(${PROJECT_NAME}_comp - PLUGIN "capabilities2_fabric::CapabilitiesFabric" - EXECUTABLE capabilities2_fabric_component -) - -ament_export_targets(capabilities2_fabric_component) - -install(TARGETS ${PROJECT_NAME}_comp - EXPORT capabilities2_fabric_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -############################################################################ -# fabric_client node executable -############################################################################ - -add_executable(fabric_client - src/capabilities_fabric_client_node.cpp -) - -ament_target_dependencies(fabric_client - rclcpp - rclcpp_action - capabilities2_msgs - capabilities2_events - capabilities2_utils - TinyXML2 -) - -install(TARGETS fabric_client - DESTINATION lib/${PROJECT_NAME} -) - -############################################################################ -# fabric_client component library -############################################################################ - -add_library(fabric_client_comp SHARED - src/capabilities_fabric_client_component.cpp -) - -ament_target_dependencies(fabric_client_comp - rclcpp - rclcpp_action - rclcpp_components - bondcpp - capabilities2_msgs - capabilities2_events - capabilities2_utils - TinyXML2 -) - -rclcpp_components_register_node(fabric_client_comp - PLUGIN "capabilities2_fabric::CapabilitiesFabricClient" - EXECUTABLE capabilities2_fabric_client_component -) - -ament_export_targets(capabilities2_fabric_client_component) - -install(TARGETS fabric_client_comp - EXPORT capabilities2_fabric_client_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -############################################################################ -# Miscellaneous installations -############################################################################ - -install(DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY plans - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml deleted file mode 100644 index 8a94606..0000000 --- a/capabilities2_fabric/config/fabric.yaml +++ /dev/null @@ -1,9 +0,0 @@ -fabric_client: - ros__parameters: - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_4.xml" # PromptPlanRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_3.xml" # PromptPoseRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_2.xml" # PromptOccupancyRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/prompt_1.xml" # PromptCapabilityRunner Example - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_2.xml" # WaypointRunner Example 2 - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/navigation_1.xml" # WaypointRunner Example 1 - # plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" diff --git a/capabilities2_fabric/docs/prompt_capability_runner_ex1.md b/capabilities2_fabric/docs/prompt_capability_runner_ex1.md deleted file mode 100644 index 0028ccb..0000000 --- a/capabilities2_fabric/docs/prompt_capability_runner_ex1.md +++ /dev/null @@ -1,45 +0,0 @@ -## PromptCapabilityRunner Example - -### Dependencies - -This example uses prompt tools stack. Follow instructions from [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup Prompt tools stack. - -### Plan selection - -Uncomment the line related to `prompt_1.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the Prompt Tools stack - -```bash -source install/setup.bash -ros2 launch prompt_bridge prompt_bridge.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md b/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md deleted file mode 100644 index 52c6ea3..0000000 --- a/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md +++ /dev/null @@ -1,59 +0,0 @@ -## PromptOccupancyRunner Example - -### Dependencies - -This example uses prompt tools stack, nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack and [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `prompt_2.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Prompt Tools stack - -```bash -source install/setup.bash -ros2 launch prompt_bridge prompt_bridge.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_plan_runner_ex1.md b/capabilities2_fabric/docs/prompt_plan_runner_ex1.md deleted file mode 100644 index bdd8b30..0000000 --- a/capabilities2_fabric/docs/prompt_plan_runner_ex1.md +++ /dev/null @@ -1,59 +0,0 @@ -## PromptPlanRunner Example - -### Dependencies - -This example uses prompt tools stack, nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack and [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `prompt_4.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Prompt Tools stack - -```bash -source install/setup.bash -ros2 launch prompt_bridge prompt_bridge.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/prompt_pose_runner_ex1.md b/capabilities2_fabric/docs/prompt_pose_runner_ex1.md deleted file mode 100644 index d572598..0000000 --- a/capabilities2_fabric/docs/prompt_pose_runner_ex1.md +++ /dev/null @@ -1,59 +0,0 @@ -## PromptPoseRunner Example - -### Dependencies - -This example uses prompt tools stack, nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack and [Propmt Tools Dependency Installation](../../docs/prompt_tools_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `prompt_3.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Prompt Tools stack - -```bash -source install/setup.bash -ros2 launch prompt_bridge prompt_bridge.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md deleted file mode 100644 index ef0d785..0000000 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ /dev/null @@ -1,52 +0,0 @@ -## WaypointRunner Example 1 - Single Goal - -### Dependencies - -This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `navigation_1.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md deleted file mode 100644 index b13df68..0000000 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ /dev/null @@ -1,52 +0,0 @@ -## WaypointRunner Example 2 - Goal Sequence - -### Dependencies - -This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. - -### Plan selection - -Uncomment the line related to `navigation_2.xml` in the `config/fabric,yaml` file - -### Build the package to apply changes - -In the workspace root run, - -```bash -colcon build -``` - -### Start the turtlebot simulation - -```bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py -``` - -### Start the Navigation2 stack - -```bash -source install/setup.bash -ros2 launch nav_stack system.launch.py -``` - -### Start the Capabilities2 Server - -```bash -source install/setup.bash -ros2 launch capabilities2_server server.launch.py -``` - -### Start the Capabilities2 Fabric - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - -### Start the Capabilities2 Event Listener (Optional for Debugging) - -```bash -source install/setup.bash -ros2 launch capabilities2_events listener.launch.py -``` \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp deleted file mode 100644 index 6372066..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ /dev/null @@ -1,874 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -/** - * @brief Capabilities Fabric - * - * Capabilities fabric node that provides a ROS client for the capabilities server. - * Able to receive a XML file that implements a plan via action server that it exposes - * - */ - -class CapabilitiesFabric : public rclcpp::Node -{ -public: - using Plan = capabilities2_msgs::action::Plan; - using GoalHandlePlan = rclcpp_action::ServerGoalHandle; - - using GetInterfaces = capabilities2_msgs::srv::GetInterfaces; - using GetSemanticInterfaces = capabilities2_msgs::srv::GetSemanticInterfaces; - using GetProviders = capabilities2_msgs::srv::GetProviders; - using EstablishBond = capabilities2_msgs::srv::EstablishBond; - using UseCapability = capabilities2_msgs::srv::UseCapability; - using FreeCapability = capabilities2_msgs::srv::FreeCapability; - using ConfigureCapability = capabilities2_msgs::srv::ConfigureCapability; - using TriggerCapability = capabilities2_msgs::srv::TriggerCapability; - - using GetInterfacesClient = rclcpp::Client; - using GetSemanticInterfacesClient = rclcpp::Client; - using GetProvidersClient = rclcpp::Client; - using EstablishBondClient = rclcpp::Client; - using UseCapabilityClient = rclcpp::Client; - using FreeCapabilityClient = rclcpp::Client; - using ConfigureCapabilityClient = rclcpp::Client; - using TriggerCapabilityClient = rclcpp::Client; - - CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options) - { - try - { - // Only call setup if this object is already owned by a shared_ptr - if (shared_from_this()) - { - initialize(); - } - } - catch (const std::bad_weak_ptr&) - { - // Not yet safe — probably standalone without make_shared - } - } - - /** - * @brief Initializer function for Ccapabilities2 fabric. - * Configure the action server for the capabilieites fabric and configure server clients for the capability runners from the - * capabilities2 server - * - */ - void initialize() - { - control_tag_list = xml_parser::get_control_list(); - - event_ = std::make_shared(shared_from_this(), "fabric", "/events"); - - this->planner_server_ = rclcpp_action::create_server( - this, "/capabilities_fabric", std::bind(&CapabilitiesFabric::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&CapabilitiesFabric::handle_cancel, this, std::placeholders::_1), - std::bind(&CapabilitiesFabric::handle_accepted, this, std::placeholders::_1)); - - get_interfaces_client_ = this->create_client("/capabilities/get_interfaces"); - get_sem_interf_client_ = this->create_client("/capabilities/get_semantic_interfaces"); - get_providers_client_ = this->create_client("/capabilities/get_providers"); - establish_bond_client_ = this->create_client("/capabilities/establish_bond"); - use_capability_client_ = this->create_client("/capabilities/use_capability"); - free_capability_client_ = this->create_client("/capabilities/free_capability"); - trig_capability_client_ = this->create_client("/capabilities/trigger_capability"); - conf_capability_client_ = this->create_client("/capabilities/configure_capability"); - - // Wait for services to become available - check_service(!get_interfaces_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_interfaces"); - check_service(!get_sem_interf_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_semantic_interfaces"); - check_service(!get_providers_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_providers"); - check_service(!establish_bond_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/establish_bond"); - check_service(!use_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/use_capability"); - check_service(!free_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/free_capability"); - check_service(!trig_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/trigger_capability"); - check_service(!conf_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/configure_capability"); - - result_msg = std::make_shared(); - } - -private: - /** - * @brief Handle the goal request that comes in from client. returns whether goal is accepted or rejected - * - * - * @param uuid uuid of the goal - * @param goal pointer to the action goal message - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) - { - event_->info("Received the goal request with the plan"); - - (void)uuid; - - event_->info("Following plan was received :\n\n " + goal->plan); - - // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file - tinyxml2::XMLError xml_status = document.Parse(goal->plan.c_str()); - - // check if the file parsing failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - event_->error("Parsing the plan from goal message failed"); - return rclcpp_action::GoalResponse::REJECT; - } - - event_->info("Plan parsed and accepted"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - /** - * @brief Handle the goal cancel request that comes in from client. - * - * @param goal_handle pointer to the action goal handle - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) - { - event_->info("Received the request to cancel the plan"); - (void)goal_handle; - - for (auto& [bond_id, bond_client] : bond_client_cache_) - { - bond_client->stop(); - } - - return rclcpp_action::CancelResponse::ACCEPT; - } - - /** - * @brief Handle the goal accept event originating from handle_goal. - * - * @param goal_handle pointer to the action goal handle - */ - void handle_accepted(const std::shared_ptr goal_handle) - { - goal_handle_ = goal_handle; - event_->info("Goal handle accepted"); - - execution(); - } - - /** - * @brief Start the execution of the capabilities2 fabric - */ - void execution() - { - event_->info("A new execution started"); - - xml_parser::add_closing_event(document); - xml_parser::convert_to_string(document, modified_plan); - - event_->info("Plan after adding closing event :\n\n " + modified_plan); - - interface_list.clear(); - providers_list.clear(); - rejected_list.clear(); - connection_map.clear(); - - expected_providers_ = 0; - completed_providers_ = 0; - - expected_interfaces_ = 0; - completed_interfaces_ = 0; - - expected_capabilities_ = 0; - completed_capabilities_ = 0; - freed_capabilities_ = 0; - - expected_configurations_ = 0; - completed_configurations_ = 0; - - getInterfaces(); - } - - /** - * @brief Get Interfaces available in the capabilities2 server via relavant service - */ - void getInterfaces() - { - event_->info("Requesting Interface information"); - - auto request_interface = std::make_shared(); - - // request data from the server - auto result_future = get_interfaces_client_->async_send_request(request_interface, [this](GetInterfacesClient::SharedFuture future) { - auto result = std::make_shared(); - - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to get Interface information. Server execution cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - auto response = future.get(); - expected_interfaces_ = response->interfaces.size(); - - event_->info("Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"); - - // Request each interface recursively for Semantic interfaces - getSemanticInterfaces(response->interfaces); - }); - } - - /** - * @brief Get the Semantic Interfaces from the capabilities2 server via related service client - * - * @param interfaces std::vector of interfaces for which the semantic interfaces will be requested - */ - void getSemanticInterfaces(const std::vector& interfaces) - { - std::string requested_interface = interfaces[completed_interfaces_]; - - event_->info("Requesting semantic interfaces for " + requested_interface); - - auto request_semantic = std::make_shared(); - request_semantic->interface = requested_interface; - - // request semantic interface from the server - auto result_semantic_future = get_sem_interf_client_->async_send_request( - request_semantic, [this, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to get Semantic Interface information. Server execution cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - completed_interfaces_++; - auto response = future.get(); - - // if semantic interfaces are availble for a given interface, add the semantic interface - if (response->semantic_interfaces.size() > 0) - { - for (const auto& semantic_interface : response->semantic_interfaces) - { - interface_list.push_back(semantic_interface); - is_semantic_list.push_back(true); - - event_->info(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + - " for " + requested_interface + ". So added " + semantic_interface); - } - } - // if no semantic interfaces are availble for a given interface, add the interface instead - else - { - interface_list.push_back(requested_interface); - is_semantic_list.push_back(false); - - event_->info(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + - requested_interface + ". So added " + requested_interface); - } - - if (completed_interfaces_ != expected_interfaces_) - { - // Request next interface recursively for Semantic interfaces - getSemanticInterfaces(interfaces); - } - else - { - event_->info("Received all requested Interface information"); - - expected_providers_ = interface_list.size(); - - event_->info("Requsting Provider information for " + std::to_string(expected_providers_) + " providers"); - - // request providers from the interfaces in the interfaces_list - getProvider(interface_list, is_semantic_list); - } - }); - } - - /** - * @brief Get the Provider information for the related interfaces - * - * @param interfaces std::vector of interfaces - * @param is_semantic std::vector of masks about interfaces with true value for semantic interfaces - */ - void getProvider(const std::vector& interfaces, const std::vector& is_semantic) - { - std::string requested_interface = interfaces[completed_providers_]; - bool semantic_flag = is_semantic[completed_providers_]; - - event_->info("Requesting provider for " + requested_interface); - - auto request_providers = std::make_shared(); - - // request providers of the semantic interface - request_providers->interface = requested_interface; - request_providers->include_semantic = semantic_flag; - - auto result_providers_future = get_providers_client_->async_send_request(request_providers, [this, is_semantic, requested_interface, interfaces]( - GetProvidersClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to retrieve providers for interface: " + requested_interface; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - completed_providers_++; - auto response = future.get(); - - if (response->default_provider != "") - { - // add defualt provider to the list - providers_list.push_back(response->default_provider); - - event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + - " for " + requested_interface + ". So added " + response->default_provider); - } - - // add additional providers to the list if available - if (response->providers.size() > 0) - { - for (const auto& provider : response->providers) - { - providers_list.push_back(provider); - - event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + - " for " + requested_interface); - } - } - else - { - event_->info(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface); - } - - // Check if all expected calls are completed before calling verify_plan - if (completed_providers_ != expected_providers_) - { - // request providers for the next interface in the interfaces_list - getProvider(interfaces, is_semantic); - } - else - { - event_->info("All requested interface, semantic interface and provider data recieved"); - - verify_and_continue(); - } - }); - } - - /** - * @brief Verify the plan before continuing the execution using xml parsing and collected interface, semantic interface - * and provider information - * - */ - void verify_and_continue() - { - event_->info("Verifying the plan"); - - bool verification_success = true; - - auto result = std::make_shared(); - - // extract the components within the 'plan' tags - bool extraction_success = false; - tinyxml2::XMLElement* plan = xml_parser::get_plan(document, extraction_success); - - if (!extraction_success) - { - result_msg->success = false; - result_msg->message = "Execution plan is not compatible. Please recheck and update"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - verification_success = false; - } - - event_->info("Plan extraction complete"); - - // verify whether the plan is valid by checking the tags - std::string error_message; - - if (!xml_parser::check_tags(plan, interface_list, providers_list, control_tag_list, rejected_list, error_message)) - { - result_msg->success = false; - result_msg->message = "Execution plan is faulty. Please recheck and update"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - verification_success = false; - } - - event_->info("Checking tags successful"); - - // verify the plan - if (!verification_success) - { - event_->info("Plan verification failed"); - - if (rejected_list.size() > 0) - { - // TODO: improve with error codes - auto result = std::make_shared(); - result->success = false; - result->message = "Plan verification failed. There are mismatched events"; - - for (const auto& rejected_element : rejected_list) - { - result->failed_elements.push_back(rejected_element); - } - - goal_handle_->abort(result); - event_->info(result->message); - } - else - { - // TODO: improve with error codes - result_msg->success = false; - result_msg->message = "Plan verification failed. Server Execution Cancelled."; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - event_->error("Server Execution Cancelled"); - } - - event_->info("Plan verification successful. Proceeding with connections extraction"); - - // Extract the connections from the plan - xml_parser::extract_connections(plan, connection_map); - - event_->info("Connection extraction successful"); - - // estasblish the bond with the server - request_bond(); - } - - /** - * @brief Request the bond from the capabilities2 server - * - */ - void request_bond() - { - event_->info("Requesting bond id"); - - // create bond establishing server request - auto request_bond = std::make_shared(); - - // send the request - auto result_future = establish_bond_client_->async_send_request(request_bond, [this](EstablishBondClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to retrieve the bond id. Server execution cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - auto response = future.get(); - bond_id_ = response->bond_id; - event_->info("Received the bond id : " + bond_id_); - - establish_bond(); - }); - } - - /** - * @brief establish the bond with capabilities2 server - * - */ - void establish_bond() - { - bond_client_cache_[bond_id_] = std::make_unique(shared_from_this(), bond_id_); - bond_client_cache_[bond_id_]->start(); - - event_->info("Bond sucessfully established with bond id : " + bond_id_); - - if (bond_client_cache_.size() > 1) - { - for (auto& [old_bond_id, bond_client] : bond_client_cache_) - { - if (old_bond_id != bond_id_) - { - bond_client->stop(); - event_->info("Stopping and removing old bond with id : " + old_bond_id); - } - } - } - - expected_capabilities_ = connection_map.size(); - - event_->info("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); - - use_capability(connection_map); - } - - /** - * @brief Request use of capability from capabilities2 server - * - * @param capabilities capability list to be started - * @param provider provider of the capability - */ - void use_capability(std::map& capabilities) - { - std::string capability = capabilities[completed_capabilities_].source.runner; - std::string provider = capabilities[completed_capabilities_].source.provider; - - auto request_use = std::make_shared(); - request_use->capability = capability; - request_use->preferred_provider = provider; - request_use->bond_id = bond_id_; - - event_->info("Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + - capabilities[completed_capabilities_].source.runner); - - // send the request - auto result_future = - use_capability_client_->async_send_request(request_use, [this, capability, provider](UseCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - - // release all capabilities that were used since not all started successfully - free_capability_all(connection_map); - - for (auto& [bond_id, bond_client] : bond_client_cache_) - { - bond_client->stop(); - } - return; - } - - completed_capabilities_++; - - auto response = future.get(); - - event_->info(std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"); - - // Check if all expected calls are completed before calling verify_plan - if (completed_capabilities_ == expected_capabilities_) - { - event_->info("All requested capabilities have been started. Configuring the capabilities with events"); - - expected_configurations_ = connection_map.size(); - - event_->info("Requsting capability configuration for " + std::to_string(expected_configurations_) + " capabilities"); - - configure_capabilities(connection_map); - } - else - { - use_capability(connection_map); - } - }); - } - - /** - * @brief Free all started capabilities in the capabilities map - * - * @param capabilities map of capabilities to be freed - */ - void free_capability_all(std::map& capabilities) - { - std::string capability = capabilities[freed_capabilities_].source.runner; - - auto request_free = std::make_shared(); - request_free->capability = capability; - request_free->bond_id = bond_id_; - - // send the request - auto result_future = free_capability_client_->async_send_request(request_free, [this, capability](FreeCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to free capability " + capability; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - auto response = future.get(); - event_->info("Successfully freed capability " + capability); - - freed_capabilities_++; - - // Check if all expected calls are completed before calling verify_plan - if (freed_capabilities_ == completed_capabilities_) - { - event_->info("All started capabilities have been freed."); - } - else - { - free_capability_all(connection_map); - } - }); - } - - /** - * @brief Request use of capability from capabilities2 server - */ - void configure_capabilities(std::map& capabilities) - { - auto request_configure = std::make_shared(); - - event_->info("Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + - capabilities[completed_configurations_].source.runner); - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) - { - request_configure->source.capability = capabilities[completed_configurations_].source.runner; - request_configure->source.provider = capabilities[completed_configurations_].source.provider; - } - else - { - request_configure->source.capability = ""; - request_configure->source.provider = ""; - } - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_start.parameters, - request_configure->target_on_start.parameters)) - { - request_configure->target_on_start.capability = capabilities[completed_configurations_].target_on_start.runner; - request_configure->target_on_start.provider = capabilities[completed_configurations_].target_on_start.provider; - } - else - { - request_configure->target_on_start.capability = ""; - request_configure->target_on_start.provider = ""; - } - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_stop.parameters, - request_configure->target_on_stop.parameters)) - { - request_configure->target_on_stop.capability = capabilities[completed_configurations_].target_on_stop.runner; - request_configure->target_on_stop.provider = capabilities[completed_configurations_].target_on_stop.provider; - } - else - { - request_configure->target_on_stop.capability = ""; - request_configure->target_on_stop.provider = ""; - } - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_success.parameters, - request_configure->target_on_success.parameters)) - { - request_configure->target_on_success.capability = capabilities[completed_configurations_].target_on_success.runner; - request_configure->target_on_success.provider = capabilities[completed_configurations_].target_on_success.provider; - } - else - { - request_configure->target_on_success.capability = ""; - request_configure->target_on_success.provider = ""; - } - - if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_failure.parameters, - request_configure->target_on_failure.parameters)) - { - request_configure->target_on_failure.capability = capabilities[completed_configurations_].target_on_failure.runner; - request_configure->target_on_failure.provider = capabilities[completed_configurations_].target_on_failure.provider; - } - else - { - request_configure->target_on_failure.capability = ""; - request_configure->target_on_failure.provider = ""; - } - - std::string source_capability = capabilities[completed_configurations_].source.runner; - - // send the request - auto result_future = - conf_capability_client_->async_send_request(request_configure, [this, source_capability](ConfigureCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - completed_configurations_++; - - auto response = future.get(); - - event_->info(std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + - " : Successfully configured capability : " + source_capability); - - // Check if all expected calls are completed before calling verify_plan - if (completed_configurations_ == expected_configurations_) - { - event_->info("All requested capabilities have been configured. Triggering the first capability"); - - trigger_first_node(); - } - else - { - configure_capabilities(connection_map); - } - }); - } - - /** - * @brief Trigger the first node - */ - void trigger_first_node() - { - auto request_trigger = std::make_shared(); - - std::string parameter_string; - xml_parser::convert_to_string(connection_map[0].source.parameters, parameter_string); - request_trigger->capability = connection_map[0].source.runner; - request_trigger->parameters = parameter_string; - - // send the request - auto result_future = trig_capability_client_->async_send_request(request_trigger, [this](TriggerCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - result_msg->success = false; - result_msg->message = "Failed to trigger capability " + connection_map[0].source.runner; - event_->error(result_msg->message); - goal_handle_->abort(result_msg); - return; - } - - auto response = future.get(); - event_->info("Successfully triggered capability " + connection_map[0].source.runner); - - result_msg->success = true; - result_msg->message = "Successfully completed capabilities2 fabric"; - event_->info(result_msg->message); - goal_handle_->succeed(result_msg); - }); - } - - void check_service(bool wait_for_logic, const std::string& service_name) - { - while (wait_for_logic) - { - event_->error(service_name + " not available"); - rclcpp::shutdown(); - return; - } - event_->info(service_name + " connected"); - } - -private: - /** File Path link */ - std::string plan_file_path; - - /** Modified plan with closing capabilities */ - std::string modified_plan; - - /** flag to select loading from file or accepting via action server */ - bool read_file; - - int expected_interfaces_; - int completed_interfaces_; - - int expected_providers_; - int completed_providers_; - - int expected_capabilities_; - int completed_capabilities_; - int freed_capabilities_; - - int expected_configurations_; - int completed_configurations_; - - /** Bond id */ - std::string bond_id_; - - /** Manages bond between capabilities server and this client */ - std::map> bond_client_cache_; - - /** XML Document */ - tinyxml2::XMLDocument document; - - /** vector of connections */ - std::map connection_map; - - /** Interface List */ - std::vector is_semantic_list; - - /** Interface List */ - std::vector interface_list; - - /** Providers List */ - std::vector providers_list; - - /** Control flow List */ - std::vector control_tag_list; - - /** Invalid events list */ - std::vector rejected_list; - - /** Result message for plan action server*/ - std::shared_ptr result_msg; - - /** action server that exposes executor*/ - std::shared_ptr> planner_server_; - - /** action server goal handle*/ - std::shared_ptr goal_handle_; - - /** Get interfaces from capabilities server */ - GetInterfacesClient::SharedPtr get_interfaces_client_; - - /** Get semantic interfaces from capabilities server */ - GetSemanticInterfacesClient::SharedPtr get_sem_interf_client_; - - /** Get providers from capabilities server */ - GetProvidersClient::SharedPtr get_providers_client_; - - /** establish bond */ - EstablishBondClient::SharedPtr establish_bond_client_; - - /** use an selected capability */ - UseCapabilityClient::SharedPtr use_capability_client_; - - /** free an selected capability */ - FreeCapabilityClient::SharedPtr free_capability_client_; - - /** configure an selected capability */ - ConfigureCapabilityClient::SharedPtr conf_capability_client_; - - /** trigger an selected capability */ - TriggerCapabilityClient::SharedPtr trig_capability_client_; - - /** Event client for publishing events */ - std::shared_ptr event_; - - /** capabilities2 server and fabric synchronization tools */ - // std::mutex mutex_; - // std::condition_variable cv_; - // bool fabric_completed_; - // std::unique_lock lock_; -}; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp deleted file mode 100644 index 1434d0a..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ /dev/null @@ -1,359 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include - -/** - * @brief Capabilities Executor File Parser - * - * Capabilities Executor File Parser node that provides a ROS client for the capabilities executor. - * Will read an XML file that implements a plan and send it to the server - */ - -class CapabilitiesFabricClient : public rclcpp::Node -{ - enum Status - { - IDLE, - RUNNING, - CANCELED, - ABORTED, - FAILED, - LAUNCHED, - COMPLETED - }; - -public: - using Plan = capabilities2_msgs::action::Plan; - using GoalHandlePlan = rclcpp_action::ClientGoalHandle; - - using GetFabricStatus = capabilities2_msgs::srv::GetFabricStatus; - using SetFabricPlan = capabilities2_msgs::srv::SetFabricPlan; - using CancelFabricPlan = capabilities2_msgs::srv::CancelFabricPlan; - using CompleteFabric = capabilities2_msgs::srv::CompleteFabric; - - CapabilitiesFabricClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric_Client", options) - { - try - { - // Only call setup if this object is already owned by a shared_ptr - if (shared_from_this()) - { - initialize(); - } - } - catch (const std::bad_weak_ptr&) - { - // Not yet safe — probably standalone without make_shared - } - } - - /** - * @brief Initializer function - * - */ - void initialize() - { - declare_parameter("plan_file_path", "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml"); - plan_file_path = get_parameter("plan_file_path").as_string(); - - fabric_state = Status::IDLE; - - event_ = std::make_shared(shared_from_this(), "client", "/events"); - - status_server_ = - this->create_service("/capabilities_fabric/get_status", std::bind(&CapabilitiesFabricClient::getStatusCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - plan_server_ = this->create_service( - "/capabilities_fabric/set_plan", std::bind(&CapabilitiesFabricClient::setPlanCallback, this, std::placeholders::_1, std::placeholders::_2)); - - cancel_server_ = - this->create_service("/capabilities_fabric/cancel_plan", std::bind(&CapabilitiesFabricClient::cancelPlanCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - completion_server_ = - this->create_service("/capabilities_fabric/set_completion", std::bind(&CapabilitiesFabricClient::setCompleteCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - // Create the action client for capabilities_fabric after the node is fully constructed - this->planner_client_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); - - if (!this->planner_client_->wait_for_action_server(std::chrono::seconds(5))) - { - event_->error("Action server not available after waiting"); - rclcpp::shutdown(); - return; - } - - event_->info("Sucessfully connected to the capabilities_fabric action server"); - - // try to load the file - tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); - - // check if the file loading failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - event_->error("Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName()); - rclcpp::shutdown(); - } - - event_->info("Plan loaded from : " + plan_file_path); - - std::string plan; - xml_parser::convert_to_string(document, plan); - - plan_queue.push_back(plan); - - goal_send_thread = std::thread(&CapabilitiesFabricClient::manage_goal, this); - } - -private: - void manage_goal() - { - while (plan_queue.size() > 0) - { - event_->info("Fabric client thread starting"); - - std::unique_lock lock(mutex_); - completed_ = false; - - send_goal(); - - event_->info("Fabric plan sent. Waiting for acceptance."); - - // Conditional wait - cv_.wait(lock, [this] { return completed_; }); - event_->info("Fabric client thread closing"); - } - } - - void send_goal() - { - // Create Plan Goal message - auto goal_msg = Plan::Goal(); - - // load the latest plan from the queue - goal_msg.plan = plan_queue[0]; - plan_queue.pop_front(); - - event_->info("Sending goal to the capabilities_fabric action server"); - - // send goal options - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // goal response callback - send_goal_options.goal_response_callback = [this](const GoalHandlePlan::SharedPtr& goal_handle) { - if (!goal_handle) - { - event_->error("Goal was rejected by server"); - fabric_state = Status::FAILED; - } - else - { - event_->info("Goal accepted by server, waiting for completion"); - goal_handle_ = goal_handle; - fabric_state = Status::RUNNING; - } - }; - - // result callback - send_goal_options.result_callback = [this](const GoalHandlePlan::WrappedResult& result) { - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - fabric_state = Status::LAUNCHED; - break; - case rclcpp_action::ResultCode::ABORTED: - event_->error("Goal was aborted"); - fabric_state = Status::ABORTED; - break; - case rclcpp_action::ResultCode::CANCELED: - event_->error("Goal was canceled"); - fabric_state = Status::CANCELED; - break; - default: - event_->error("Unknown result code"); - fabric_state = Status::FAILED; - break; - } - - if (result.result->success) - { - event_->info("Plan launched successfully"); - } - else - { - event_->error("Plan failed to launch"); - - if (result.result->failed_elements.size() > 0) - { - event_->error("Plan failed due to incompatible XMLElements in the plan"); - - for (const auto& failed_element : result.result->failed_elements) - event_->error_element(failed_element); - } - } - }; - - this->planner_client_->async_send_goal(goal_msg, send_goal_options); - } - - void cancelPlanCallback(const std::shared_ptr request, std::shared_ptr response) - { - if (fabric_state == Status::RUNNING) - { - event_->info("Plan canncelling requested"); - this->planner_client_->async_cancel_goal(goal_handle_); - } - - response->success = true; - } - - void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) - { - fabric_state = Status::COMPLETED; - event_->info("Plan completed successfully"); - completed_ = true; - cv_.notify_all(); - } - - void getStatusCallback(const std::shared_ptr request, std::shared_ptr response) - { - if (fabric_state == Status::IDLE) - { - response->status = GetFabricStatus::Response::FABRIC_IDLE; - } - else if (fabric_state == Status::RUNNING) - { - response->status = GetFabricStatus::Response::FABRIC_RUNNING; - } - else if (fabric_state == Status::CANCELED) - { - response->status = GetFabricStatus::Response::FABRIC_CANCELED; - } - else if (fabric_state == Status::ABORTED) - { - response->status = GetFabricStatus::Response::FABRIC_ABORTED; - } - else if (fabric_state == Status::FAILED) - { - response->status = GetFabricStatus::Response::FABRIC_FAILED; - } - else if (fabric_state == Status::LAUNCHED) - { - response->status = GetFabricStatus::Response::FABRIC_LAUNCHED; - } - else if (fabric_state == Status::COMPLETED) - { - response->status = GetFabricStatus::Response::FABRIC_COMPLETED; - } - else - { - response->status = GetFabricStatus::Response::UNKNOWN; - } - } - - void setPlanCallback(const std::shared_ptr request, std::shared_ptr response) - { - event_->info("Received the request with a plan"); - - // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file - tinyxml2::XMLError xml_status = documentChecking.Parse(request->plan.c_str()); - - // check if the file parsing failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - event_->info("Parsing the plan from service request message failed"); - response->success = false; - } - - event_->info("Plan parsed and valid"); - - plan_queue.push_back(request->plan); - - event_->info("Plan queued and waiting for execution"); - - if ((fabric_state == Status::RUNNING) or (fabric_state == Status::LAUNCHED)) - { - event_->info("Prior plan under exeution. Will defer the new plan"); - } - else - { - event_->info("Plan parsed and accepted"); - goal_send_thread = std::thread(&CapabilitiesFabricClient::manage_goal, this); - } - - response->success = true; - } - -private: - /** File Path link */ - std::string plan_file_path; - - /** Status message */ - std::string status; - - /** Vector of plans */ - std::deque plan_queue; - - /** XML Document */ - tinyxml2::XMLDocument document; - - /** XML Document */ - tinyxml2::XMLDocument documentChecking; - - /** Thread to manage sending goal */ - std::thread goal_send_thread; - - /** action client */ - rclcpp_action::Client::SharedPtr planner_client_; - - /** Goal handle for action client control */ - GoalHandlePlan::SharedPtr goal_handle_; - - /** server to get the status of the capabilities2 fabric */ - rclcpp::Service::SharedPtr status_server_; - - /** server to set a new plan to the capabilities2 fabric */ - rclcpp::Service::SharedPtr plan_server_; - - /** server to cancel the current plan in the capabilities2 fabric */ - rclcpp::Service::SharedPtr cancel_server_; - - /** server to get the status of the capabilities2 fabric */ - rclcpp::Service::SharedPtr completion_server_; - - /** Status of the fabric */ - Status fabric_state; - - /** Event client for publishing events */ - std::shared_ptr event_; - - /** mutex for threadpool synchronisation. */ - std::mutex mutex_; - - /** conditional variable for threadpool synchronisation */ - std::condition_variable cv_; - - /** flag for threadpool synchronisation. */ - bool completed_; -}; diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp deleted file mode 100644 index d3b8e92..0000000 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ /dev/null @@ -1,313 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include - -namespace xml_parser -{ -/** - * @brief extract elements related plan and return the first child element - * - * @param document XML document to extract plan from - * @param success boolean to indicate if the plan was found - * - * @return plan in the form of tinyxml2::XMLElement* - */ -tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document, bool& success) -{ - std::string plan_tag(document.FirstChildElement()->Name()); - - if (plan_tag == "Plan") - { - success = true; - return document.FirstChildElement("Plan")->FirstChildElement(); - } - else - { - success = false; - return nullptr; - } -} - -/** - * @brief search a string in a vector of strings - * - * @param list vector of strings to be searched - * @param value string to be searched in the vector - * - * @return `true` if value is found in list and `false` otherwise - */ -bool search(std::vector list, std::string value) -{ - return (std::find(list.begin(), list.end(), value) != list.end()); -} - -/** - * @brief convert XMLElement to std::string - * - * @param element XMLElement element to be converted - * @param paramters parameter to hold std::string - * - * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr - */ -bool convert_to_string(tinyxml2::XMLElement* element, std::string& parameters) -{ - if (element) - { - tinyxml2::XMLPrinter printer; - element->Accept(&printer); - parameters = printer.CStr(); - return true; - } - else - { - parameters = ""; - return false; - } -} - -/** - * @brief convert XMLDocument to std::string - * - * @param document element to be converted - * - * @return std::string converted document - */ -void convert_to_string(tinyxml2::XMLDocument& document_xml, std::string& document_string) -{ - tinyxml2::XMLPrinter printer; - document_xml.Print(&printer); - document_string = printer.CStr(); -} - -void add_closing_event(tinyxml2::XMLDocument& document) -{ - // Get the root element - tinyxml2::XMLElement* plan = document.FirstChildElement("Plan"); - - // Get the existing element inside - tinyxml2::XMLElement* innerControl = plan->FirstChildElement("Control"); - - // Create the outer element - tinyxml2::XMLElement* outerControl = document.NewElement("Control"); - outerControl->SetAttribute("name", "sequential"); - - // Clone the existing element instead of deleting it - tinyxml2::XMLElement* clonedControl = innerControl->DeepClone(&document)->ToElement(); - - // Insert the cloned inner control inside the new outer control - outerControl->InsertEndChild(clonedControl); - - // Create and append the new element - tinyxml2::XMLElement* newEvent = document.NewElement("Event"); - newEvent->SetAttribute("name", "std_capabilities/FabricCompletionRunner"); - newEvent->SetAttribute("provider", "std_capabilities/FabricCompletionRunner"); - outerControl->InsertEndChild(newEvent); - - // Remove the original innerControl (after cloning) - plan->DeleteChild(innerControl); - - // Append the new outer control to - plan->InsertEndChild(outerControl); -} - -/** - * @brief check the plan for invalid/unsupported control and event tags - * uses recursive approach to go through the plan - * - * @param event EventClient used for logging and event publishing - * @param element XML Element to be evaluated - * @param events list containing valid event tags - * @param providers list containing providers - * @param control list containing valid control tags - * @param rejected list containing invalid tags - * - * @return `true` if element valid and supported and `false` otherwise - */ -bool check_tags(tinyxml2::XMLElement* element, std::vector& events, std::vector& providers, - std::vector& control, std::vector& rejected, std::string& error) -{ - const char* name; - const char* provider; - - std::string parameter_string; - convert_to_string(element, parameter_string); - - element->QueryStringAttribute("name", &name); - element->QueryStringAttribute("provider", &provider); - - std::string nametag; - std::string providertag; - std::string typetag(element->Name()); - - if (name) - nametag = name; - else - nametag = ""; - - if (provider) - providertag = provider; - else - providertag = ""; - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = (element->NextSiblingElement() != nullptr); - bool foundInControl = xml_parser::search(control, nametag); - bool foundInEvents = xml_parser::search(events, nametag); - bool foundInProviders = xml_parser::search(providers, providertag); - bool returnValue = true; - - if (typetag == "Control") - { - if (!foundInControl) - { - error = "Control tag '" + nametag + "' not available in the valid list"; - rejected.push_back(parameter_string); - return false; - } - - if (hasChildren) - returnValue &= xml_parser::check_tags(element->FirstChildElement(), events, providers, control, rejected, error); - - if (hasSiblings) - returnValue &= xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected, error); - } - else if (typetag == "Event") - { - if (!foundInEvents || !foundInProviders) - { - error = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; - rejected.push_back(parameter_string); - return false; - } - - if (hasSiblings) - returnValue &= xml_parser::check_tags(element->NextSiblingElement(), events, providers, control, rejected, error); - } - else - { - error = "XML element is not valid :" + parameter_string; - rejected.push_back(parameter_string); - return false; - } - - return returnValue; -} - -/** - * @brief Returns control xml tags supported in extract_connections method - * - */ -std::vector get_control_list() -{ - std::vector tag_list; - - tag_list.push_back("sequential"); - tag_list.push_back("parallel"); - tag_list.push_back("recovery"); - - return tag_list; -} - -/** - * @brief parse through the plan and extract the connections - * - * @param element XML Element to be evaluated - * @param connections std::map containing extracted connections - * @param connection_id numerical id of the connection - * @param connection_type the type of connection - */ -int extract_connections(tinyxml2::XMLElement* element, std::map& connections, int connection_id = 0, - capabilities2::connection_type_t connection_type = capabilities2::connection_type_t::ON_SUCCESS) -{ - int predecessor_id; - - const char* name = element->Attribute("name"); - const char* provider = element->Attribute("provider"); - - std::string typetag(element->Name()); - - std::string nametag; - std::string providertag; - - if (name) - nametag = name; - else - nametag = ""; - - if (provider) - providertag = provider; - else - providertag = ""; - - bool hasChildren = (element->FirstChildElement() != nullptr); - bool hasSiblings = (element->NextSiblingElement() != nullptr); - - if (typetag == "Control") - { - if (nametag == "sequential") - { - if (hasChildren) - predecessor_id = - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_SUCCESS); - } - else if (nametag == "parallel") - { - if (hasChildren) - predecessor_id = - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_START); - } - else if (nametag == "recovery") - { - if (hasChildren) - predecessor_id = - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_FAILURE); - } - - if (hasSiblings) - { - predecessor_id = xml_parser::extract_connections(element->NextSiblingElement(), connections, predecessor_id + 1, connection_type); - } - - return predecessor_id; - } - else if (typetag == "Event") - { - capabilities2::node_t node; - - node.source.runner = nametag; - node.source.provider = providertag; - node.source.parameters = element; - - predecessor_id = connection_id - 1; - - while (connections.count(connection_id) > 0) - connection_id += 1; - - connections[connection_id] = node; - - if (connection_id != 0) - { - if (connection_type == capabilities2::connection_type_t::ON_SUCCESS) - connections[predecessor_id].target_on_success = connections[connection_id].source; - - else if (connection_type == capabilities2::connection_type_t::ON_START) - connections[predecessor_id].target_on_start = connections[connection_id].source; - - else if (connection_type == capabilities2::connection_type_t::ON_FAILURE) - connections[predecessor_id].target_on_failure = connections[connection_id].source; - } - - if (hasSiblings) - predecessor_id = extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); - else - predecessor_id += 1; // connection_id - - return predecessor_id; - } -} - -} // namespace xml_parser diff --git a/capabilities2_fabric/launch/fabric.launch.py b/capabilities2_fabric/launch/fabric.launch.py deleted file mode 100644 index 71493f1..0000000 --- a/capabilities2_fabric/launch/fabric.launch.py +++ /dev/null @@ -1,40 +0,0 @@ -''' -capabilities2_server launch file -''' - -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - """Generate launch description for capabilities2 server - - Returns: - LaunchDescription: The launch description for capabilities2 executor - """ - # load config file - fabric_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'fabric.yaml') - - capabilities2_fabric = Node( - package='capabilities2_fabric', - namespace='', - executable='capabilities2_fabric', - name='capabilities2_fabric', - output='screen' - ) - - fabric_client = Node( - package='capabilities2_fabric', - namespace='', - executable='fabric_client', - name='fabric_client', - parameters=[fabric_config], - output='screen' - ) - - return LaunchDescription([ - capabilities2_fabric, - fabric_client - ]) diff --git a/capabilities2_fabric/launch/fabric_composed.launch.py b/capabilities2_fabric/launch/fabric_composed.launch.py deleted file mode 100644 index 6215018..0000000 --- a/capabilities2_fabric/launch/fabric_composed.launch.py +++ /dev/null @@ -1,49 +0,0 @@ -''' -capabilities2_server launch file -''' - -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - """Generate launch description for capabilities2 server - - Returns: - LaunchDescription: The launch description for capabilities2 executor - """ - # load config file - fabric_config = os.path.join(get_package_share_directory('capabilities2_fabric'), 'config', 'fabric.yaml') - - # create bridge composition - fabric_container = ComposableNodeContainer( - name='capabilities2_fabric_container', - namespace='', - package='rclcpp_components', - executable='component_container_mt', - # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix - arguments=['--ros-args', '--log-level', 'info'], - composable_node_descriptions=[ - ComposableNode( - package='capabilities2_fabric', - plugin='CapabilitiesFabric', - name='capabilities2_fabric', - output='screen' - ), - ComposableNode( - package='capabilities2_fabric', - plugin='CapabilitiesFabricClient', - name='fabric_client', - parameters=[fabric_config], - output='screen' - ) - ] - ) - - return LaunchDescription([ - fabric_container - ]) diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml deleted file mode 100644 index a49f15b..0000000 --- a/capabilities2_fabric/plans/default.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml deleted file mode 100644 index 4dbae38..0000000 --- a/capabilities2_fabric/plans/navigation_1.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml deleted file mode 100644 index 5c283d1..0000000 --- a/capabilities2_fabric/plans/navigation_2.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml deleted file mode 100644 index 8508f12..0000000 --- a/capabilities2_fabric/plans/prompt_1.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml deleted file mode 100644 index 1f33259..0000000 --- a/capabilities2_fabric/plans/prompt_2.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml deleted file mode 100644 index 6fc5494..0000000 --- a/capabilities2_fabric/plans/prompt_3.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml deleted file mode 100644 index dde90ac..0000000 --- a/capabilities2_fabric/plans/prompt_4.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md deleted file mode 100644 index 3bfeb86..0000000 --- a/capabilities2_fabric/readme.md +++ /dev/null @@ -1,102 +0,0 @@ -# Capabilities2_fabric - -Capabilities2_fabric is a ROS2 package that provides a system to coordinate and manage various capabilities as defined by the Capabilities2 framework. This package extends the functionality of the Capabilities2 package to implement a control framework based on capabilities. It is designed to parse an execution plan given via an XML file and then to identify connections between various capabilities in the system. - -Currently the system support 3 types of Control fuctions `sequential`, `parallel` and `recovery`, and multitude of Event functions. - -## Features - -- Dynamic Capability Loading: Interacts with and manages capabilities defined by the capabilities2 framework. -- Flexible Workflow Execution: Parses XML-based plans and identifies event-driven callbacks for success, failure, or in-progress states. - - -## Launching capabilities2_fabric - -`capabilities2_fabric/plans` folder includes sample XML plans that can be used to test the system. New plans can be added to the same folder or a different location. - -Then modify the `capabilities2_fabric/config/fabric.yaml` file to change the active execution plan. -A number of plans are availabe with the package and included in the `fabric.yaml` file that has been commented out. Uncomment them to use. Make sure to leave only one line uncommented. - -```yaml -/**: - ros__parameters: - plan_file_path: "install/capabilities2_fabric/share/capabilities2_fabric/plans/default.xml" - -``` -Finally start the capabilities2 server. Run the following on a new terminal - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - - -## XML Plan Parsing - -The capabilities2_fabric package relies on XML-based plans to define workflows. These plans specify the sequence of capabilities to execute, along with the associated parameters. The XML format includes tags for capabilities as events, and control flows enabling complex workflows to be structured in a modular way. - -Below is an example XML plan for configuring a set of capabilities: - -```xml - - - - - - - - - - - - - - - - - - - - - - - - - -``` - -## API - -| Node | Description | -| :--- | :--- | -| `capabilities2_Fabric` | Implemented the XML parsing and connection identification as well as communicating with `capabilities_server` to configure capability events | -| `capabilities2_File_Parser` | Reads an exection plan from a given path and sends it to the `capabilities2_fabric` node. Can be used as a sample action client to work with the `capabilities2_fabric` | - -| Action | Action Message | Description | -| :--- | :--- | :--- | -| `~/capabilities_fabric` | `Plan.action` | Receive and XML plan via the message for execution| - -## Samples and Testing - -### Navigation - -1. [WaypointRunner Example 1](./docs/waypoint_runner_ex1.md) -Implements at the very basic fabric triggering that moves the robot from one point to another. - -2. [WaypointRunner Example 2](./docs/waypoint_runner_ex2.md) -Implements navigating through 2 points using 'sequential' control functionality. - - -### Prompting - -1. [PromptCapabilityRunner Example](./docs/prompt_capability_runner_ex1.md) -Implements requesting for robot's capabilities and prompting them to the LLM - -2. [PromptOccupancyRunner Example](./docs/prompt_occupancy_runner_ex1.md) -Implements listening for robot's occupancy grid and prompting them to the LLM - -2. [PromptPoseRunner Example](./docs/prompt_pose_runner_ex1.md) -Implements listening for robot's pose and prompting them to the LLM - -2. [PromptPlanRunner Example](./docs/prompt_plan_runner_ex1.md) -Implements prompting the LLM for a plan for a new task and setting it to Capabilities Fabric diff --git a/capabilities2_fabric/src/capabilities_fabric_client_component.cpp b/capabilities2_fabric/src/capabilities_fabric_client_component.cpp deleted file mode 100644 index 9189005..0000000 --- a/capabilities2_fabric/src/capabilities_fabric_client_component.cpp +++ /dev/null @@ -1,4 +0,0 @@ -#include -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(CapabilitiesFabricClient) diff --git a/capabilities2_fabric/src/capabilities_fabric_client_node.cpp b/capabilities2_fabric/src/capabilities_fabric_client_node.cpp deleted file mode 100644 index f10f3c9..0000000 --- a/capabilities2_fabric/src/capabilities_fabric_client_node.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include - -int main(int argc, char* argv[]) -{ - // Initialize the ROS 2 C++ client library - rclcpp::init(argc, argv); - - // Create a shared pointer to the CapabilitiesFabricClient - auto parser_node = std::make_shared(); - - // Initialize the node - parser_node->initialize(); // Call initialize after construction - - // Spin the node to process callbacks - rclcpp::spin(parser_node); - - rclcpp::shutdown(); - - return 0; -} diff --git a/capabilities2_fabric/src/capabilities_fabric_component.cpp b/capabilities2_fabric/src/capabilities_fabric_component.cpp deleted file mode 100644 index 0219a1e..0000000 --- a/capabilities2_fabric/src/capabilities_fabric_component.cpp +++ /dev/null @@ -1,4 +0,0 @@ -#include -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(CapabilitiesFabric) \ No newline at end of file diff --git a/capabilities2_fabric/src/capabilities_fabric_node.cpp b/capabilities2_fabric/src/capabilities_fabric_node.cpp deleted file mode 100644 index e89ccbb..0000000 --- a/capabilities2_fabric/src/capabilities_fabric_node.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - // Create the node instance - auto node = std::make_shared(); - - // Initialize the node components after construction - node->initialize(); - - rclcpp::spin(node); - - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 0d19c29..0fbf54e 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -43,16 +43,11 @@ set(srv_files "srv/ConfigureCapability.srv" "srv/TriggerCapability.srv" "srv/Launch.srv" - "srv/GetFabricStatus.srv" - "srv/SetFabricPlan.srv" - "srv/CancelFabricPlan.srv" - "srv/CompleteFabric.srv" ) set(action_files "action/Capability.action" "action/Launch.action" - "action/Plan.action" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/capabilities2_msgs/action/Plan.action b/capabilities2_msgs/action/Plan.action deleted file mode 100644 index ab827c1..0000000 --- a/capabilities2_msgs/action/Plan.action +++ /dev/null @@ -1,14 +0,0 @@ -# action to transfer a plan for execution -# goal -std_msgs/Header header -string plan ---- -# result -std_msgs/Header header -bool success -string message -string[] failed_elements ---- -# feedback -std_msgs/Header header -string progress diff --git a/capabilities2_msgs/srv/CancelFabricPlan.srv b/capabilities2_msgs/srv/CancelFabricPlan.srv deleted file mode 100644 index 25a4b22..0000000 --- a/capabilities2_msgs/srv/CancelFabricPlan.srv +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header ---- -std_msgs/Header header -bool success \ No newline at end of file diff --git a/capabilities2_msgs/srv/CompleteFabric.srv b/capabilities2_msgs/srv/CompleteFabric.srv deleted file mode 100644 index 73b314f..0000000 --- a/capabilities2_msgs/srv/CompleteFabric.srv +++ /dev/null @@ -1 +0,0 @@ ---- \ No newline at end of file diff --git a/capabilities2_msgs/srv/GetFabricStatus.srv b/capabilities2_msgs/srv/GetFabricStatus.srv deleted file mode 100644 index 675f947..0000000 --- a/capabilities2_msgs/srv/GetFabricStatus.srv +++ /dev/null @@ -1,12 +0,0 @@ -std_msgs/Header header ---- -std_msgs/Header header -uint8 status -uint8 FABRIC_IDLE=0 -uint8 FABRIC_RUNNING=1 -uint8 FABRIC_CANCELED=2 -uint8 FABRIC_ABORTED=3 -uint8 FABRIC_FAILED=4 -uint8 FABRIC_LAUNCHED=5 -uint8 FABRIC_COMPLETED=6 -uint8 UNKNOWN=7 \ No newline at end of file diff --git a/capabilities2_msgs/srv/SetFabricPlan.srv b/capabilities2_msgs/srv/SetFabricPlan.srv deleted file mode 100644 index 49d3223..0000000 --- a/capabilities2_msgs/srv/SetFabricPlan.srv +++ /dev/null @@ -1,5 +0,0 @@ -std_msgs/Header header -string plan ---- -std_msgs/Header header -bool success \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 2c9bbdf..f9e702f 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -453,7 +453,7 @@ class RunnerBase auto message = Event(); message.header.stamp = node_->now(); - message.origin_node = "capability_runners"; + message.origin_node = "runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; message.target.capability = target_capability; @@ -493,7 +493,7 @@ class RunnerBase auto message = Event(); message.header.stamp = node_->now(); - message.origin_node = "capability_runners"; + message.origin_node = "runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; message.target.capability = ""; @@ -512,7 +512,7 @@ class RunnerBase auto message = Event(); message.header.stamp = node_->now(); - message.origin_node = "capability_runners"; + message.origin_node = "runners"; message.source.capability = run_config_.interface; message.source.provider = run_config_.provider; message.target.capability = ""; diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp similarity index 100% rename from capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp rename to capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml index 32fd1e9..f4ff0f8 100644 --- a/capabilities2_runner_capabilities/plugins.xml +++ b/capabilities2_runner_capabilities/plugins.xml @@ -4,14 +4,4 @@ A plugin that request capabilities from the capabilities server and transfers to an LLM - - - A plugin that notifies about the completion of the fabric to the action server - - - - - A plugin that sets a new Fabric plan to the Fabric - - diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp index 39c18de..c6c35a5 100644 --- a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -1,10 +1,6 @@ #include #include -#include -#include -#include +#include // register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricSetPlanRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_fabric/.clang-format b/capabilities2_runner_fabric/.clang-format similarity index 99% rename from capabilities2_fabric/.clang-format rename to capabilities2_runner_fabric/.clang-format index 92effdd..d36804f 100644 --- a/capabilities2_fabric/.clang-format +++ b/capabilities2_runner_fabric/.clang-format @@ -14,7 +14,7 @@ BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true BinPackParameters: true -ColumnLimit: 150 +ColumnLimit: 120 ConstructorInitializerAllOnOneLineOrOnePerLine: true DerivePointerBinding: false PointerBindsToType: true diff --git a/capabilities2_runner_fabric/CMakeLists.txt b/capabilities2_runner_fabric/CMakeLists.txt new file mode 100644 index 0000000..f8caf64 --- /dev/null +++ b/capabilities2_runner_fabric/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_runner_fabric) + +# 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(fabric_msgs REQUIRED) +find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_runner REQUIRED) +find_package(capabilities2_events 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/fabric_runners.cpp +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_action + pluginlib + fabric_msgs + capabilities2_msgs + capabilities2_runner + capabilities2_events + 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 +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp b/capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp similarity index 74% rename from capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp rename to capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp index c07d7ca..2475636 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp +++ b/capabilities2_runner_fabric/include/capabilities2_runner_fabric/completion_runner.hpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include namespace capabilities2_runner { @@ -14,7 +14,7 @@ namespace capabilities2_runner * call on the /capabilities_fabric/set_completion service, providing it as a * capability that notifys the completion of the fabric */ -class FabricCompletionRunner : public ServiceRunner +class FabricCompletionRunner : public ServiceRunner { public: FabricCompletionRunner() : ServiceRunner() @@ -29,7 +29,7 @@ class FabricCompletionRunner : public ServiceRunner #include -#include +#include namespace capabilities2_runner { @@ -18,7 +18,7 @@ namespace capabilities2_runner * Class to run capabilities2 executor action based capability * */ -class FabricSetPlanRunner : public ServiceRunner +class FabricSetPlanRunner : public ServiceRunner { public: FabricSetPlanRunner() : ServiceRunner() @@ -33,7 +33,7 @@ class FabricSetPlanRunner : public ServiceRunnerFirstChildElement("ReceievdPlan"); - capabilities2_msgs::srv::SetFabricPlan::Request request; + fabric_msgs::srv::SetFabricPlan::Request request; // Check if the element was found and has text content if (planElement && planElement->GetText()) diff --git a/capabilities2_fabric/package.xml b/capabilities2_runner_fabric/package.xml similarity index 75% rename from capabilities2_fabric/package.xml rename to capabilities2_runner_fabric/package.xml index 45bb847..3be6c38 100644 --- a/capabilities2_fabric/package.xml +++ b/capabilities2_runner_fabric/package.xml @@ -1,25 +1,25 @@ - capabilities2_fabric + capabilities2_runner_fabric 0.0.0 TODO: Package description kalana - - MIT + TODO: License declaration ament_cmake + ament_lint_auto + ament_lint_common + rclcpp rclcpp_action - capabilities2_msgs + pluginlib + std_msgs + fabric_msgs + capabilities2_runner capabilities2_events - capabilities2_utils tinyxml2_vendor - backward_ros - - ament_lint_auto - ament_lint_common ament_cmake diff --git a/capabilities2_runner_fabric/plugins.xml b/capabilities2_runner_fabric/plugins.xml new file mode 100644 index 0000000..5168f4b --- /dev/null +++ b/capabilities2_runner_fabric/plugins.xml @@ -0,0 +1,12 @@ + + + + A plugin that notifies about the completion of the fabric to the action server + + + + + A plugin that sets a new Fabric plan to the Fabric + + + diff --git a/capabilities2_runner_fabric/src/fabric_runners.cpp b/capabilities2_runner_fabric/src/fabric_runners.cpp new file mode 100644 index 0000000..6167db2 --- /dev/null +++ b/capabilities2_runner_fabric/src/fabric_runners.cpp @@ -0,0 +1,8 @@ +#include +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricSetPlanRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp index de20de9..ef7d675 100644 --- a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp +++ b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp @@ -2,20 +2,34 @@ #include #include #include +#include class BondClient { public: - BondClient(rclcpp::Node::SharedPtr node, const std::string& bond_id, const std::string& bonds_topic = "/capabilities/bond") + /** + * @brief Construct a new Bond Client object + * + * @param node pointer to the node + * @param event_client pointer to the event client + * @param bond_id Bond id string + * @param bonds_topic Bond topic to be published + */ + BondClient(rclcpp::Node::SharedPtr node, std::shared_ptr event_client, const std::string &bond_id, const std::string &bonds_topic = "/capabilities/bond") { topic_ = bonds_topic; bond_id_ = bond_id; node_ = node; + event_ = event_client; } + /** + * @brief start the bond + * + */ void start() { - RCLCPP_INFO(node_->get_logger(), "[BondClient] creating bond to capabilities server"); + event_->info("[BondClient] creating bond to capabilities server"); bond_ = std::make_unique(topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); @@ -24,9 +38,13 @@ class BondClient bond_->start(); } + /** + * @brief stop the bond + * + */ void stop() { - RCLCPP_INFO(node_->get_logger(), "[BondClient] destroying bond to capabilities server"); + event_->info("[BondClient] destroying bond to capabilities server"); if (bond_) { @@ -40,16 +58,24 @@ class BondClient } private: + /** + * @brief callback function for bond formed event + * + */ void on_formed() { // log bond established event - RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server formed with id: %s", bond_id_.c_str()); + event_->info("[BondClient] bond with capabilities server formed with id: " + bond_id_); } + /** + * @brief callback function for bond broken event + * + */ void on_broken() { // log bond established event - RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server broken with id: %s", bond_id_.c_str()); + event_->info("[BondClient] bond with capabilities server broken with id: " + bond_id_); } /** Ros node pointer */ @@ -63,4 +89,7 @@ class BondClient /** Heart beat bond with capabilities server */ std::shared_ptr bond_; + + /** Event client for publishing events */ + std::shared_ptr event_; }; \ No newline at end of file diff --git a/readme.md b/readme.md index 9d0c9d9..7da78ab 100644 --- a/readme.md +++ b/readme.md @@ -9,7 +9,10 @@ A reimplementation of the [capabilities](https://github.com/osrf/capabilities) package. This package is implemented using C++17 and extends the capabilities package features. - [capabilities2_server](./capabilities2_server/readme.md) package contains the core of the system. - [capabilities2_runner](./capabilities2_server/readme.md) package contains base and template classes for capability implementations. -- [capabilities2_fabric](./capabilities2_fabric/readme.md) package implements a control framework that utlizes capabilities2 system. + + +## Extentions +- [Fabric](https://github.com/CollaborativeRoboticsLab/fabric) package implements a behaviour planning framework that utlizes capabilities2 system. ## System structure @@ -44,15 +47,6 @@ source install/setup.bash ros2 launch capabilities2_server server.launch.py ``` -### Starting the Capabilities2 Fabric - -Start the capabilities2 server first. Then run the following on a new terminal - -```bash -source install/setup.bash -ros2 launch capabilities2_fabric fabric.launch.py -``` - ## Additional Information - [Motivation and Example Use Cases](./docs/motivation_and_examples.md) - [Design Information](./docs/design.md)