From ae618e804827559476e3b7723b2b9c0e7527b099 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 4 Feb 2025 02:42:37 +1100 Subject: [PATCH 01/17] added capabilities_runners. integration not finished --- .../capabilities_fabric.hpp | 235 +++++++++++------- .../capabilities_fabric_client.hpp | 51 ++-- .../utils/fabric_status.hpp | 14 ++ capabilities2_msgs/CMakeLists.txt | 1 + capabilities2_msgs/srv/CompleteFabric.srv | 1 + .../capabilities2_runner/action_runner.hpp | 15 +- .../capabilities2_runner/runner_base.hpp | 14 +- .../capabilities2_runner/service_runner.hpp | 10 +- .../.clang-format | 64 +++++ .../CMakeLists.txt | 63 +++++ .../fabric_completion_runner.hpp | 53 ++++ capabilities2_runner_capabilities/package.xml | 28 +++ capabilities2_runner_capabilities/plugins.xml | 7 + .../src/capabilities2_runner.cpp | 6 + capabilities2_runner_nav2/CMakeLists.txt | 2 - .../waypoint_runner.hpp | 3 - 16 files changed, 428 insertions(+), 139 deletions(-) create mode 100644 capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp create mode 100644 capabilities2_msgs/srv/CompleteFabric.srv create mode 100644 capabilities2_runner_capabilities/.clang-format create mode 100644 capabilities2_runner_capabilities/CMakeLists.txt create mode 100644 capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp create mode 100644 capabilities2_runner_capabilities/package.xml create mode 100644 capabilities2_runner_capabilities/plugins.xml create mode 100644 capabilities2_runner_capabilities/src/capabilities2_runner.cpp diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index e0b5f06..d2cb291 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -11,6 +12,7 @@ #include #include #include +#include #include @@ -22,6 +24,7 @@ #include #include #include +#include /** * @brief Capabilities Fabric @@ -46,6 +49,8 @@ class CapabilitiesFabric : public rclcpp::Node using ConfigureCapability = capabilities2_msgs::srv::ConfigureCapability; using TriggerCapability = capabilities2_msgs::srv::TriggerCapability; + using CompleteFabric = capabilities2_msgs::srv::CompleteFabric; + using GetInterfacesClient = rclcpp::Client; using GetSemanticInterfacesClient = rclcpp::Client; using GetProvidersClient = rclcpp::Client; @@ -55,7 +60,9 @@ class CapabilitiesFabric : public rclcpp::Node using ConfigureCapabilityClient = rclcpp::Client; using TriggerCapabilityClient = rclcpp::Client; - CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options) + using Status = capabilities2::fabric_status; + + CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options), lock_(mutex_, std::defer_lock) { control_tag_list = xml_parser::get_control_list(); } @@ -75,6 +82,10 @@ class CapabilitiesFabric : public rclcpp::Node std::bind(&CapabilitiesFabric::handle_cancel, this, std::placeholders::_1), std::bind(&CapabilitiesFabric::handle_accepted, this, std::placeholders::_1)); + completion_server_ = + this->create_service("/capabilities_fabric/set_completion", + std::bind(&CapabilitiesFabric::setCompleteCallback, this, std::placeholders::_1, std::placeholders::_2)); + 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"); @@ -96,6 +107,8 @@ class CapabilitiesFabric : public rclcpp::Node feedback_msg = std::make_shared(); result_msg = std::make_shared(); + + fabric_state = Status::IDLE; } private: @@ -114,7 +127,7 @@ class CapabilitiesFabric : public rclcpp::Node (void)uuid; // 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()); + tinyxml2::XMLError xml_status = documentEvaluation.Parse(goal->plan.c_str()); // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) @@ -123,9 +136,18 @@ class CapabilitiesFabric : public rclcpp::Node return rclcpp_action::GoalResponse::REJECT; } - status_->info("Plan parsed and accepted"); - - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + if (fabric_state == Status::RUNNING) + { + status_->info("Prior plan under exeution. Will defer the new plan"); + plan_queue.push_back(goal->plan); + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + } + else + { + status_->info("Plan parsed and accepted"); + plan_queue.push_back(goal->plan); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } } /** @@ -141,6 +163,8 @@ class CapabilitiesFabric : public rclcpp::Node bond_client_->stop(); + fabric_state = Status::CANCELED; + return rclcpp_action::CancelResponse::ACCEPT; } @@ -153,15 +177,26 @@ class CapabilitiesFabric : public rclcpp::Node { goal_handle_ = goal_handle; + fabric_state = Status::RUNNING; + execution(); } + void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) + { + fabric_completed_ = true; + cv_.notify_all(); + } + /** * @brief Start the execution of the capabilities2 fabric */ void execution() { - process_feedback("Execution started"); + process_feedback("A new execution started"); + + lock_.lock(); + fabric_completed_ = false; expected_providers_ = 0; completed_providers_ = 0; @@ -175,7 +210,22 @@ class CapabilitiesFabric : public rclcpp::Node expected_configurations_ = 0; completed_configurations_ = 0; - getInterfaces(); + std::string plan_to_be_executed = plan_queue[0]; + + tinyxml2::XMLError xml_status = document.Parse(plan_to_be_executed.c_str()); + + // check if the file parsing failed + if (xml_status != tinyxml2::XMLError::XML_SUCCESS) + { + status_->error("Parsing the plan from queue failed"); + return; + } + else + { + status_->error("Parsing the plan from queue successful"); + plan_queue.pop_front(); + getInterfaces(); + } } /** @@ -200,8 +250,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); expected_interfaces_ = response->interfaces.size(); - status = "Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"; - process_feedback(status); + process_feedback("Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"); // Request each interface recursively for Semantic interfaces getSemanticInterfaces(response->interfaces); @@ -217,8 +266,7 @@ class CapabilitiesFabric : public rclcpp::Node { std::string requested_interface = interfaces[completed_interfaces_]; - status = "Requesting semantic interfaces for " + requested_interface; - process_feedback(status, true); + process_feedback("Requesting semantic interfaces for " + requested_interface, true); auto request_semantic = std::make_shared(); request_semantic->interface = requested_interface; @@ -243,9 +291,8 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(semantic_interface); is_semantic_list.push_back(true); - status = (completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + " for " + - requested_interface + ". So added " + semantic_interface; - process_feedback(status); + process_feedback(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 @@ -254,9 +301,8 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(requested_interface); is_semantic_list.push_back(false); - status = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + - requested_interface + ". So added " + requested_interface; - process_feedback(status); + process_feedback(std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + + requested_interface + ". So added " + requested_interface); } if (completed_interfaces_ != expected_interfaces_) @@ -270,8 +316,7 @@ class CapabilitiesFabric : public rclcpp::Node expected_providers_ = interface_list.size(); - status = "Requsting Provider information for " + std::to_string(expected_providers_) + " providers"; - process_feedback(status); + process_feedback("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); @@ -290,8 +335,7 @@ class CapabilitiesFabric : public rclcpp::Node std::string requested_interface = interfaces[completed_providers_]; bool semantic_flag = is_semantic[completed_providers_]; - status = "Requesting provider for " + requested_interface; - process_feedback(status, true); + process_feedback("Requesting provider for " + requested_interface, true); auto request_providers = std::make_shared(); @@ -303,8 +347,7 @@ class CapabilitiesFabric : public rclcpp::Node request_providers, [this, is_semantic, requested_interface, interfaces](GetProvidersClient::SharedFuture future) { if (!future.valid()) { - status = "Did not retrieve providers for interface: " + requested_interface; - process_result(status, false, false); + process_result("Did not retrieve providers for interface: " + requested_interface, false, false); return; } @@ -316,9 +359,8 @@ class CapabilitiesFabric : public rclcpp::Node // add defualt provider to the list providers_list.push_back(response->default_provider); - status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + - " for " + requested_interface + ". So added " + response->default_provider; - process_feedback(status); + process_feedback(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 @@ -328,15 +370,14 @@ class CapabilitiesFabric : public rclcpp::Node { providers_list.push_back(provider); - status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + - " for " + requested_interface; - process_feedback(status); + process_feedback(std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + + provider + " for " + requested_interface); } } else { - status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; - process_feedback(status); + process_feedback(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 @@ -469,16 +510,14 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id_ = response->bond_id; - status = "Received the bond id : " + bond_id_; - process_feedback(status); + process_feedback("Received the bond id : " + bond_id_); bond_client_ = std::make_unique(shared_from_this(), bond_id_); bond_client_->start(); expected_capabilities_ = connection_map.size(); - status = "Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"; - process_feedback(status); + process_feedback("Requsting start of " + std::to_string(expected_capabilities_) + " capabilities"); use_capability(connection_map); }); @@ -500,28 +539,25 @@ class CapabilitiesFabric : public rclcpp::Node request_use->preferred_provider = provider; request_use->bond_id = bond_id_; - status = "Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + capabilities[completed_capabilities_].source.runner; - process_feedback(status, true); + process_feedback("Starting capability of Runner " + std::to_string(completed_capabilities_) + " : " + + capabilities[completed_capabilities_].source.runner, + true); // send the request auto result_future = use_capability_client_->async_send_request(request_use, [this, capability, provider](UseCapabilityClient::SharedFuture future) { if (!future.valid()) { - status = "Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"; - process_result(status); + process_result("Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"); // release all capabilities that were used since not all started successfully for (const auto& [key, value] : connection_map) { - status = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; - process_feedback(status); - + process_feedback("Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner); free_capability(value.source.runner); } bond_client_->stop(); - return; } @@ -529,8 +565,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - status = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"; - process_feedback(status); + process_feedback(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_) @@ -539,8 +574,7 @@ class CapabilitiesFabric : public rclcpp::Node expected_configurations_ = connection_map.size(); - status = "Requsting capability configuration for " + std::to_string(expected_configurations_) + " capabilities"; - process_feedback(status, true); + process_feedback("Requsting capability configuration for " + std::to_string(expected_configurations_) + " capabilities", true); configure_capabilities(connection_map); } @@ -566,15 +600,12 @@ class CapabilitiesFabric : public rclcpp::Node auto result_future = free_capability_client_->async_send_request(request_free, [this, capability](FreeCapabilityClient::SharedFuture future) { if (!future.valid()) { - status = "Failed to free capability " + capability; - process_result(status); + process_result("Failed to free capability " + capability); return; } auto response = future.get(); - - status = "Successfully freed capability " + capability; - process_feedback(status, true); + process_feedback("Successfully freed capability " + capability, true); }); } @@ -585,9 +616,9 @@ class CapabilitiesFabric : public rclcpp::Node { auto request_configure = std::make_shared(); - status = "Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + - capabilities[completed_configurations_].source.runner; - process_feedback(status, true); + process_feedback("Configuring capability of Runner " + std::to_string(completed_configurations_) + " named " + + capabilities[completed_configurations_].source.runner, + true); if (xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { @@ -608,8 +639,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_start.capability = ""; - request_configure->target_on_start.provider = ""; + request_configure->target_on_start.capability = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_start.provider = "std_capabilities/FabricCompletionRunner"; } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_stop.parameters, @@ -620,8 +651,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_stop.capability = ""; - request_configure->target_on_stop.provider = ""; + request_configure->target_on_stop.capability = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_stop.provider = "std_capabilities/FabricCompletionRunner"; } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_success.parameters, @@ -632,8 +663,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_success.capability = ""; - request_configure->target_on_success.provider = ""; + request_configure->target_on_success.capability = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_success.provider = "std_capabilities/FabricCompletionRunner"; } if (xml_parser::convert_to_string(capabilities[completed_configurations_].target_on_failure.parameters, @@ -644,8 +675,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_failure.capability = ""; - request_configure->target_on_failure.provider = ""; + request_configure->target_on_failure.capability = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_failure.provider = "std_capabilities/FabricCompletionRunner"; } std::string source_capability = capabilities[completed_configurations_].source.runner; @@ -655,8 +686,7 @@ class CapabilitiesFabric : public rclcpp::Node conf_capability_client_->async_send_request(request_configure, [this, source_capability](ConfigureCapabilityClient::SharedFuture future) { if (!future.valid()) { - status = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; - process_result(status); + process_result("Failed to configure capability :" + source_capability + ". Server execution cancelled"); return; } @@ -664,9 +694,8 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - status = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + - " : Successfully configured capability : " + source_capability; - process_feedback(status); + process_feedback(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_) @@ -698,32 +727,55 @@ class CapabilitiesFabric : public rclcpp::Node auto result_future = trig_capability_client_->async_send_request(request_trigger, [this](TriggerCapabilityClient::SharedFuture future) { if (!future.valid()) { - status = "Failed to trigger capability " + connection_map[0].source.runner; - process_result(status); + process_result("Failed to trigger capability " + connection_map[0].source.runner); return; } auto response = future.get(); + process_feedback("Successfully triggered capability " + connection_map[0].source.runner); - status = "Successfully triggered capability " + connection_map[0].source.runner; - process_feedback(status); - - process_result("Successfully launched capabilities2 fabric", true); + wait_for_completion(); }); } + /** + * @brief Wait for the execution completion of the fabric + * + */ + void wait_for_completion() + { + process_feedback("Waiting for fabric execution completion"); + + // Conditional wait + while (!fabric_completed_) + { + cv_.wait(lock_); + } + + lock_.unlock(); + + if (plan_queue.size()>0) + { + process_feedback("Successfully completed capabilities2 fabric. Starting the next fabric"); + + execution(); + } + else + { + process_result("Successfully completed capabilities2 fabric", true); + } + } + void check_service(bool wait_for_logic, const std::string& service_name) { while (wait_for_logic) { - std::string failed = service_name + " not available"; - status_->error(failed); + status_->error(service_name + " not available"); rclcpp::shutdown(); return; } - std::string success = service_name + " connected"; - status_->info(success); + status_->info(service_name + " connected"); } /** @@ -754,13 +806,15 @@ class CapabilitiesFabric : public rclcpp::Node if (success) { - goal_handle_->succeed(result_msg); status_->info(text, newline); + fabric_state = Status::SUCCEEDED; + goal_handle_->succeed(result_msg); } else { - goal_handle_->abort(result_msg); status_->error(text, newline); + fabric_state = Status::ABORTED; + goal_handle_->abort(result_msg); } } @@ -783,21 +837,27 @@ class CapabilitiesFabric : public rclcpp::Node int expected_configurations_; int completed_configurations_; - /** status message string */ - std::string status; - /** Bond id */ std::string bond_id_; + /** Status of the fabric */ + Status fabric_state; + /** Manages bond between capabilities server and this client */ std::shared_ptr bond_client_; /** Handles status message sending and printing to logging */ std::shared_ptr status_; + /** Vector of plans */ + std::deque plan_queue; + /** XML Document */ tinyxml2::XMLDocument document; + /** XML Document */ + tinyxml2::XMLDocument documentEvaluation; + /** vector of connections */ std::map connection_map; @@ -851,4 +911,13 @@ class CapabilitiesFabric : public rclcpp::Node /** trigger an selected capability */ TriggerCapabilityClient::SharedPtr trig_capability_client_; + + /** server to get the status of the capabilities2 fabric */ + rclcpp::Service::SharedPtr completion_server_; + + /** 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 index a13976c..0b5670a 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -12,6 +12,7 @@ #include #include +#include #include @@ -29,16 +30,8 @@ class CapabilitiesFabricClient : public rclcpp::Node { public: - enum fabric_status - { - IDLE, - RUNNING, - CANCELED, - ABORTED, - FAILED, - SUCCEEDED - }; + using Status = capabilities2::fabric_status; using Plan = capabilities2_msgs::action::Plan; using GoalHandlePlan = rclcpp_action::ClientGoalHandle; @@ -58,7 +51,7 @@ class CapabilitiesFabricClient : public rclcpp::Node */ void initialize() { - fabric_state = fabric_status::IDLE; + fabric_state = Status::IDLE; status_ = std::make_shared(shared_from_this(), "capabilities_fabric_client", "/status/capabilities_fabric_client"); @@ -91,13 +84,11 @@ class CapabilitiesFabricClient : public rclcpp::Node // check if the file loading failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) { - status = "Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName(); - status_->error(status); + status_->error("Error loading plan: " + plan_file_path + ", Error: " + document.ErrorName()); rclcpp::shutdown(); } - status = "Plan loaded from : " + plan_file_path; - status_->info(status); + status_->info("Plan loaded from : " + plan_file_path); send_goal(document); } @@ -109,9 +100,7 @@ class CapabilitiesFabricClient : public rclcpp::Node xml_parser::convert_to_string(document_xml, goal_msg.plan); - status = "Following plan was loaded :\n\n " + goal_msg.plan; - status_->info(status); - + status_->info("Following plan was loaded :\n\n " + goal_msg.plan); status_->info("Sending goal to the capabilities_fabric action server"); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); @@ -122,13 +111,13 @@ class CapabilitiesFabricClient : public rclcpp::Node if (!goal_handle) { status_->error("Goal was rejected by server"); - fabric_state = fabric_status::FAILED; + fabric_state = Status::FAILED; } else { status_->info("Goal accepted by server, waiting for result"); goal_handle_ = goal_handle; - fabric_state = fabric_status::RUNNING; + fabric_state = Status::RUNNING; } }; @@ -137,19 +126,19 @@ class CapabilitiesFabricClient : public rclcpp::Node switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - fabric_state = fabric_status::SUCCEEDED; + fabric_state = Status::SUCCEEDED; break; case rclcpp_action::ResultCode::ABORTED: status_->error("Goal was aborted"); - fabric_state = fabric_status::ABORTED; + fabric_state = Status::ABORTED; break; case rclcpp_action::ResultCode::CANCELED: status_->error("Goal was canceled"); - fabric_state = fabric_status::CANCELED; + fabric_state = Status::CANCELED; break; default: status_->error("Unknown result code"); - fabric_state = fabric_status::FAILED; + fabric_state = Status::FAILED; break; } @@ -176,27 +165,27 @@ class CapabilitiesFabricClient : public rclcpp::Node void getStatusCallback(const std::shared_ptr request, std::shared_ptr response) { - if (fabric_state == fabric_status::IDLE) + if (fabric_state == Status::IDLE) { response->status = GetFabricStatus::Response::FABRIC_IDLE; } - else if (fabric_state == fabric_status::RUNNING) + else if (fabric_state == Status::RUNNING) { response->status = GetFabricStatus::Response::FABRIC_RUNNING; } - else if (fabric_state == fabric_status::CANCELED) + else if (fabric_state == Status::CANCELED) { response->status = GetFabricStatus::Response::FABRIC_CANCELED; } - else if (fabric_state == fabric_status::ABORTED) + else if (fabric_state == Status::ABORTED) { response->status = GetFabricStatus::Response::FABRIC_ABORTED; } - else if (fabric_state == fabric_status::FAILED) + else if (fabric_state == Status::FAILED) { response->status = GetFabricStatus::Response::FABRIC_FAILED; } - else if (fabric_state == fabric_status::SUCCEEDED) + else if (fabric_state == Status::SUCCEEDED) { response->status = GetFabricStatus::Response::FABRIC_SUCCEEDED; } @@ -229,7 +218,7 @@ class CapabilitiesFabricClient : public rclcpp::Node void cancelPlanCallback(const std::shared_ptr request, std::shared_ptr response) { - if (fabric_state == fabric_status::RUNNING) + if (fabric_state == Status::RUNNING) { this->planner_client_->async_cancel_goal(goal_handle_); } @@ -266,5 +255,5 @@ class CapabilitiesFabricClient : public rclcpp::Node rclcpp::Service::SharedPtr cancel_server_; /** Status of the fabric */ - fabric_status fabric_state; + Status fabric_state; }; diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp new file mode 100644 index 0000000..bd3fffb --- /dev/null +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp @@ -0,0 +1,14 @@ +#pragma once + +namespace capabilities2 +{ +enum fabric_status +{ + IDLE, + RUNNING, + CANCELED, + ABORTED, + FAILED, + SUCCEEDED +}; +} \ No newline at end of file diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index b50c9eb..05e02d5 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -47,6 +47,7 @@ set(srv_files "srv/GetFabricStatus.srv" "srv/SetFabricPlan.srv" "srv/CancelFabricPlan.srv" + "srv/CompleteFabric.srv" ) set(action_files diff --git a/capabilities2_msgs/srv/CompleteFabric.srv b/capabilities2_msgs/srv/CompleteFabric.srv new file mode 100644 index 0000000..73b314f --- /dev/null +++ b/capabilities2_msgs/srv/CompleteFabric.srv @@ -0,0 +1 @@ +--- \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 4ca876e..43f0fde 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -137,8 +137,8 @@ class ActionRunner : public RunnerBase info_("goal generated", id); - std::unique_lock lock(send_goal_mutex); - action_complete = false; + std::unique_lock lock(mutex_); + completed_ = false; // trigger the action client with goal send_goal_options_.goal_response_callback = @@ -201,15 +201,15 @@ class ActionRunner : public RunnerBase } result_ = wrapped_result.result; - action_complete = true; - send_goal_cv.notify_all(); + completed_ = true; + cv_.notify_all(); }; goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); info_("goal sent. Waiting for acceptance.", id); // Conditional wait - send_goal_cv.wait(lock, [this] { return action_complete; }); + cv_.wait(lock, [this] { return completed_; }); info_("action complete. Thread closing.", id); } @@ -264,11 +264,6 @@ class ActionRunner : public RunnerBase /** Result Future*/ std::shared_future::WrappedResult> result_future_; - - // Synchronization tools - std::mutex mutex_; - std::condition_variable cv_; - bool action_completed_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 46249b3..11e4ec2 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -570,15 +570,19 @@ class RunnerBase std::map executionThreadPool; /** - * @brief mutex and conditional variable for threadpool synchronisation. + * @brief mutex for threadpool synchronisation. */ - std::mutex send_goal_mutex; - std::condition_variable send_goal_cv; + std::mutex mutex_; /** - * @brief boolean flag for thread completion. + * @brief conditional variable for threadpool synchronisation. */ - bool action_complete; + std::condition_variable cv_; + + /** + * @brief flag for threadpool synchronisation. + */ + bool completed_; /** * @brief external function that triggers capability runners diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 46040a8..eda0065 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -72,8 +72,8 @@ class ServiceRunner : public RunnerBase info_("request generated", id); - std::unique_lock lock(send_goal_mutex); - action_complete = false; + std::unique_lock lock(mutex_); + completed_ = false; auto result_future = service_client_->async_send_request( request_msg, [this, id](typename rclcpp::Client::SharedFuture future) { @@ -102,8 +102,8 @@ class ServiceRunner : public RunnerBase } } - action_complete = true; - send_goal_cv.notify_all(); + completed_ = true; + cv_.notify_all(); }); // trigger the events related to on_started state @@ -114,7 +114,7 @@ class ServiceRunner : public RunnerBase } // Conditional wait - send_goal_cv.wait(lock, [this] { return action_complete; }); + cv_.wait(lock, [this] { return completed_; }); info_("Service request complete. Thread closing.", id); } diff --git a/capabilities2_runner_capabilities/.clang-format b/capabilities2_runner_capabilities/.clang-format new file mode 100644 index 0000000..d36804f --- /dev/null +++ b/capabilities2_runner_capabilities/.clang-format @@ -0,0 +1,64 @@ + +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} diff --git a/capabilities2_runner_capabilities/CMakeLists.txt b/capabilities2_runner_capabilities/CMakeLists.txt new file mode 100644 index 0000000..d72bd90 --- /dev/null +++ b/capabilities2_runner_capabilities/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_runner_capabilities) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(pluginlib REQUIRED) +find_package(capabilities2_msgs REQUIRED) +find_package(capabilities2_runner REQUIRED) + +# Locate the static version of tinyxml2 +find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) + +if(NOT TINYXML2_LIB) + message(FATAL_ERROR "tinyxml2 library not found. Make sure it's installed as a static library.") +endif() + +include_directories(include + /usr/local/include +) + +add_library(${PROJECT_NAME} SHARED + src/capabilities2_runner.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${TINYXML2_LIB} +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_action + pluginlib + capabilities2_msgs + capabilities2_runner +) + +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_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp new file mode 100644 index 0000000..828fb7c --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_completion_runner.hpp @@ -0,0 +1,53 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace capabilities2_runner +{ +/** + * @brief fabric completion runner + * + * This class is a wrapper around the capabilities2 service runner and is used to + * call on the /capabilities_fabric/set_completion service, providing it as a + * capability that notifys the completion of the fabric + */ +class FabricCompletionRunner : public ServiceRunner +{ +public: + FabricCompletionRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the service runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/capabilities_fabric/set_completion", print); + } + +protected: + /** + * @brief Generate a empty request. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request + */ + virtual typename capabilities2_msgs::srv::CompleteFabric::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + { + capabilities2_msgs::srv::CompleteFabric::Request request; + return request; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_capabilities/package.xml b/capabilities2_runner_capabilities/package.xml new file mode 100644 index 0000000..93ddac1 --- /dev/null +++ b/capabilities2_runner_capabilities/package.xml @@ -0,0 +1,28 @@ + + + + capabilities2_runner_capabilities + 0.0.0 + TODO: Package description + kalana + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_action + pluginlib + std_msgs + capabilities2_msgs + capabilities2_runner + + + tinyxml2 + + + ament_cmake + + diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml new file mode 100644 index 0000000..4789f11 --- /dev/null +++ b/capabilities2_runner_capabilities/plugins.xml @@ -0,0 +1,7 @@ + + + + A plugin that notifies about the completion of the fabric to the action server + + + diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp new file mode 100644 index 0000000..26e1b95 --- /dev/null +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -0,0 +1,6 @@ +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) \ No newline at end of file diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 1ce3c58..3edafbc 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -16,7 +16,6 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(nav2_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(lifecycle_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) @@ -45,7 +44,6 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib nav2_msgs geometry_msgs - lifecycle_msgs capabilities2_msgs capabilities2_runner ) diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp index dc7a14d..1a93591 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include @@ -94,8 +93,6 @@ class WayPointRunner : public ActionRunner std::string robot_base_frame_; /**The frame of the robot base*/ double x, y; /**Coordinate frame parameters*/ - - rclcpp::Client::SharedPtr get_state_client_; }; } // namespace capabilities2_runner From d5882fbef7c79145186ed1352cf0040c40c2f44a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 4 Feb 2025 17:47:34 +1100 Subject: [PATCH 02/17] restored fabric to original and modified fabric client with completion server and threading for sending goal --- .../capabilities_fabric.hpp | 132 +++--------------- .../capabilities_fabric_client.hpp | 119 +++++++++++++--- .../utils/fabric_status.hpp | 3 +- .../capabilities2_fabric/utils/xml_parser.hpp | 34 +++++ capabilities2_msgs/srv/GetFabricStatus.srv | 5 +- 5 files changed, 160 insertions(+), 133 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index d2cb291..454a698 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -2,7 +2,6 @@ #include #include #include -#include #include #include @@ -12,7 +11,6 @@ #include #include #include -#include #include @@ -24,7 +22,6 @@ #include #include #include -#include /** * @brief Capabilities Fabric @@ -49,8 +46,6 @@ class CapabilitiesFabric : public rclcpp::Node using ConfigureCapability = capabilities2_msgs::srv::ConfigureCapability; using TriggerCapability = capabilities2_msgs::srv::TriggerCapability; - using CompleteFabric = capabilities2_msgs::srv::CompleteFabric; - using GetInterfacesClient = rclcpp::Client; using GetSemanticInterfacesClient = rclcpp::Client; using GetProvidersClient = rclcpp::Client; @@ -60,9 +55,7 @@ class CapabilitiesFabric : public rclcpp::Node using ConfigureCapabilityClient = rclcpp::Client; using TriggerCapabilityClient = rclcpp::Client; - using Status = capabilities2::fabric_status; - - CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options), lock_(mutex_, std::defer_lock) + CapabilitiesFabric(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("Capabilities2_Fabric", options) { control_tag_list = xml_parser::get_control_list(); } @@ -82,10 +75,6 @@ class CapabilitiesFabric : public rclcpp::Node std::bind(&CapabilitiesFabric::handle_cancel, this, std::placeholders::_1), std::bind(&CapabilitiesFabric::handle_accepted, this, std::placeholders::_1)); - completion_server_ = - this->create_service("/capabilities_fabric/set_completion", - std::bind(&CapabilitiesFabric::setCompleteCallback, this, std::placeholders::_1, std::placeholders::_2)); - 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"); @@ -107,8 +96,6 @@ class CapabilitiesFabric : public rclcpp::Node feedback_msg = std::make_shared(); result_msg = std::make_shared(); - - fabric_state = Status::IDLE; } private: @@ -126,8 +113,10 @@ class CapabilitiesFabric : public rclcpp::Node (void)uuid; + status_->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 = documentEvaluation.Parse(goal->plan.c_str()); + tinyxml2::XMLError xml_status = document.Parse(goal->plan.c_str()); // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) @@ -136,18 +125,9 @@ class CapabilitiesFabric : public rclcpp::Node return rclcpp_action::GoalResponse::REJECT; } - if (fabric_state == Status::RUNNING) - { - status_->info("Prior plan under exeution. Will defer the new plan"); - plan_queue.push_back(goal->plan); - return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; - } - else - { - status_->info("Plan parsed and accepted"); - plan_queue.push_back(goal->plan); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } + status_->info("Plan parsed and accepted"); + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } /** @@ -163,8 +143,6 @@ class CapabilitiesFabric : public rclcpp::Node bond_client_->stop(); - fabric_state = Status::CANCELED; - return rclcpp_action::CancelResponse::ACCEPT; } @@ -177,17 +155,9 @@ class CapabilitiesFabric : public rclcpp::Node { goal_handle_ = goal_handle; - fabric_state = Status::RUNNING; - execution(); } - void setCompleteCallback(const std::shared_ptr request, std::shared_ptr response) - { - fabric_completed_ = true; - cv_.notify_all(); - } - /** * @brief Start the execution of the capabilities2 fabric */ @@ -195,9 +165,6 @@ class CapabilitiesFabric : public rclcpp::Node { process_feedback("A new execution started"); - lock_.lock(); - fabric_completed_ = false; - expected_providers_ = 0; completed_providers_ = 0; @@ -210,22 +177,7 @@ class CapabilitiesFabric : public rclcpp::Node expected_configurations_ = 0; completed_configurations_ = 0; - std::string plan_to_be_executed = plan_queue[0]; - - tinyxml2::XMLError xml_status = document.Parse(plan_to_be_executed.c_str()); - - // check if the file parsing failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - status_->error("Parsing the plan from queue failed"); - return; - } - else - { - status_->error("Parsing the plan from queue successful"); - plan_queue.pop_front(); - getInterfaces(); - } + getInterfaces(); } /** @@ -639,8 +591,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_start.capability = "std_capabilities/FabricCompletionRunner"; - request_configure->target_on_start.provider = "std_capabilities/FabricCompletionRunner"; + 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, @@ -651,8 +603,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_stop.capability = "std_capabilities/FabricCompletionRunner"; - request_configure->target_on_stop.provider = "std_capabilities/FabricCompletionRunner"; + 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, @@ -663,8 +615,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_success.capability = "std_capabilities/FabricCompletionRunner"; - request_configure->target_on_success.provider = "std_capabilities/FabricCompletionRunner"; + 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, @@ -675,8 +627,8 @@ class CapabilitiesFabric : public rclcpp::Node } else { - request_configure->target_on_failure.capability = "std_capabilities/FabricCompletionRunner"; - request_configure->target_on_failure.provider = "std_capabilities/FabricCompletionRunner"; + request_configure->target_on_failure.capability = ""; + request_configure->target_on_failure.provider = ""; } std::string source_capability = capabilities[completed_configurations_].source.runner; @@ -734,36 +686,8 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); process_feedback("Successfully triggered capability " + connection_map[0].source.runner); - wait_for_completion(); - }); - } - - /** - * @brief Wait for the execution completion of the fabric - * - */ - void wait_for_completion() - { - process_feedback("Waiting for fabric execution completion"); - - // Conditional wait - while (!fabric_completed_) - { - cv_.wait(lock_); - } - - lock_.unlock(); - - if (plan_queue.size()>0) - { - process_feedback("Successfully completed capabilities2 fabric. Starting the next fabric"); - - execution(); - } - else - { process_result("Successfully completed capabilities2 fabric", true); - } + }); } void check_service(bool wait_for_logic, const std::string& service_name) @@ -807,13 +731,11 @@ class CapabilitiesFabric : public rclcpp::Node if (success) { status_->info(text, newline); - fabric_state = Status::SUCCEEDED; goal_handle_->succeed(result_msg); } else { status_->error(text, newline); - fabric_state = Status::ABORTED; goal_handle_->abort(result_msg); } } @@ -840,24 +762,15 @@ class CapabilitiesFabric : public rclcpp::Node /** Bond id */ std::string bond_id_; - /** Status of the fabric */ - Status fabric_state; - /** Manages bond between capabilities server and this client */ std::shared_ptr bond_client_; /** Handles status message sending and printing to logging */ std::shared_ptr status_; - /** Vector of plans */ - std::deque plan_queue; - /** XML Document */ tinyxml2::XMLDocument document; - /** XML Document */ - tinyxml2::XMLDocument documentEvaluation; - /** vector of connections */ std::map connection_map; @@ -912,12 +825,9 @@ class CapabilitiesFabric : public rclcpp::Node /** trigger an selected capability */ TriggerCapabilityClient::SharedPtr trig_capability_client_; - /** server to get the status of the capabilities2 fabric */ - rclcpp::Service::SharedPtr completion_server_; - /** capabilities2 server and fabric synchronization tools */ - std::mutex mutex_; - std::condition_variable cv_; - bool fabric_completed_; - std::unique_lock lock_; + // 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 index 0b5670a..d5462f8 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric_client.hpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -19,6 +20,7 @@ #include #include #include +#include /** * @brief Capabilities Executor File Parser @@ -30,7 +32,6 @@ class CapabilitiesFabricClient : public rclcpp::Node { public: - using Status = capabilities2::fabric_status; using Plan = capabilities2_msgs::action::Plan; using GoalHandlePlan = rclcpp_action::ClientGoalHandle; @@ -38,6 +39,7 @@ class CapabilitiesFabricClient : public rclcpp::Node 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) { @@ -66,6 +68,10 @@ class CapabilitiesFabricClient : public rclcpp::Node 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"); @@ -90,22 +96,48 @@ class CapabilitiesFabricClient : public rclcpp::Node status_->info("Plan loaded from : " + plan_file_path); - send_goal(document); + 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 send_goal(tinyxml2::XMLDocument& document_xml) + void manage_goal() + { + while (plan_queue.size() > 0) + { + status_->info("Fabric client thread starting"); + + std::unique_lock lock(mutex_); + completed_ = false; + + send_goal(); + + status_->info("Fabric plan sent. Waiting for acceptance."); + + // Conditional wait + cv_.wait(lock, [this] { return completed_; }); + status_->info("Fabric client thread closing"); + } + } + + void send_goal() { + // Create Plan Goal message auto goal_msg = Plan::Goal(); - xml_parser::convert_to_string(document_xml, goal_msg.plan); + // load the latest plan from the queue + goal_msg.plan = plan_queue[0]; + plan_queue.pop_front(); - status_->info("Following plan was loaded :\n\n " + goal_msg.plan); status_->info("Sending goal to the capabilities_fabric action server"); + // send goal options auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - // send goal options // goal response callback send_goal_options.goal_response_callback = [this](const GoalHandlePlan::SharedPtr& goal_handle) { if (!goal_handle) @@ -115,7 +147,7 @@ class CapabilitiesFabricClient : public rclcpp::Node } else { - status_->info("Goal accepted by server, waiting for result"); + status_->info("Goal accepted by server, waiting for completion"); goal_handle_ = goal_handle; fabric_state = Status::RUNNING; } @@ -126,7 +158,7 @@ class CapabilitiesFabricClient : public rclcpp::Node switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - fabric_state = Status::SUCCEEDED; + fabric_state = Status::LAUNCHED; break; case rclcpp_action::ResultCode::ABORTED: status_->error("Goal was aborted"); @@ -163,6 +195,24 @@ class CapabilitiesFabricClient : public rclcpp::Node 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) + { + 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; + + completed_ = true; + cv_.notify_all(); + } + void getStatusCallback(const std::shared_ptr request, std::shared_ptr response) { if (fabric_state == Status::IDLE) @@ -185,9 +235,13 @@ class CapabilitiesFabricClient : public rclcpp::Node { response->status = GetFabricStatus::Response::FABRIC_FAILED; } - else if (fabric_state == Status::SUCCEEDED) + else if (fabric_state == Status::LAUNCHED) + { + response->status = GetFabricStatus::Response::FABRIC_LAUNCHED; + } + else if (fabric_state == Status::COMPLETED) { - response->status = GetFabricStatus::Response::FABRIC_SUCCEEDED; + response->status = GetFabricStatus::Response::FABRIC_COMPLETED; } else { @@ -200,7 +254,7 @@ class CapabilitiesFabricClient : public rclcpp::Node status_->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 = document.Parse(request->plan.c_str()); + tinyxml2::XMLError xml_status = documentChecking.Parse(request->plan.c_str()); // check if the file parsing failed if (xml_status != tinyxml2::XMLError::XML_SUCCESS) @@ -209,18 +263,18 @@ class CapabilitiesFabricClient : public rclcpp::Node response->success = false; } - status_->info("Plan parsed and accepted"); + status_->info("Plan parsed and valid"); - response->success = true; + plan_queue.push_back(request->plan); - send_goal(document); - } - - void cancelPlanCallback(const std::shared_ptr request, std::shared_ptr response) - { if (fabric_state == Status::RUNNING) { - this->planner_client_->async_cancel_goal(goal_handle_); + status_->info("Prior plan under exeution. Will defer the new plan"); + } + else + { + status_->info("Plan parsed and accepted"); + goal_send_thread = std::thread(&CapabilitiesFabricClient::manage_goal, this); } response->success = true; @@ -233,9 +287,18 @@ class CapabilitiesFabricClient : public rclcpp::Node /** 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_; @@ -254,6 +317,24 @@ class CapabilitiesFabricClient : public rclcpp::Node /** 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; + + /** + * @brief mutex for threadpool synchronisation. + */ + std::mutex mutex_; + + /** + * @brief conditional variable for threadpool synchronisation. + */ + std::condition_variable cv_; + + /** + * @brief flag for threadpool synchronisation. + */ + bool completed_; }; diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp index bd3fffb..e862452 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp @@ -9,6 +9,7 @@ enum fabric_status CANCELED, ABORTED, FAILED, - SUCCEEDED + LAUNCHED, + COMPLETED }; } \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index 71f450d..aae33d7 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -20,6 +20,40 @@ namespace xml_parser return document.FirstChildElement("Plan")->FirstChildElement(); } + /** + * @brief add std_capabilities/FabricCompletionRunner into the plan + * as an internal mechanism + * + * @param document XML document to modify + * + * @return modified document + */ + void add_closing_event(tinyxml2::XMLDocument &document) + { + // Get the root element + tinyxml2::XMLElement* plan = document.FirstChildElement("Plan"); + + // Get the element + tinyxml2::XMLElement* innerControl = plan->FirstChildElement("Control"); + + // Create the outer element + tinyxml2::XMLElement* outerControl = document.NewElement("Control"); + outerControl->SetAttribute("name", "sequential"); + + // Remove the inner from and move it inside the new outer + plan->DeleteChild(innerControl); + outerControl->InsertEndChild(innerControl); + + // 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); + + // Append the outer to + plan->InsertEndChild(outerControl); + } + /** * @brief search a string in a vector of strings * diff --git a/capabilities2_msgs/srv/GetFabricStatus.srv b/capabilities2_msgs/srv/GetFabricStatus.srv index 6e12c28..675f947 100644 --- a/capabilities2_msgs/srv/GetFabricStatus.srv +++ b/capabilities2_msgs/srv/GetFabricStatus.srv @@ -7,5 +7,6 @@ uint8 FABRIC_RUNNING=1 uint8 FABRIC_CANCELED=2 uint8 FABRIC_ABORTED=3 uint8 FABRIC_FAILED=4 -uint8 FABRIC_SUCCEEDED=5 -uint8 UNKNOWN=6 \ No newline at end of file +uint8 FABRIC_LAUNCHED=5 +uint8 FABRIC_COMPLETED=6 +uint8 UNKNOWN=7 \ No newline at end of file From 3749b53102f9a49cdbbdc5ef438ffd6b09b34f00 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 6 Feb 2025 01:01:02 +1100 Subject: [PATCH 03/17] fixed issues in the recursive functions used for extracting connections in the xml_parser::extract_cconnections --- .../capabilities_fabric.hpp | 8 + .../capabilities2_fabric/utils/xml_parser.hpp | 517 +++++++++--------- .../capabilities2_server/runner_cache.hpp | 2 +- 3 files changed, 264 insertions(+), 263 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 454a698..04c9120 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -165,6 +165,14 @@ class CapabilitiesFabric : public rclcpp::Node { process_feedback("A new execution started"); + xml_parser::add_closing_event(document); + + std::string modified_plan; + + xml_parser::convert_to_string(document, modified_plan); + + status_->info("Plan after adding closing event :\n\n " + modified_plan); + expected_providers_ = 0; completed_providers_ = 0; diff --git a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp index aae33d7..fc4fba1 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -8,322 +8,315 @@ namespace xml_parser { - /** - * @brief extract elements related plan - * - * @param document XML document to extract plan from - * - * @return plan in the form of tinyxml2::XMLElement* - */ - tinyxml2::XMLElement *get_plan(tinyxml2::XMLDocument &document) - { - return document.FirstChildElement("Plan")->FirstChildElement(); - } - - /** - * @brief add std_capabilities/FabricCompletionRunner into the plan - * as an internal mechanism - * - * @param document XML document to modify - * - * @return modified document - */ - void add_closing_event(tinyxml2::XMLDocument &document) - { - // Get the root element - tinyxml2::XMLElement* plan = document.FirstChildElement("Plan"); - - // Get the element - tinyxml2::XMLElement* innerControl = plan->FirstChildElement("Control"); - - // Create the outer element - tinyxml2::XMLElement* outerControl = document.NewElement("Control"); - outerControl->SetAttribute("name", "sequential"); - - // Remove the inner from and move it inside the new outer - plan->DeleteChild(innerControl); - outerControl->InsertEndChild(innerControl); - - // 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); - - // Append the outer to - plan->InsertEndChild(outerControl); - } - - /** - * @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 check if the element in question is the last in its level - * - * @param element element to be checked - * - * @return `true` if last and `false` otherwise - */ - bool isLastElement(tinyxml2::XMLElement *element) - { - return (element == element->Parent()->LastChildElement()); - } - - /** - * @brief check if the xml document has valid plan tags - * - * @param document XML document in question - * - * @return `true` if its valid and `false` otherwise - */ - bool check_plan_tag(tinyxml2::XMLDocument &document) +/** + * @brief extract elements related plan + * + * @param document XML document to extract plan from + * + * @return plan in the form of tinyxml2::XMLElement* + */ +tinyxml2::XMLElement* get_plan(tinyxml2::XMLDocument& document) +{ + return document.FirstChildElement("Plan")->FirstChildElement(); +} + +/** + * @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 check if the xml document has valid plan tags + * + * @param document XML document in question + * + * @return `true` if its valid and `false` otherwise + */ +bool check_plan_tag(tinyxml2::XMLDocument& document) +{ + std::string plan_tag(document.FirstChildElement()->Name()); + + if (plan_tag == "Plan") + return true; + else + return false; +} + +/** + * @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) { - std::string plan_tag(document.FirstChildElement()->Name()); - - if (plan_tag == "Plan") - return true; - else - return false; + tinyxml2::XMLPrinter printer; + element->Accept(&printer); + parameters = printer.CStr(); + return true; } - - /** - * @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 ¶meters) + else { - if (element) - { - tinyxml2::XMLPrinter printer; - element->Accept(&printer); - parameters = printer.CStr(); - return true; - } - else - { - parameters = ""; - return false; - } + 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(); +} - /** - * @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 status StatusClient used for logging and status 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(const std::shared_ptr status, tinyxml2::XMLElement* element, std::vector& events, + std::vector& providers, std::vector& control, std::vector& rejected) +{ + const char* name; + const char* provider; - /** - * @brief check the plan for invalid/unsupported control and event tags - * uses recursive approach to go through the plan - * - * @param status StatusClient used for logging and status 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(const std::shared_ptr status, tinyxml2::XMLElement *element, std::vector &events, std::vector &providers, - std::vector &control, std::vector &rejected) - { - const char *name; - const char *provider; + std::string parameter_string; + convert_to_string(element, parameter_string); - std::string parameter_string; - convert_to_string(element, parameter_string); + element->QueryStringAttribute("name", &name); + element->QueryStringAttribute("provider", &provider); - element->QueryStringAttribute("name", &name); - element->QueryStringAttribute("provider", &provider); + std::string nametag; + std::string providertag; - std::string nametag; - std::string providertag; + std::string typetag(element->Name()); - std::string typetag(element->Name()); + if (name) + nametag = name; + else + nametag = ""; - if (name) - nametag = name; - else - nametag = ""; + if (provider) + providertag = provider; + else + providertag = ""; - 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; - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !xml_parser::isLastElement(element); - 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 (typetag == "Control") + { + if (!foundInControl) { - if (!foundInControl) - { - std::string msg = "Control tag '" + nametag + "' not available in the valid list"; - status->error(msg); - rejected.push_back(parameter_string); - return false; - } + std::string msg = "Control tag '" + nametag + "' not available in the valid list"; + status->error(msg); + rejected.push_back(parameter_string); + return false; + } - if (hasChildren) - returnValue &= xml_parser::check_tags(status, element->FirstChildElement(), events, providers, control, rejected); + if (hasChildren) + returnValue &= xml_parser::check_tags(status, element->FirstChildElement(), events, providers, control, rejected); - if (hasSiblings) - returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); - } - else if (typetag == "Event") - { - if (!foundInEvents || !foundInProviders) - { - std::string msg = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; - status->error(msg); - rejected.push_back(parameter_string); - return false; - } - - if (hasSiblings) - returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); - } - else + if (hasSiblings) + returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); + } + else if (typetag == "Event") + { + if (!foundInEvents || !foundInProviders) { - std::string msg = "XML element is not valid :" + parameter_string; + std::string msg = "Event tag name '" + nametag + "' or provider '" + providertag + "' not available in the valid list"; status->error(msg); rejected.push_back(parameter_string); return false; } - return returnValue; + if (hasSiblings) + returnValue &= xml_parser::check_tags(status, element->NextSiblingElement(), events, providers, control, rejected); } - - /** - * @brief Returns control xml tags supported in extract_connections method - * - */ - std::vector get_control_list() + else { - std::vector tag_list; - - tag_list.push_back("sequential"); - tag_list.push_back("parallel"); - tag_list.push_back("recovery"); - - return tag_list; + std::string msg = "XML element is not valid :" + parameter_string; + status->error(msg); + rejected.push_back(parameter_string); + return false; } - /** - * @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 - */ - void 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; + return returnValue; +} - const char *name; - const char *provider; +/** + * @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; - element->QueryStringAttribute("name", &name); - element->QueryStringAttribute("provider", &provider); + const char* name = element->Attribute("name"); + const char* provider = element->Attribute("provider"); - std::string typetag(element->Name()); + std::string typetag(element->Name()); - std::string nametag; - std::string providertag; + std::string nametag; + std::string providertag; - if (name) - nametag = name; - else - nametag = ""; + if (name) + nametag = name; + else + nametag = ""; - if (provider) - providertag = provider; - else - providertag = ""; + if (provider) + providertag = provider; + else + providertag = ""; - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !xml_parser::isLastElement(element); + bool hasChildren = (element->FirstChildElement() != nullptr); + bool hasSiblings = (element->NextSiblingElement() != nullptr); - if ((typetag == "Control") and (nametag == "sequential")) + if (typetag == "Control") + { + if (nametag == "sequential") { if (hasChildren) - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_SUCCESS); - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + predecessor_id = + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_SUCCESS); } - else if ((typetag == "Control") and (nametag == "parallel")) + else if (nametag == "parallel") { if (hasChildren) - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_START); - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + predecessor_id = + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_START); } - else if ((typetag == "Control") and (nametag == "recovery")) + else if (nametag == "recovery") { if (hasChildren) - xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_FAILURE); - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id, connection_type); + predecessor_id = + xml_parser::extract_connections(element->FirstChildElement(), connections, connection_id, capabilities2::connection_type_t::ON_FAILURE); } - else if (typetag == "Event") + + if (hasSiblings) { - capabilities2::node_t node; + predecessor_id = xml_parser::extract_connections(element->NextSiblingElement(), connections, predecessor_id+1, connection_type); + } - node.source.runner = nametag; - node.source.provider = providertag; - node.source.parameters = element; + return predecessor_id; + } + else if (typetag == "Event") + { + capabilities2::node_t node; - predecessor_id = connection_id - 1; + node.source.runner = nametag; + node.source.provider = providertag; + node.source.parameters = element; - while (connections.count(connection_id) > 0) - connection_id += 1; + predecessor_id = connection_id - 1; - connections[connection_id] = node; + while (connections.count(connection_id) > 0) + connection_id += 1; - if ((connection_type == capabilities2::connection_type_t::ON_SUCCESS) and (connection_id != 0)) + 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) and (connection_id != 0)) + 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) and (connection_id != 0)) + else if (connection_type == capabilities2::connection_type_t::ON_FAILURE) connections[predecessor_id].target_on_failure = connections[connection_id].source; - - if (hasSiblings) - xml_parser::extract_connections(element->NextSiblingElement(), connections, connection_id + 1, connection_type); } + + 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 +} // namespace xml_parser diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index af1a2e3..784334f 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -152,7 +152,7 @@ class RunnerCache event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, std::placeholders::_2)); - print_("Configured triggers for capability" + capability + ": \n\tStarted: " + on_started_capability + + print_("Configured triggers for capability " + capability + ": \n\tStarted: " + on_started_capability + " \n\tFailure: " + on_failure_capability + " \n\tSuccess: " + on_success_capability + "\n\tStopped: " + on_stopped_capability + "\n", true, false); } From 43bf4fa246de83e82c3f096df10cce0bf9978721 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sat, 8 Feb 2025 17:31:26 +1100 Subject: [PATCH 04/17] added more runner for capabilities --- capabilities2_msgs/msg/CapabilityEvent.msg | 6 + .../capabilities2_runner/runner_base.hpp | 18 +++ .../capability_get_runner.hpp | 91 +++++++++++++++ .../fabric_set_plan_runner.hpp | 62 ++++++++++ .../src/capabilities2_runner.cpp | 6 +- .../occupancygrid_runner.hpp | 37 +++++- .../robotpose_runner.hpp | 16 ++- .../prompt_capability_runner.hpp | 63 ++++++++++ .../prompt_occupancy_runner.hpp | 5 +- .../prompt_plan_response_runner.hpp | 109 ------------------ ...uest_runner.hpp => prompt_plan_runner.hpp} | 4 +- .../prompt_pose_runner.hpp | 6 +- .../src/prompt_runners.cpp | 4 +- 13 files changed, 306 insertions(+), 121 deletions(-) create mode 100644 capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp create mode 100644 capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp create mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp delete mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp rename capabilities2_runner_prompt/include/capabilities2_runner_prompt/{prompt_plan_request_runner.hpp => prompt_plan_runner.hpp} (96%) diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg index 581a67d..baf45b4 100644 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ b/capabilities2_msgs/msg/CapabilityEvent.msg @@ -12,6 +12,9 @@ int8 thread_id # status message string text +# status message +string element + # Event types uint8 IDLE=0 uint8 STARTED=1 @@ -29,5 +32,8 @@ bool server_ready # Whether a failure occured bool error +# Whether output data +bool is_element + # PID of the related process int32 pid diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 11e4ec2..e4f81cb 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -529,6 +529,24 @@ class RunnerBase print_(message); } + void output_(const std::string text, const std::string element, int thread_id = -1) + { + auto message = Event(); + + message.header.stamp = rclcpp::Clock().now(); + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.thread_id = thread_id; + message.text = text; + message.element = element; + message.is_element = true; + message.error = false; + message.server_ready = true; + message.event = Event::IDLE; + + print_(message); + } + /** * @brief shared pointer to the capabilities node. Allows to use ros node related functionalities */ diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp new file mode 100644 index 0000000..49c340f --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp @@ -0,0 +1,91 @@ +#pragma once + +#include + +#include +#include +#include + +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief Executor runner class + * + * Class to run capabilities2 executor action based capability + * + */ +class CapabilityGetRunner : public ServiceRunner +{ +public: + CapabilityGetRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/capabilities/get_capability_specs", print); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + '' + * @return ActionT::Goal the generated goal + */ + virtual capabilities2_msgs::srv::GetCapabilitySpecs::Request generate_request(tinyxml2::XMLElement* parameters, + int id) override + { + capabilities2_msgs::srv::GetCapabilitySpecs::Request request; + return request; + } + + virtual std::string update_on_success(std::string& parameters) + { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + + // Create the OccupancyGrid element as a child of the existing parameters element + tinyxml2::XMLElement* capabilitySpecsElement = element->GetDocument()->NewElement("CapabilitySpecs"); + element->InsertEndChild(capabilitySpecsElement); + + for (const auto& spec : response_->capability_specs) + { + // Create a element + tinyxml2::XMLElement* specElement = element->GetDocument()->NewElement("CapabilitySpec"); + + // Set attributes + specElement->SetAttribute("package", spec.package.c_str()); + specElement->SetAttribute("type", spec.type.c_str()); + + if (!spec.default_provider.empty()) + { + specElement->SetAttribute("default_provider", spec.default_provider.c_str()); + } + + // Add content as a child element inside + tinyxml2::XMLElement* contentElement = element->GetDocument()->NewElement("content"); + contentElement->SetText(spec.content.c_str()); + specElement->InsertEndChild(contentElement); + + // Append to + capabilitySpecsElement->InsertEndChild(specElement); + } + + // Convert updated XML back to string and return + std::string result = convert_to_string(element); + return result; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp new file mode 100644 index 0000000..3d83b04 --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/fabric_set_plan_runner.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include + +#include +#include +#include + +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief Executor runner class + * + * Class to run capabilities2 executor action based capability + * + */ +class FabricSetPlanRunner : public ServiceRunner +{ +public: + FabricSetPlanRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/capabilities_fabric/set_plan", print); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + * @return ActionT::Goal the generated goal + */ + virtual capabilities2_msgs::srv::SetFabricPlan::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + { + tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); + + capabilities2_msgs::srv::SetFabricPlan::Request request; + + // Check if the element was found and has text content + if (planElement && planElement->GetText()) + { + request.plan = std::string(planElement->GetText()); + } + + return request; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp index 26e1b95..39c18de 100644 --- a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -1,6 +1,10 @@ #include #include #include +#include +#include // register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) \ No newline at end of file +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_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index d95b9f3..9e658e1 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -37,12 +37,43 @@ class OccupancyGridRunner : public TopicRunner protected: /** - * @brief Update on_success event parameters with new data if avaible. + * @brief Update on_success event parameters with new data from an OccupancyGrid message if avaible. * * This function is used to inject new data into the XMLElement containing * parameters related to the on_success trigger event * - * A pattern needs to be implemented in the derived class + * + *
+ * + * 1700000000 + * 123456789 + * + * map + *
+ * + * 0.05 + * 100 + * 100 + * + * + * 1.0 + * 2.0 + * 0.0 + * + * + * 0.0 + * 0.0 + * 0.0 + * 1.0 + * + * + * + * 1700000000 + * 987654321 + * + * + * 0 1 2 3 4 5 6 7 8 9 + * * * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters @@ -154,6 +185,8 @@ class OccupancyGridRunner : public TopicRunner // Return the updated parameters element with OccupancyGrid added std::string result = convert_to_string(element); + output_("on_success trigger parameter", result); + return result; }; }; diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index c68ccfd..ed963a3 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -42,7 +42,19 @@ class RobotPoseRunner : public TopicRunner + * + * 1.23 + * 4.56 + * 7.89 + * + * + * 0.12 + * 0.34 + * 0.56 + * 0.78 + * + * * * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters @@ -96,6 +108,8 @@ class RobotPoseRunner : public TopicRunner + +#include +#include +#include + +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief Executor runner class + * + * Class to run capabilities2 executor action based capability + * + */ +class FabricSetPlanRunner : public ServiceRunner +{ +public: + FabricSetPlanRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/capabilities_fabric/set_plan", print); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + '' + * @return ActionT::Goal the generated goal + */ + virtual capabilities2_msgs::srv::SetFabricPlan::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + { + tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); + + capabilities2_msgs::srv::SetFabricPlan::Request request; + + // Check if the element was found and has text content + if (planElement && planElement->GetText()) + { + request.plan = std::string(planElement->GetText()); + } + + return request; + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index af21915..8a97b1f 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -57,8 +57,7 @@ class PromptOccupancyRunner : public ServiceRunner prompt_msgs::srv::Prompt::Request request; request.prompt.prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " - "is given as a ros2 nav_msgs occupancy_grid of which the content are " + - data; + "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + data; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; @@ -73,6 +72,8 @@ class PromptOccupancyRunner : public ServiceRunner request.prompt.options.push_back(modelOption2); + info_("prompting with : " + request.prompt.prompt, id); + return request; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp deleted file mode 100644 index 784c52d..0000000 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief Executor runner class - * - * Class to run capabilities2 executor action based capability - * - */ -class PromptPlanResponseRunner : public ActionRunner -{ -public: - PromptPlanResponseRunner() : ActionRunner() - { - } - - /** - * @brief Starter function for starting the action runner - * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_action(node, run_config, "capabilities_fabric", print); - } - -protected: - /** - * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * @param parameters XMLElement that contains parameters in the format - '' - * @return ActionT::Goal the generated goal - */ - virtual capabilities2_msgs::action::Plan::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) override - { - tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); - - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); - - // Check if the element was found and has text content - if (planElement && planElement->GetText()) - { - goal_msg.plan = std::string(planElement->GetText()); - } - - return goal_msg; - } - - /** - * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() - * - * @param msg feedback message from the action server - * @return std::string of feedback information - */ - virtual std::string - generate_feedback(const typename capabilities2_msgs::action::Plan::Feedback::ConstSharedPtr msg) override - { - std::string feedback = msg->progress; - return feedback; - } - - /** - * @brief Update on_failure event parameters with new data if avaible. - * - * This function is used to inject new data into the XMLElement containing - * parameters related to the on_failure trigger event - * - * A pattern needs to be implemented in the derived class - * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters - */ - virtual std::string update_on_failure(std::string& parameters) - { - tinyxml2::XMLElement* element = convert_to_xml(parameters); - - element->SetAttribute("replan", true); - - // Create the failed elements element as a child of the existing parameters element - tinyxml2::XMLElement* failedElements = element->GetDocument()->NewElement("FailedElements"); - element->InsertEndChild(failedElements); - - std::string failedElementsString = ""; - - for (const auto& element : result_->failed_elements) - failedElementsString += element; - - failedElements->SetText(failedElementsString.c_str()); - - std::string result = convert_to_string(element); - - return result; - }; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp similarity index 96% rename from capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp rename to capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index 62935ac..ce88b40 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -15,10 +15,10 @@ namespace capabilities2_runner * call on the prompt_tools/prompt service, providing it as a capability that prompts * capabilitie plans values */ -class PromptPlanRequestRunner : public ServiceRunner +class PromptPlanRunner : public ServiceRunner { public: - PromptPlanRequestRunner() : ServiceRunner() + PromptPlanRunner() : ServiceRunner() { } diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index b7cb004..d5dff0a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -56,8 +56,8 @@ class PromptPoseRunner : public ServiceRunner prompt_msgs::srv::Prompt::Request request; - request.prompt.prompt = - "The position of the robot is given as a standard ros2 geometry message of which the content are " + data; + request.prompt.prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " + "the content are " + data; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; @@ -72,6 +72,8 @@ class PromptPoseRunner : public ServiceRunner request.prompt.options.push_back(modelOption2); + info_("prompting with : " + request.prompt.prompt, id); + return request; } }; diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp index 361733f..080dda2 100644 --- a/capabilities2_runner_prompt/src/prompt_runners.cpp +++ b/capabilities2_runner_prompt/src/prompt_runners.cpp @@ -3,12 +3,12 @@ #include #include #include -#include +#include #include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptTextRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPoseRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptOccupancyRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanRequestRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanResponseRunner, capabilities2_runner::RunnerBase) \ No newline at end of file From 76e3f8f5e9777aca7a2e48fc644381f503de92c3 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 10 Feb 2025 14:53:41 +1100 Subject: [PATCH 05/17] updated various capabilities --- capabilities2_fabric/plans/default.xml | 6 +- capabilities2_fabric/plans/prompt_1.xml | 7 +++ capabilities2_fabric/plans/prompt_2.xml | 7 +++ capabilities2_fabric/plans/prompt_3.xml | 7 +++ capabilities2_fabric/plans/prompt_4.xml | 9 +++ .../capability_get_runner.hpp | 27 ++++++++ .../prompt_capability_runner.hpp | 63 ++++++++++++------- .../prompt_occupancy_runner.hpp | 14 ++--- .../prompt_plan_runner.hpp | 19 ++++-- .../prompt_pose_runner.hpp | 22 +++---- 10 files changed, 129 insertions(+), 52 deletions(-) create mode 100644 capabilities2_fabric/plans/prompt_1.xml create mode 100644 capabilities2_fabric/plans/prompt_2.xml create mode 100644 capabilities2_fabric/plans/prompt_3.xml create mode 100644 capabilities2_fabric/plans/prompt_4.xml diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index 40fdfd9..038690d 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -1,11 +1,13 @@ + + - - + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml new file mode 100644 index 0000000..08e399b --- /dev/null +++ b/capabilities2_fabric/plans/prompt_1.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml new file mode 100644 index 0000000..79a50da --- /dev/null +++ b/capabilities2_fabric/plans/prompt_2.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml new file mode 100644 index 0000000..5a81e17 --- /dev/null +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml new file mode 100644 index 0000000..55d3155 --- /dev/null +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp index 49c340f..89b03f1 100644 --- a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp @@ -51,6 +51,33 @@ class CapabilityGetRunner : public ServiceRunner + * name: Navigation dependencies: + * - MapServer + * - PathPlanner + * + * + * + * + * name: ObjectDetection + * dependencies: + * - Camera + * - ImageProcessor + * + * + * + * + * @param parameters + * @return std::string + */ + virtual std::string update_on_success(std::string& parameters) { tinyxml2::XMLElement* element = convert_to_xml(parameters); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp index 4ce82e8..ab8f43a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp @@ -1,27 +1,25 @@ #pragma once - -#include - #include #include #include - #include -#include +#include +#include namespace capabilities2_runner { /** - * @brief Executor runner class - * - * Class to run capabilities2 executor action based capability + * @brief prompt capabilities runner * + * This class is a wrapper around the capabilities2 service runner and is used to pass + * data to prompt_tools/prompt service, providing it as a capability that prompts + * robot capabilities. */ -class FabricSetPlanRunner : public ServiceRunner +class PromptCapabilityRunner : public ServiceRunner { public: - FabricSetPlanRunner() : ServiceRunner() + PromptCapabilityRunner() : ServiceRunner() { } @@ -34,27 +32,46 @@ class FabricSetPlanRunner : public ServiceRunner print) override { - init_service(node, run_config, "/capabilities_fabric/set_plan", print); + init_service(node, run_config, "prompt", print); } protected: /** - * @brief This generate goal function overrides the generate_goal() function from ActionRunner() - * @param parameters XMLElement that contains parameters in the format - '' - * @return ActionT::Goal the generated goal + * @brief Generate a request from parameters given. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request */ - virtual capabilities2_msgs::srv::SetFabricPlan::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { - tinyxml2::XMLElement* planElement = parameters->FirstChildElement("ReceievdPlan"); + tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); + + tinyxml2::XMLPrinter printer; + capabilitySpecsElement->Accept(&printer); + + std::string data(printer.CStr()); + + prompt_msgs::srv::Prompt::Request request; + + request.prompt.prompt = "The capabilities of the robot are given as follows" + data; + + prompt_msgs::msg::ModelOption modelOption1; + modelOption1.key = "model"; + modelOption1.value = "llama3.1:8b"; + + request.prompt.options.push_back(modelOption1); + + prompt_msgs::msg::ModelOption modelOption2; + modelOption2.key = "stream"; + modelOption2.value = "false"; + modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - capabilities2_msgs::srv::SetFabricPlan::Request request; + request.prompt.options.push_back(modelOption2); - // Check if the element was found and has text content - if (planElement && planElement->GetText()) - { - request.plan = std::string(planElement->GetText()); - } + info_("prompting with : " + request.prompt.prompt, id); return request; } diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 8a97b1f..2b918aa 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -35,15 +35,11 @@ class PromptOccupancyRunner : public ServiceRunner } /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * A pattern needs to be implemented in the derived class - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request + * @brief Generate request for the PromptOccupancyRunner + * + * @param parameters parameter values + * @param id thread id + * @return prompt_msgs::srv::Prompt::Request */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index ce88b40..5768d21 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -49,22 +49,31 @@ class PromptPlanRunner : public ServiceRunner virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { bool replan; + const char* task; + std::string taskString; + parameters->QueryBoolAttribute("replan", &replan); + parameters->QueryStringAttribute("task", &task); + + if (task) + taskString = task; + else + taskString = ""; prompt_msgs::srv::Prompt::Request request; if (!replan) { - request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Return " - "only the xml plan without explanations or comments"; + request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + + taskString + ". Return only the xml plan without explanations or comments"; } else { tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); - request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task. " - "Just give the xml plan without explanations or comments. These XML elements had " - "incompatibilities. " + + request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task of " + + taskString + ". Just give the xml plan without explanations or comments. These XML " + "elements had incompatibilities. " + std::string(failedElements->GetText()) + "Recorrect them as well"; } diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index d5dff0a..ff88918 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -9,10 +9,10 @@ namespace capabilities2_runner { /** - * @brief prompt capability runner + * @brief prompt pose runner * - * This class is a wrapper around the capabilities2 service runner and is used to - * call on the prompt_tools/prompt service, providing it as a capability that prompts + * This class is a wrapper around the capabilities2 service runner and is used to pass + * data to prompt_tools/prompt service, providing it as a capability that prompts * robot pose values */ class PromptPoseRunner : public ServiceRunner @@ -35,17 +35,13 @@ class PromptPoseRunner : public ServiceRunner } /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * A pattern needs to be implemented in the derived class - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request + * @brief Generate request for trnasforming data for PrompPoseRunner + * + * @param parameters parameter values + * @param id thread id + * @return prompt_msgs::srv::Prompt::Request */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); From 52c14dfadf21cc4c36fd4145c3f3889c78bbefbd Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 10 Feb 2025 17:17:16 +1100 Subject: [PATCH 06/17] completed prompt tools runner implementations --- capabilities2_fabric/plans/default.xml | 8 +- capabilities2_fabric/plans/prompt_1.xml | 2 +- capabilities2_fabric/plans/prompt_2.xml | 2 +- capabilities2_fabric/plans/prompt_3.xml | 2 +- capabilities2_fabric/plans/prompt_4.xml | 4 +- .../capabilities2_runner/service_runner.hpp | 11 +++ .../prompt_capability_runner.hpp | 53 ++-------- .../prompt_occupancy_runner.hpp | 53 +++------- .../prompt_plan_runner.hpp | 64 +++--------- .../prompt_pose_runner.hpp | 54 +++------- .../prompt_service_runner.hpp | 99 +++++++++++++++++++ .../prompt_text_runner.hpp | 52 ++-------- .../src/prompt_runners.cpp | 4 +- 13 files changed, 176 insertions(+), 232 deletions(-) create mode 100644 capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index 038690d..c09a2b0 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -2,12 +2,12 @@ - + - + - - + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml index 08e399b..b83d8b8 100644 --- a/capabilities2_fabric/plans/prompt_1.xml +++ b/capabilities2_fabric/plans/prompt_1.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml index 79a50da..61243ab 100644 --- a/capabilities2_fabric/plans/prompt_2.xml +++ b/capabilities2_fabric/plans/prompt_2.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index 5a81e17..64a54c9 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml index 55d3155..6a51a15 100644 --- a/capabilities2_fabric/plans/prompt_4.xml +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -2,8 +2,8 @@ - - + + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index eda0065..d744cf6 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -93,6 +93,7 @@ class ServiceRunner : public RunnerBase info_("get result call succeeded", id); response_ = future.get(); + process_response(response_, id); // trigger the events related to on_success state if (events[execute_id].on_success != "") @@ -158,6 +159,16 @@ class ServiceRunner : public RunnerBase */ virtual typename ServiceT::Request generate_request(tinyxml2::XMLElement* parameters, int id) = 0; + /** + * @brief Process the reponse and print data as required + * + * @param response service reponse + * @param id thread id + */ + virtual void process_response(typename ServiceT::Response::SharedPtr response, int id) + { + } + typename rclcpp::Client::SharedPtr service_client_; typename ServiceT::Response::SharedPtr response_; }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp index ab8f43a..65b71d2 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -16,36 +14,20 @@ namespace capabilities2_runner * data to prompt_tools/prompt service, providing it as a capability that prompts * robot capabilities. */ -class PromptCapabilityRunner : public ServiceRunner +class PromptCapabilityRunner : public PromptServiceRunner { public: - PromptCapabilityRunner() : ServiceRunner() + PromptCapabilityRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the action runner + * @brief generate the prompt used for prompting the capabilities. * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file + * @param parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - -protected: - /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request - */ - virtual prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); @@ -54,26 +36,9 @@ class PromptCapabilityRunner : public ServiceRunner std::string data(printer.CStr()); - prompt_msgs::srv::Prompt::Request request; - - request.prompt.prompt = "The capabilities of the robot are given as follows" + data; - - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); - - info_("prompting with : " + request.prompt.prompt, id); + std::string prompt = "The capabilities of the robot are given as follows" + data; - return request; + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 2b918aa..52eb134 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -15,33 +13,20 @@ namespace capabilities2_runner * call on the prompt_tools/prompt service, providing it as a capability that prompts * occupancy grid map values */ -class PromptOccupancyRunner : public ServiceRunner +class PromptOccupancyRunner : public PromptServiceRunner { public: - PromptOccupancyRunner() : ServiceRunner() + PromptOccupancyRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the service runner + * @brief generate the prompt used for prompting the occupancy grids. * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file + * @param parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - - /** - * @brief Generate request for the PromptOccupancyRunner - * - * @param parameters parameter values - * @param id thread id - * @return prompt_msgs::srv::Prompt::Request - */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); @@ -50,27 +35,11 @@ class PromptOccupancyRunner : public ServiceRunner std::string data(printer.CStr()); - prompt_msgs::srv::Prompt::Request request; - - request.prompt.prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " - "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + data; - - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); - - info_("prompting with : " + request.prompt.prompt, id); + std::string prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " + "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + + data; - return request; + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index 5768d21..40a15cd 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -15,42 +13,25 @@ namespace capabilities2_runner * call on the prompt_tools/prompt service, providing it as a capability that prompts * capabilitie plans values */ -class PromptPlanRunner : public ServiceRunner +class PromptPlanRunner : public PromptServiceRunner { public: - PromptPlanRunner() : ServiceRunner() + PromptPlanRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the service runner + * @brief generate the prompt used for prompting the occupancy grids. * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file + * @param parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - -protected: - /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * A pattern needs to be implemented in the derived class - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request - */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { bool replan; const char* task; std::string taskString; + std::string prompt; parameters->QueryBoolAttribute("replan", &replan); parameters->QueryStringAttribute("task", &task); @@ -60,37 +41,22 @@ class PromptPlanRunner : public ServiceRunner else taskString = ""; - prompt_msgs::srv::Prompt::Request request; - if (!replan) { - request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + - taskString + ". Return only the xml plan without explanations or comments"; + prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + + ". Return only the xml plan without explanations or comments"; } else { tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); - request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task of " + - taskString + ". Just give the xml plan without explanations or comments. These XML " - "elements had incompatibilities. " + - std::string(failedElements->GetText()) + "Recorrect them as well"; + prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + + ". Just give the xml plan without explanations or comments. These XML " + "elements had incompatibilities. " + + std::string(failedElements->GetText()) + "Recorrect them as well"; } - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); - - return request; + return prompt; } /** diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index ff88918..58c3a17 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -15,33 +13,20 @@ namespace capabilities2_runner * data to prompt_tools/prompt service, providing it as a capability that prompts * robot pose values */ -class PromptPoseRunner : public ServiceRunner +class PromptPoseRunner : public PromptServiceRunner { public: - PromptPoseRunner() : ServiceRunner() + PromptPoseRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the service runner + * @brief generate the prompt used for prompting the capabilities. * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file + * @param parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - - /** - * @brief Generate request for trnasforming data for PrompPoseRunner - * - * @param parameters parameter values - * @param id thread id - * @return prompt_msgs::srv::Prompt::Request - */ - virtual prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); @@ -50,27 +35,10 @@ class PromptPoseRunner : public ServiceRunner std::string data(printer.CStr()); - prompt_msgs::srv::Prompt::Request request; - - request.prompt.prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " - "the content are " + data; - - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); - - info_("prompting with : " + request.prompt.prompt, id); - - return request; + std::string prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " + "the content are " + + data; + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp new file mode 100644 index 0000000..50b5c4a --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -0,0 +1,99 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +namespace capabilities2_runner +{ +/** + * @brief prompt service runner + * + * This class is a base class and a wrapper around the capabilities2 service runner + * and is used to call on the prompt_tools/prompt service, providing it as a capability + * that prompts capabilitie plans values + */ +class PromptServiceRunner : public ServiceRunner +{ +public: + PromptServiceRunner() : ServiceRunner() + { + } + + /** + * @brief Starter function for starting the service runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + std::function print) override + { + init_service(node, run_config, "/prompt_tools/prompt", print); + } + +protected: + /** + * @brief Generate a request from parameters given. + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed action + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return prompt_msgs::srv::Prompt::Request the generated request + */ + virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + { + bool stream; + const char* modelChar; + std::string model; + + parameters->QueryBoolAttribute("stream", &stream); + parameters->QueryStringAttribute("model", &modelChar); + + if (modelChar) + model = modelChar; + else + model = "llama3.1:8b"; + + prompt_msgs::srv::Prompt::Request request; + + prompt_msgs::msg::ModelOption modelOption1; + modelOption1.key = "model"; + modelOption1.value = model; + + request.prompt.options.push_back(modelOption1); + + prompt_msgs::msg::ModelOption modelOption2; + modelOption2.key = "stream"; + modelOption2.value = stream; + modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; + + request.prompt.options.push_back(modelOption2); + + request.prompt.prompt = generate_prompt(parameters); + + info_("prompting with : " + request.prompt.prompt, id); + + return request; + } + + /** + * @brief generate the prompt used for prompting the data. + * + * @param parameters tinyXML2 parameters + * @return std::string + */ + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) = 0; + + virtual void process_response(typename prompt_msgs::srv::Prompt::Response::SharedPtr response, int id) + { + info_("response received : " + response->response.response, id); + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 6262cc7..318d797 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -2,9 +2,7 @@ #include #include #include -#include -#include -#include +#include namespace capabilities2_runner { @@ -15,37 +13,20 @@ namespace capabilities2_runner * call on the prompt_tools/prompt service, providing it as a capability that prompts * text values */ -class PromptTextRunner : public ServiceRunner +class PromptTextRunner : public PromptServiceRunner { public: - PromptTextRunner() : ServiceRunner() + PromptTextRunner() : PromptServiceRunner() { } /** - * @brief Starter function for starting the service runner + * @brief generate the prompt used for prompting the text. * - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities - * @param run_config runner configuration loaded from the yaml file + * @param parameters tinyXML2 parameters + * @return std::string */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function print) override - { - init_service(node, run_config, "prompt", print); - } - - /** - * @brief Generate a request from parameters given. - * - * This function is used in conjunction with the trigger function to inject type erased parameters - * into the typed action - * - * A pattern needs to be implemented in the derived class - * - * @param parameters - * @return prompt_msgs::srv::Prompt::Request the generated request - */ - virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) { tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); @@ -54,24 +35,9 @@ class PromptTextRunner : public ServiceRunner std::string data(printer.CStr()); - prompt_msgs::srv::Prompt::Request request; - - request.prompt.prompt = "The response was " + data; - - prompt_msgs::msg::ModelOption modelOption1; - modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; - - request.prompt.options.push_back(modelOption1); - - prompt_msgs::msg::ModelOption modelOption2; - modelOption2.key = "stream"; - modelOption2.value = "false"; - modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; - - request.prompt.options.push_back(modelOption2); + std::string prompt = "The response was " + data; - return request; + return prompt; } }; diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp index 080dda2..1ebcd4d 100644 --- a/capabilities2_runner_prompt/src/prompt_runners.cpp +++ b/capabilities2_runner_prompt/src/prompt_runners.cpp @@ -4,11 +4,11 @@ #include #include #include -#include +#include // register runner plugins PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptTextRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPoseRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptOccupancyRunner, capabilities2_runner::RunnerBase) PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanRunner, capabilities2_runner::RunnerBase) -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanResponseRunner, capabilities2_runner::RunnerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptCapabilityRunner, capabilities2_runner::RunnerBase) \ No newline at end of file From 944c90d084bde9056148ecf3773820fee63f7016 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 10 Feb 2025 23:33:54 +1100 Subject: [PATCH 07/17] added new examples and sample cases for prompting --- capabilities2_fabric/config/fabric.yaml | 6 ++- .../docs/prompt_capability_runner_ex1.md | 38 ++++++++++++++ .../docs/prompt_occupancy_runner_ex1.md | 52 +++++++++++++++++++ .../docs/prompt_plan_runner_ex1.md | 52 +++++++++++++++++++ .../docs/prompt_pose_runner_ex1.md | 52 +++++++++++++++++++ .../docs/waypoint_runner_ex1.md | 4 +- .../docs/waypoint_runner_ex2.md | 4 +- capabilities2_fabric/readme.md | 23 ++++++-- docs/prompt_tools_setup.md | 18 +++++++ docs/setup.md | 10 ++++ readme.md | 1 + 11 files changed, 251 insertions(+), 9 deletions(-) create mode 100644 capabilities2_fabric/docs/prompt_capability_runner_ex1.md create mode 100644 capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md create mode 100644 capabilities2_fabric/docs/prompt_plan_runner_ex1.md create mode 100644 capabilities2_fabric/docs/prompt_pose_runner_ex1.md create mode 100644 docs/prompt_tools_setup.md diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index e12c542..c6a2b99 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,5 +1,9 @@ fabric_client: ros__parameters: - 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/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 new file mode 100644 index 0000000..b23e1c7 --- /dev/null +++ b/capabilities2_fabric/docs/prompt_capability_runner_ex1.md @@ -0,0 +1,38 @@ +## 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 +``` \ 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 new file mode 100644 index 0000000..2489180 --- /dev/null +++ b/capabilities2_fabric/docs/prompt_occupancy_runner_ex1.md @@ -0,0 +1,52 @@ +## 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 +``` \ 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 new file mode 100644 index 0000000..01ee941 --- /dev/null +++ b/capabilities2_fabric/docs/prompt_plan_runner_ex1.md @@ -0,0 +1,52 @@ +## 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 +``` \ 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 new file mode 100644 index 0000000..7c17a52 --- /dev/null +++ b/capabilities2_fabric/docs/prompt_pose_runner_ex1.md @@ -0,0 +1,52 @@ +## 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 +``` \ No newline at end of file diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 4e978e8..5e199ef 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -1,4 +1,4 @@ -## WaypointRunner Example 1 +## WaypointRunner Example 1 - Single Goal ### Dependencies @@ -37,7 +37,7 @@ source install/setup.bash ros2 launch capabilities2_server server.launch.py ``` -### Start the fabric +### Start the Capabilities2 Fabric ```bash source install/setup.bash diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 8a49adb..871b296 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -1,4 +1,4 @@ -## WaypointRunner Example 2 +## WaypointRunner Example 2 - Goal Sequence ### Dependencies @@ -37,7 +37,7 @@ source install/setup.bash ros2 launch capabilities2_server server.launch.py ``` -### Start the fabric +### Start the Capabilities2 Fabric ```bash source install/setup.bash diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index f541a5a..6217de6 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -51,8 +51,8 @@ Below is an example XML plan for configuring a set of capabilities: - - + + ``` @@ -73,7 +73,22 @@ Below is an example XML plan for configuring a set of capabilities: ### 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 +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 \ No newline at end of file +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 \ No newline at end of file diff --git a/docs/prompt_tools_setup.md b/docs/prompt_tools_setup.md new file mode 100644 index 0000000..466c21e --- /dev/null +++ b/docs/prompt_tools_setup.md @@ -0,0 +1,18 @@ +# Dependency installation for Prompt Tools Runners + +## Clone packages + +Clone the prompt tools package same workspace if its not already availabe in the workspace. Capabilities2 Prompt Runners are dependent on this package. + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/prompt_tools.git -b develop +``` + +## Dependency Installation + +Move to workspace root and run the following command to install dependencies + +```bash +rosdep install --from-paths src --ignore-src -r -y +``` \ No newline at end of file diff --git a/docs/setup.md b/docs/setup.md index 736481e..74bce4a 100644 --- a/docs/setup.md +++ b/docs/setup.md @@ -11,6 +11,16 @@ mkdir -p /home/$USER/capabilities_ws/src cd /home/$USER/capabilities_ws/src ``` +### Tinyxml2 Installation + +Clone and install the tinyXML2 package + +```bash +git clone https://github.com/leethomason/tinyxml2.git +cd tinyxml2 +make install +``` + ### Cloning the Packages Clone the package using Git diff --git a/readme.md b/readme.md index a5efcdf..d47f97b 100644 --- a/readme.md +++ b/readme.md @@ -33,6 +33,7 @@ Runners can be created using the runner API parent classes [here](./capabilities - [Setup Instructions with devcontainer](./docs/setup_with_dev.md) - [Setup Instructions without devcontainer](./docs/setup.md) - [Dependency installation for Nav2 Runners](./docs/nav2_setup.md) +- [Dependency installation for Prompt Runners](./docs/prompt_tools_setup.md) ## Quick Startup information From 819bf9089d9a42fd6d54e17c17a6ceedbca9b09b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 12 Feb 2025 00:04:48 +1100 Subject: [PATCH 08/17] removed streaming option from prompt_runner commands as prompt_tools does not support that --- capabilities2_fabric/plans/default.xml | 8 ++++---- capabilities2_fabric/plans/prompt_1.xml | 2 +- capabilities2_fabric/plans/prompt_2.xml | 2 +- capabilities2_fabric/plans/prompt_3.xml | 2 +- capabilities2_fabric/plans/prompt_4.xml | 4 ++-- capabilities2_fabric/readme.md | 6 +++--- .../capabilities2_runner_prompt/prompt_service_runner.hpp | 4 +--- 7 files changed, 13 insertions(+), 15 deletions(-) diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index c09a2b0..12afddf 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -2,12 +2,12 @@ - + - + - - + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml index b83d8b8..c56b94f 100644 --- a/capabilities2_fabric/plans/prompt_1.xml +++ b/capabilities2_fabric/plans/prompt_1.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml index 61243ab..e32c29a 100644 --- a/capabilities2_fabric/plans/prompt_2.xml +++ b/capabilities2_fabric/plans/prompt_2.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index 64a54c9..861b021 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml index 6a51a15..cce166e 100644 --- a/capabilities2_fabric/plans/prompt_4.xml +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -2,8 +2,8 @@ - - + + \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index 6217de6..e7963a9 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -44,14 +44,14 @@ Below is an example XML plan for configuring a set of capabilities: - + - + - + diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 50b5c4a..2095251 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -48,11 +48,9 @@ class PromptServiceRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { - bool stream; const char* modelChar; std::string model; - parameters->QueryBoolAttribute("stream", &stream); parameters->QueryStringAttribute("model", &modelChar); if (modelChar) @@ -70,7 +68,7 @@ class PromptServiceRunner : public ServiceRunner prompt_msgs::msg::ModelOption modelOption2; modelOption2.key = "stream"; - modelOption2.value = stream; + modelOption2.value = false; modelOption2.type = prompt_msgs::msg::ModelOption::BOOL_TYPE; request.prompt.options.push_back(modelOption2); From 24d178ee8e009ae9d0c4be36a887c7a86d18056a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 12 Feb 2025 14:05:10 +1100 Subject: [PATCH 09/17] updated incorrect plugin.xml files --- capabilities2_runner_capabilities/plugins.xml | 10 +++++++++ capabilities2_runner_nav2/plugins.xml | 8 +++---- .../prompt_service_runner.hpp | 2 +- capabilities2_runner_prompt/plugins.xml | 21 ++++++++++++------- 4 files changed, 28 insertions(+), 13 deletions(-) diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml index 4789f11..32fd1e9 100644 --- a/capabilities2_runner_capabilities/plugins.xml +++ b/capabilities2_runner_capabilities/plugins.xml @@ -1,7 +1,17 @@ + + + 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_nav2/plugins.xml b/capabilities2_runner_nav2/plugins.xml index b7fcdaf..2c4d4ca 100644 --- a/capabilities2_runner_nav2/plugins.xml +++ b/capabilities2_runner_nav2/plugins.xml @@ -1,7 +1,7 @@ - + - A plugin that provide nav2 waypoint navigation capability + A plugin that provide occupancy grid map extraction capability @@ -9,9 +9,9 @@ A plugin that provide robot pose extraction capability - + - A plugin that provide occupancy grid map extraction capability + A plugin that provide nav2 waypoint navigation capability diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 2095251..1e93f12 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -31,7 +31,7 @@ class PromptServiceRunner : public ServiceRunner virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, std::function print) override { - init_service(node, run_config, "/prompt_tools/prompt", print); + init_service(node, run_config, "/prompt_bridge/prompt", print); } protected: diff --git a/capabilities2_runner_prompt/plugins.xml b/capabilities2_runner_prompt/plugins.xml index f7ea14e..343617d 100644 --- a/capabilities2_runner_prompt/plugins.xml +++ b/capabilities2_runner_prompt/plugins.xml @@ -1,22 +1,27 @@ - + - A plugin that provide text prompting capability + A plugin that provide capability prompting capability - + - A plugin that provide pose prompting capability + A plugin that provide occupancy grid map prompting capability - + - A plugin that provide occupancy grid map prompting capability + A plugin that requests a new pplan based on available capabilities and a defined task - + - A plugin that provide execution plan requesting capability + A plugin that provide pose prompting capability + + + + + A plugin that provide text prompting capability From 10c58097810ad1457fbd81c5917b9d1e8c15e986 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Sat, 15 Feb 2025 11:44:42 +1100 Subject: [PATCH 10/17] minor update to xml commands --- capabilities2_fabric/plans/default.xml | 8 ++++---- capabilities2_fabric/plans/prompt_1.xml | 2 +- capabilities2_fabric/plans/prompt_2.xml | 2 +- capabilities2_fabric/plans/prompt_3.xml | 2 +- capabilities2_fabric/plans/prompt_4.xml | 4 ++-- capabilities2_fabric/readme.md | 6 +++--- .../prompt_service_runner.hpp | 12 +----------- 7 files changed, 13 insertions(+), 23 deletions(-) diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index 12afddf..3a9d4b5 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -2,12 +2,12 @@ - + - + - - + + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_1.xml b/capabilities2_fabric/plans/prompt_1.xml index c56b94f..8508f12 100644 --- a/capabilities2_fabric/plans/prompt_1.xml +++ b/capabilities2_fabric/plans/prompt_1.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_2.xml b/capabilities2_fabric/plans/prompt_2.xml index e32c29a..1f33259 100644 --- a/capabilities2_fabric/plans/prompt_2.xml +++ b/capabilities2_fabric/plans/prompt_2.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index 861b021..6598ab7 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/prompt_4.xml b/capabilities2_fabric/plans/prompt_4.xml index cce166e..b95119b 100644 --- a/capabilities2_fabric/plans/prompt_4.xml +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -2,8 +2,8 @@ - - + + \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index e7963a9..2e53634 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -44,14 +44,14 @@ Below is an example XML plan for configuring a set of capabilities: - + - + - + diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 1e93f12..850a9dd 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -48,21 +48,11 @@ class PromptServiceRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters, int id) override { - const char* modelChar; - std::string model; - - parameters->QueryStringAttribute("model", &modelChar); - - if (modelChar) - model = modelChar; - else - model = "llama3.1:8b"; - prompt_msgs::srv::Prompt::Request request; prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; - modelOption1.value = model; + modelOption1.value = "llama3.1:8b"; request.prompt.options.push_back(modelOption1); From de0ac6c9267beb780e15346e3ca8bf62640e848b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 17 Feb 2025 15:04:59 +1100 Subject: [PATCH 11/17] successfully tested prompt_2.xml --- capabilities2_fabric/config/fabric.yaml | 4 ++-- .../prompt_capability_runner.hpp | 2 +- .../prompt_occupancy_runner.hpp | 4 +++- .../capabilities2_runner_prompt/prompt_plan_runner.hpp | 4 +++- .../capabilities2_runner_prompt/prompt_pose_runner.hpp | 9 ++++++--- .../prompt_service_runner.hpp | 8 +++----- .../capabilities2_runner_prompt/prompt_text_runner.hpp | 4 +++- 7 files changed, 21 insertions(+), 14 deletions(-) diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index c6a2b99..9ee5bb5 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -2,8 +2,8 @@ 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/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_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp index 65b71d2..d84e545 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp @@ -27,7 +27,7 @@ class PromptCapabilityRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 52eb134..fe6fa29 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -26,7 +26,7 @@ class PromptOccupancyRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); @@ -39,6 +39,8 @@ class PromptOccupancyRunner : public PromptServiceRunner "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + data; + info_("prompting with : " + prompt, id); + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index 40a15cd..d39f269 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -26,7 +26,7 @@ class PromptPlanRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { bool replan; const char* task; @@ -56,6 +56,8 @@ class PromptPlanRunner : public PromptServiceRunner std::string(failedElements->GetText()) + "Recorrect them as well"; } + info_("prompting with : " + prompt, id); + return prompt; } diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 58c3a17..9ac97bf 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -26,7 +26,7 @@ class PromptPoseRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); @@ -36,8 +36,11 @@ class PromptPoseRunner : public PromptServiceRunner std::string data(printer.CStr()); std::string prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " - "the content are " + - data; + "the content are " + + data; + + info_("prompting with : " + prompt, id); + return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 850a9dd..44d7bb8 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -52,7 +52,7 @@ class PromptServiceRunner : public ServiceRunner prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; - modelOption1.value = "llama3.1:8b"; + modelOption1.value = "deepseek-r1:32b"; request.prompt.options.push_back(modelOption1); @@ -63,9 +63,7 @@ class PromptServiceRunner : public ServiceRunner request.prompt.options.push_back(modelOption2); - request.prompt.prompt = generate_prompt(parameters); - - info_("prompting with : " + request.prompt.prompt, id); + request.prompt.prompt = generate_prompt(parameters, id); return request; } @@ -76,7 +74,7 @@ class PromptServiceRunner : public ServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) = 0; + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) = 0; virtual void process_response(typename prompt_msgs::srv::Prompt::Response::SharedPtr response, int id) { diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 318d797..970ff3a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -26,7 +26,7 @@ class PromptTextRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters) + virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) { tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); @@ -37,6 +37,8 @@ class PromptTextRunner : public PromptServiceRunner std::string prompt = "The response was " + data; + info_("prompting with : " + prompt, id); + return prompt; } }; From dbf7637f18c5330ceb143be42b294b31a409659b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 18 Feb 2025 18:19:19 +1100 Subject: [PATCH 12/17] modified topic runner --- capabilities2_fabric/config/fabric.yaml | 4 +- .../capabilities2_runner/topic_runner.hpp | 22 ++- .../occupancygrid_runner.hpp | 138 +++++------------- .../robotpose_runner.hpp | 58 ++------ 4 files changed, 72 insertions(+), 150 deletions(-) diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index 9ee5bb5..260f935 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,8 +1,8 @@ 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_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 diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 868c401..1d28990 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -40,7 +40,7 @@ class TopicRunner : public RunnerBase // create an service client subscription_ = node_->create_subscription( - topic_name, 1, [this](const typename TopicT::SharedPtr msg) { this->callback(msg); }); + topic_name, 10, [this](const typename TopicT::SharedPtr msg) { this->callback(msg); }); } /** @@ -65,6 +65,15 @@ class TopicRunner : public RunnerBase triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); } + std::unique_lock lock(mutex_); + completed_ = false; + + info_("Waiting for Message.", id); + + // Conditional wait + cv_.wait(lock, [this] { return completed_; }); + info_("Message Received.", id); + if (latest_message_) { // trigger the events related to on_success state @@ -76,7 +85,7 @@ class TopicRunner : public RunnerBase } else { - error_("get result call failed"); + error_("Message receving failed."); // trigger the events related to on_failure state if (events[execute_id].on_failure != "") @@ -85,6 +94,8 @@ class TopicRunner : public RunnerBase triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); } } + + info_("Thread closing.", id); } /** @@ -122,13 +133,16 @@ class TopicRunner : public RunnerBase * * @param msg message parameter */ - void callback(const typename TopicT::SharedPtr& msg) const + void callback(const typename TopicT::SharedPtr& msg) { latest_message_ = msg; + + completed_ = true; + cv_.notify_all(); } typename rclcpp::Subscription::SharedPtr subscription_; - mutable typename TopicT::SharedPtr latest_message_{ nullptr }; + mutable typename TopicT::SharedPtr latest_message_; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index 9e658e1..92d17a7 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -32,7 +32,7 @@ class OccupancyGridRunner : public TopicRunner virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, std::function print) override { - init_subscriber(node, run_config, "map", print); + init_subscriber(node, run_config, "/map", print); } protected: @@ -42,38 +42,19 @@ class OccupancyGridRunner : public TopicRunner * This function is used to inject new data into the XMLElement containing * parameters related to the on_success trigger event * - * - *
- * - * 1700000000 - * 123456789 - * - * map - *
- * - * 0.05 - * 100 - * 100 - * - * - * 1.0 - * 2.0 - * 0.0 - * - * - * 0.0 - * 0.0 - * 0.0 - * 1.0 - * - * - * - * 1700000000 - * 987654321 - * - * - * 0 1 2 3 4 5 6 7 8 9 - * + +
+ +
+ + + + + + + + 0 100 -1 50 25 75 0 0 100 50 + * * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters @@ -86,102 +67,61 @@ class OccupancyGridRunner : public TopicRunner tinyxml2::XMLElement* occupancyGridElement = element->GetDocument()->NewElement("OccupancyGrid"); element->InsertEndChild(occupancyGridElement); - // Header element + // Header element with attributes tinyxml2::XMLElement* headerElement = element->GetDocument()->NewElement("header"); - occupancyGridElement->InsertEndChild(headerElement); + headerElement->SetAttribute("frame_id", latest_message_->header.frame_id.c_str()); tinyxml2::XMLElement* stampElement = element->GetDocument()->NewElement("stamp"); + stampElement->SetAttribute("secs", latest_message_->header.stamp.sec); + stampElement->SetAttribute("nsecs", latest_message_->header.stamp.nanosec); headerElement->InsertEndChild(stampElement); - tinyxml2::XMLElement* secsElement = element->GetDocument()->NewElement("secs"); - secsElement->SetText(latest_message_->header.stamp.sec); - stampElement->InsertEndChild(secsElement); - - tinyxml2::XMLElement* nsecsElement = element->GetDocument()->NewElement("nsecs"); - nsecsElement->SetText(latest_message_->header.stamp.nanosec); - stampElement->InsertEndChild(nsecsElement); - - tinyxml2::XMLElement* frameIdElement = element->GetDocument()->NewElement("frame_id"); - frameIdElement->SetText(latest_message_->header.frame_id.c_str()); - headerElement->InsertEndChild(frameIdElement); + occupancyGridElement->InsertEndChild(headerElement); - // Info element + // Info element with attributes tinyxml2::XMLElement* infoElement = element->GetDocument()->NewElement("info"); - occupancyGridElement->InsertEndChild(infoElement); - - tinyxml2::XMLElement* resolutionElement = element->GetDocument()->NewElement("resolution"); - resolutionElement->SetText(latest_message_->info.resolution); - infoElement->InsertEndChild(resolutionElement); + infoElement->SetAttribute("resolution", latest_message_->info.resolution); + infoElement->SetAttribute("width", latest_message_->info.width); + infoElement->SetAttribute("height", latest_message_->info.height); - tinyxml2::XMLElement* widthElement = element->GetDocument()->NewElement("width"); - widthElement->SetText(latest_message_->info.width); - infoElement->InsertEndChild(widthElement); - - tinyxml2::XMLElement* heightElement = element->GetDocument()->NewElement("height"); - heightElement->SetText(latest_message_->info.height); - infoElement->InsertEndChild(heightElement); - - // Origin element + // Origin element with position and orientation attributes tinyxml2::XMLElement* originElement = element->GetDocument()->NewElement("origin"); - infoElement->InsertEndChild(originElement); tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); + positionElement->SetAttribute("x", latest_message_->info.origin.position.x); + positionElement->SetAttribute("y", latest_message_->info.origin.position.y); + positionElement->SetAttribute("z", latest_message_->info.origin.position.z); originElement->InsertEndChild(positionElement); - tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); - posX->SetText(latest_message_->info.origin.position.x); - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); - posY->SetText(latest_message_->info.origin.position.y); - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); - posZ->SetText(latest_message_->info.origin.position.z); - positionElement->InsertEndChild(posZ); - tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); + orientationElement->SetAttribute("x", latest_message_->info.origin.orientation.x); + orientationElement->SetAttribute("y", latest_message_->info.origin.orientation.y); + orientationElement->SetAttribute("z", latest_message_->info.origin.orientation.z); + orientationElement->SetAttribute("w", latest_message_->info.origin.orientation.w); originElement->InsertEndChild(orientationElement); - tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); - oriX->SetText(latest_message_->info.origin.orientation.x); - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); - oriY->SetText(latest_message_->info.origin.orientation.y); - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); - oriZ->SetText(latest_message_->info.origin.orientation.z); - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); - oriW->SetText(latest_message_->info.origin.orientation.w); - orientationElement->InsertEndChild(oriW); + infoElement->InsertEndChild(originElement); - // Map load time + // Map load time with attributes tinyxml2::XMLElement* mapLoadTimeElement = element->GetDocument()->NewElement("map_load_time"); + mapLoadTimeElement->SetAttribute("secs", latest_message_->info.map_load_time.sec); + mapLoadTimeElement->SetAttribute("nsecs", latest_message_->info.map_load_time.nanosec); infoElement->InsertEndChild(mapLoadTimeElement); - tinyxml2::XMLElement* mapSecsElement = element->GetDocument()->NewElement("secs"); - mapSecsElement->SetText(latest_message_->info.map_load_time.sec); - mapLoadTimeElement->InsertEndChild(mapSecsElement); - - tinyxml2::XMLElement* mapNsecsElement = element->GetDocument()->NewElement("nsecs"); - mapNsecsElement->SetText(latest_message_->info.map_load_time.nanosec); - mapLoadTimeElement->InsertEndChild(mapNsecsElement); + occupancyGridElement->InsertEndChild(infoElement); // Data element for occupancy data (converted to a string) tinyxml2::XMLElement* dataElement = element->GetDocument()->NewElement("data"); - occupancyGridElement->InsertEndChild(dataElement); std::string data_str; - for (size_t i = 0; i < latest_message_->data.size(); ++i) + for (size_t i = 0; i < latest_message_->data.size(); i++) { data_str += std::to_string(latest_message_->data[i]) + " "; } dataElement->SetText(data_str.c_str()); + occupancyGridElement->InsertEndChild(dataElement); + // Return the updated parameters element with OccupancyGrid added std::string result = convert_to_string(element); diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index ed963a3..f05f0ba 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -42,19 +42,10 @@ class RobotPoseRunner : public TopicRunner - * - * 1.23 - * 4.56 - * 7.89 - * - * - * 0.12 - * 0.34 - * 0.56 - * 0.78 - * - * + + + + * * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters @@ -67,44 +58,21 @@ class RobotPoseRunner : public TopicRunnerGetDocument()->NewElement("Pose"); element->InsertEndChild(poseElement); - // Position element + // Position element with attributes tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); + positionElement->SetAttribute("x", latest_message_->pose.pose.position.x); + positionElement->SetAttribute("y", latest_message_->pose.pose.position.y); + positionElement->SetAttribute("z", latest_message_->pose.pose.position.z); poseElement->InsertEndChild(positionElement); - // Position x, y, z - tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); - posX->SetText(latest_message_->pose.pose.position.x); // Set x position value - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); - posY->SetText(latest_message_->pose.pose.position.y); // Set y position value - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); - posZ->SetText(latest_message_->pose.pose.position.z); // Set z position value - positionElement->InsertEndChild(posZ); - - // Orientation element + // Orientation element with attributes tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); + orientationElement->SetAttribute("x", latest_message_->pose.pose.orientation.x); + orientationElement->SetAttribute("y", latest_message_->pose.pose.orientation.y); + orientationElement->SetAttribute("z", latest_message_->pose.pose.orientation.z); + orientationElement->SetAttribute("w", latest_message_->pose.pose.orientation.w); poseElement->InsertEndChild(orientationElement); - // Orientation x, y, z, w - tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); - oriX->SetText(latest_message_->pose.pose.orientation.x); // Set orientation x value - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); - oriY->SetText(latest_message_->pose.pose.orientation.y); // Set orientation y value - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); - oriZ->SetText(latest_message_->pose.pose.orientation.z); // Set orientation z value - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); - oriW->SetText(latest_message_->pose.pose.orientation.w); // Set orientation w value - orientationElement->InsertEndChild(oriW); - // Return the updated parameters element with Pose added as string std::string result = convert_to_string(element); From 932fb5188381c10761df260101c5b636c9bc0b6c Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 19 Feb 2025 02:36:27 +1100 Subject: [PATCH 13/17] added tf2 listnenr instead of subscribing to pose topic in robotpose_runner --- capabilities2_fabric/plans/default.xml | 2 +- capabilities2_fabric/plans/prompt_3.xml | 2 +- capabilities2_fabric/readme.md | 2 +- capabilities2_runner_nav2/CMakeLists.txt | 4 + .../robotpose_runner.hpp | 123 ++++++++++++++++-- 5 files changed, 119 insertions(+), 14 deletions(-) diff --git a/capabilities2_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index 3a9d4b5..a49f15b 100644 --- a/capabilities2_fabric/plans/default.xml +++ b/capabilities2_fabric/plans/default.xml @@ -5,7 +5,7 @@ - + diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index 6598ab7..fd491c3 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -1,7 +1,7 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index 2e53634..ced0802 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -47,7 +47,7 @@ Below is an example XML plan for configuring a set of capabilities: - + diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 3edafbc..2c109bd 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -15,6 +15,8 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(nav2_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) @@ -43,6 +45,8 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib nav2_msgs + tf2 + tf2_ros geometry_msgs capabilities2_msgs capabilities2_runner diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index f05f0ba..aa1347b 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -3,10 +3,14 @@ #include #include -#include +#include "geometry_msgs/msg/transform_stamped.hpp" #include +#include "tf2/exceptions.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + namespace capabilities2_runner { @@ -16,10 +20,10 @@ namespace capabilities2_runner * Capability Class to grab odometry data * */ -class RobotPoseRunner : public TopicRunner +class RobotPoseRunner : public RunnerBase { public: - RobotPoseRunner() : TopicRunner() + RobotPoseRunner() : RunnerBase() { } @@ -32,7 +36,100 @@ class RobotPoseRunner : public TopicRunner runner_publish_func) override { - init_subscriber(node, run_config, "pose", runner_publish_func); + // initialize the runner base by storing node pointer and run config + init_base(node, run_config, runner_publish_func); + + tf_buffer_ = std::make_unique(node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + /** + * @brief Trigger process to be executed. + * + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters + */ + virtual void execution(int id) override + { + const char* from; + const char* to; + + execute_id += 1; + + // if parameters are not provided then cannot proceed + if (!parameters_[id]) + throw runner_exception("cannot grab data without parameters"); + + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + info_("on_started", id, events[execute_id].on_started, EventType::STARTED); + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } + + info_("Waiting for Transformation.", id); + + parameters_[id]->QueryStringAttribute("from", &from); + parameters_[id]->QueryStringAttribute("to", &to); + + // Store frame names in variables that will be used to + // compute transformations + std::string fromFrame(from); + std::string toFrame(to); + + // Look up for the transformation between target_frame and turtle2 frames + // and send velocity commands for turtle2 to reach target_frame + try + { + transform_ = tf_buffer_->lookupTransform(toFrame, fromFrame, tf2::TimePointZero); + + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } + } + catch (const tf2::TransformException& ex) + { + info_("Could not transform " + toFrame + " to " + fromFrame + " : " + std::string(ex.what()), id); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } + } + + info_("Thread closing.", id); + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + // throw an error if the service client is null + // this can happen if the runner is not able to find the action resource + + if (!tf_listener_) + throw runner_exception("cannot stop runner subscriber that was not started"); + + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + info_("on_stopped", -1, events[execute_id].on_stopped, EventType::STOPPED); + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } } protected: @@ -60,17 +157,17 @@ class RobotPoseRunner : public TopicRunnerGetDocument()->NewElement("position"); - positionElement->SetAttribute("x", latest_message_->pose.pose.position.x); - positionElement->SetAttribute("y", latest_message_->pose.pose.position.y); - positionElement->SetAttribute("z", latest_message_->pose.pose.position.z); + positionElement->SetAttribute("x", transform_.transform.translation.x); + positionElement->SetAttribute("y", transform_.transform.translation.y); + positionElement->SetAttribute("z", transform_.transform.translation.z); poseElement->InsertEndChild(positionElement); // Orientation element with attributes tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); - orientationElement->SetAttribute("x", latest_message_->pose.pose.orientation.x); - orientationElement->SetAttribute("y", latest_message_->pose.pose.orientation.y); - orientationElement->SetAttribute("z", latest_message_->pose.pose.orientation.z); - orientationElement->SetAttribute("w", latest_message_->pose.pose.orientation.w); + orientationElement->SetAttribute("x", transform_.transform.rotation.x); + orientationElement->SetAttribute("y", transform_.transform.rotation.y); + orientationElement->SetAttribute("z", transform_.transform.rotation.z); + orientationElement->SetAttribute("w", transform_.transform.rotation.w); poseElement->InsertEndChild(orientationElement); // Return the updated parameters element with Pose added as string @@ -80,5 +177,9 @@ class RobotPoseRunner : public TopicRunner tf_listener_{ nullptr }; + std::unique_ptr tf_buffer_; + geometry_msgs::msg::TransformStamped transform_; }; } // namespace capabilities2_runner \ No newline at end of file From 89e78723b39e8ba989cda3ea5225017e821e326c Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 19 Feb 2025 16:05:23 +1100 Subject: [PATCH 14/17] tested the robotpose_runner --- capabilities2_fabric/plans/prompt_3.xml | 3 +- .../robotpose_runner.hpp | 52 ++++++++++++++----- 2 files changed, 40 insertions(+), 15 deletions(-) diff --git a/capabilities2_fabric/plans/prompt_3.xml b/capabilities2_fabric/plans/prompt_3.xml index fd491c3..6fc5494 100644 --- a/capabilities2_fabric/plans/prompt_3.xml +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -1,7 +1,8 @@ - + + \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index aa1347b..db90d11 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -39,7 +39,7 @@ class RobotPoseRunner : public RunnerBase // initialize the runner base by storing node pointer and run config init_base(node, run_config, runner_publish_func); - tf_buffer_ = std::make_unique(node->get_clock()); + tf_buffer_ = std::make_unique(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); } @@ -52,8 +52,9 @@ class RobotPoseRunner : public RunnerBase */ virtual void execution(int id) override { - const char* from; - const char* to; + const char* map; + const char* odom; + const char* robot; execute_id += 1; @@ -70,19 +71,20 @@ class RobotPoseRunner : public RunnerBase info_("Waiting for Transformation.", id); - parameters_[id]->QueryStringAttribute("from", &from); - parameters_[id]->QueryStringAttribute("to", &to); + parameters_[id]->QueryStringAttribute("map", &map); + parameters_[id]->QueryStringAttribute("odom", &odom); + parameters_[id]->QueryStringAttribute("robot", &robot); // Store frame names in variables that will be used to // compute transformations - std::string fromFrame(from); - std::string toFrame(to); + std::string mapFrame(map); + std::string odomFrame(odom); + std::string robotFrame(robot); - // Look up for the transformation between target_frame and turtle2 frames - // and send velocity commands for turtle2 to reach target_frame + // Try to use map -> robot first try { - transform_ = tf_buffer_->lookupTransform(toFrame, fromFrame, tf2::TimePointZero); + transform_ = tf_buffer_->lookupTransform(mapFrame, robotFrame, tf2::TimePointZero); // trigger the events related to on_success state if (events[execute_id].on_success != "") @@ -90,10 +92,32 @@ class RobotPoseRunner : public RunnerBase info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); } + + info_("Transformation received. Thread closing.", id); + return; } - catch (const tf2::TransformException& ex) + catch (tf2::TransformException& ex) { - info_("Could not transform " + toFrame + " to " + fromFrame + " : " + std::string(ex.what()), id); + info_("Could not transform from map to robot: " + std::string(ex.what()), id); + } + + // Fall back to odom -> robot + try + { + transform_ = tf_buffer_->lookupTransform(odomFrame, robotFrame, tf2::TimePointZero); + + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + info_("on_success", id, events[execute_id].on_success, EventType::SUCCEEDED); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } + + info_("Transformation received. Thread closing.", id); + } + catch (tf2::TransformException& ex) + { + info_("Could not transform from odom to robot: " + std::string(ex.what()), id); // trigger the events related to on_failure state if (events[execute_id].on_failure != "") @@ -101,9 +125,9 @@ class RobotPoseRunner : public RunnerBase info_("on_failure", id, events[execute_id].on_failure, EventType::FAILED); triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); } - } - info_("Thread closing.", id); + info_("Transformation not received. Thread closing.", id); + } } /** From 306153a0fd6a6b08e355fb254e3c41529bb57a32 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 25 Feb 2025 16:24:52 +1100 Subject: [PATCH 15/17] minor changes to plan generation --- capabilities2_fabric/config/fabric.yaml | 4 ++-- .../capabilities2_runner_prompt/prompt_service_runner.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index 260f935..8a94606 100644 --- a/capabilities2_fabric/config/fabric.yaml +++ b/capabilities2_fabric/config/fabric.yaml @@ -1,7 +1,7 @@ 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_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 diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 44d7bb8..9993123 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -52,7 +52,7 @@ class PromptServiceRunner : public ServiceRunner prompt_msgs::msg::ModelOption modelOption1; modelOption1.key = "model"; - modelOption1.value = "deepseek-r1:32b"; + modelOption1.value = "llama3.2"; request.prompt.options.push_back(modelOption1); From 5e83c889e78d8d92fa65c7ec77d6cb3f18d0c568 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 26 Feb 2025 00:55:09 +1100 Subject: [PATCH 16/17] updated prompt_runners to match prompt_tools modifications --- .../prompt_capability_runner.hpp | 7 +++---- .../prompt_occupancy_runner.hpp | 11 +++++------ .../prompt_plan_runner.hpp | 8 ++++---- .../prompt_pose_runner.hpp | 10 ++++------ .../prompt_service_runner.hpp | 4 ++-- .../prompt_text_runner.hpp | 7 +++---- 6 files changed, 21 insertions(+), 26 deletions(-) diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp index d84e545..4bb4673 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp @@ -27,7 +27,7 @@ class PromptCapabilityRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); @@ -36,9 +36,8 @@ class PromptCapabilityRunner : public PromptServiceRunner std::string data(printer.CStr()); - std::string prompt = "The capabilities of the robot are given as follows" + data; - - return prompt; + prompt = "The capabilities of the robot are given as follows" + data; + flush = false; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index fe6fa29..083226f 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -26,7 +26,7 @@ class PromptOccupancyRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); @@ -35,13 +35,12 @@ class PromptOccupancyRunner : public PromptServiceRunner std::string data(printer.CStr()); - std::string prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " - "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + - data; + prompt = "The OccupancyGrid of the robot shows the surrounding environment of the robot. The data " + "is given as a ros2 nav_msgs::msg::OccupancyGrid of which the content are " + + data; + flush = false; info_("prompting with : " + prompt, id); - - return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp index d39f269..65b0138 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -26,12 +26,11 @@ class PromptPlanRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { bool replan; const char* task; std::string taskString; - std::string prompt; parameters->QueryBoolAttribute("replan", &replan); parameters->QueryStringAttribute("task", &task); @@ -45,6 +44,8 @@ class PromptPlanRunner : public PromptServiceRunner { prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task of " + taskString + ". Return only the xml plan without explanations or comments"; + + flush = true; } else { @@ -54,11 +55,10 @@ class PromptPlanRunner : public PromptServiceRunner ". Just give the xml plan without explanations or comments. These XML " "elements had incompatibilities. " + std::string(failedElements->GetText()) + "Recorrect them as well"; + flush = true; } info_("prompting with : " + prompt, id); - - return prompt; } /** diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 9ac97bf..f2db096 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -26,7 +26,7 @@ class PromptPoseRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); @@ -35,13 +35,11 @@ class PromptPoseRunner : public PromptServiceRunner std::string data(printer.CStr()); - std::string prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which " - "the content are " + - data; + prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which the content " + "are " + data; + flush = false; info_("prompting with : " + prompt, id); - - return prompt; } }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index 9993123..fd138ad 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -63,7 +63,7 @@ class PromptServiceRunner : public ServiceRunner request.prompt.options.push_back(modelOption2); - request.prompt.prompt = generate_prompt(parameters, id); + generate_prompt(parameters, id, request.prompt.prompt, request.prompt.flush); return request; } @@ -74,7 +74,7 @@ class PromptServiceRunner : public ServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) = 0; + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) = 0; virtual void process_response(typename prompt_msgs::srv::Prompt::Response::SharedPtr response, int id) { diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 970ff3a..2a89ab4 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -26,7 +26,7 @@ class PromptTextRunner : public PromptServiceRunner * @param parameters tinyXML2 parameters * @return std::string */ - virtual std::string generate_prompt(tinyxml2::XMLElement* parameters, int id) + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); @@ -35,11 +35,10 @@ class PromptTextRunner : public PromptServiceRunner std::string data(printer.CStr()); - std::string prompt = "The response was " + data; + prompt = "The response was " + data; + flush = true; info_("prompting with : " + prompt, id); - - return prompt; } }; From d782d51580b0734b8fe626797d364303de6900d0 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 26 Feb 2025 22:52:29 +1100 Subject: [PATCH 17/17] minor modifications --- .../prompt_service_runner.hpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp index fd138ad..d5a7aad 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -78,7 +78,14 @@ class PromptServiceRunner : public ServiceRunner virtual void process_response(typename prompt_msgs::srv::Prompt::Response::SharedPtr response, int id) { - info_("response received : " + response->response.response, id); + if (response->response.buffered) + { + info_("information buffered", id); + } + else + { + info_("response received : " + response->response.response, id); + } } };