diff --git a/capabilities2_fabric/config/fabric.yaml b/capabilities2_fabric/config/fabric.yaml index e12c542..8a94606 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/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index e0b5f06..04c9120 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -113,6 +113,8 @@ 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 = document.Parse(goal->plan.c_str()); @@ -161,7 +163,15 @@ class CapabilitiesFabric : public rclcpp::Node */ void execution() { - process_feedback("Execution started"); + 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; @@ -200,8 +210,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 +226,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 +251,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 +261,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 +276,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 +295,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 +307,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 +319,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 +330,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 +470,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 +499,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 +525,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 +534,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 +560,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 +576,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)) { @@ -655,8 +646,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 +654,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,17 +687,14 @@ 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); + process_result("Successfully completed capabilities2 fabric", true); }); } @@ -716,14 +702,12 @@ class CapabilitiesFabric : public rclcpp::Node { 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 +738,13 @@ class CapabilitiesFabric : public rclcpp::Node if (success) { - goal_handle_->succeed(result_msg); status_->info(text, newline); + goal_handle_->succeed(result_msg); } else { - goal_handle_->abort(result_msg); status_->error(text, newline); + goal_handle_->abort(result_msg); } } @@ -783,9 +767,6 @@ class CapabilitiesFabric : public rclcpp::Node int expected_configurations_; int completed_configurations_; - /** status message string */ - std::string status; - /** Bond id */ std::string bond_id_; @@ -851,4 +832,10 @@ class CapabilitiesFabric : public rclcpp::Node /** trigger an selected capability */ TriggerCapabilityClient::SharedPtr trig_capability_client_; + + /** 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..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 @@ -12,12 +13,14 @@ #include #include +#include #include #include #include #include +#include /** * @brief Capabilities Executor File Parser @@ -29,22 +32,14 @@ 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; 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) { @@ -58,7 +53,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"); @@ -73,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"); @@ -91,44 +90,66 @@ 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); + + std::string plan; + xml_parser::convert_to_string(document, plan); - send_goal(document); + 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() { - auto goal_msg = Plan::Goal(); + while (plan_queue.size() > 0) + { + status_->info("Fabric client thread starting"); - xml_parser::convert_to_string(document_xml, goal_msg.plan); + 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(); - status = "Following plan was loaded :\n\n " + goal_msg.plan; - status_->info(status); + // load the latest plan from the queue + goal_msg.plan = plan_queue[0]; + plan_queue.pop_front(); 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) { 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"); + status_->info("Goal accepted by server, waiting for completion"); goal_handle_ = goal_handle; - fabric_state = fabric_status::RUNNING; + fabric_state = Status::RUNNING; } }; @@ -137,19 +158,19 @@ class CapabilitiesFabricClient : public rclcpp::Node switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - fabric_state = fabric_status::SUCCEEDED; + fabric_state = Status::LAUNCHED; 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; } @@ -174,31 +195,53 @@ 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 == 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::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 { @@ -211,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) @@ -220,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; - - send_goal(document); - } + plan_queue.push_back(request->plan); - 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_); + 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; @@ -244,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_; @@ -265,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 */ - fabric_status fabric_state; + 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 new file mode 100644 index 0000000..e862452 --- /dev/null +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/fabric_status.hpp @@ -0,0 +1,15 @@ +#pragma once + +namespace capabilities2 +{ +enum fabric_status +{ + IDLE, + RUNNING, + CANCELED, + ABORTED, + FAILED, + 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..fc4fba1 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/utils/xml_parser.hpp @@ -8,288 +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 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) - { - 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 ¶meters) +/** + * @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) { - if (element) - { - tinyxml2::XMLPrinter printer; - element->Accept(&printer); - parameters = printer.CStr(); - return true; - } - else - { - parameters = ""; - return false; - } + tinyxml2::XMLPrinter printer; + element->Accept(&printer); + parameters = printer.CStr(); + return true; } - - /** - * @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) + else { - tinyxml2::XMLPrinter printer; - document_xml.Print(&printer); - document_string = printer.CStr(); + 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 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; +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; - 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 = !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; + bool hasChildren = !element->NoChildren(); + bool hasSiblings = (element->NextSiblingElement() != nullptr); + bool foundInControl = xml_parser::search(control, nametag); + bool foundInEvents = xml_parser::search(events, nametag); + bool foundInProviders = xml_parser::search(providers, providertag); + bool returnValue = true; - if (typetag == "Control") + if (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_fabric/plans/default.xml b/capabilities2_fabric/plans/default.xml index 40fdfd9..a49f15b 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..8508f12 --- /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..1f33259 --- /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..6fc5494 --- /dev/null +++ b/capabilities2_fabric/plans/prompt_3.xml @@ -0,0 +1,8 @@ + + + + + + + + \ 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..b95119b --- /dev/null +++ b/capabilities2_fabric/plans/prompt_4.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/capabilities2_fabric/readme.md b/capabilities2_fabric/readme.md index f541a5a..ced0802 100644 --- a/capabilities2_fabric/readme.md +++ b/capabilities2_fabric/readme.md @@ -44,15 +44,15 @@ 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/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/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_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_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 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..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 */ @@ -570,15 +588,19 @@ class RunnerBase std::map executionThreadPool; /** - * @brief mutex and conditional variable for threadpool synchronisation. + * @brief mutex for threadpool synchronisation. + */ + std::mutex mutex_; + + /** + * @brief conditional variable for threadpool synchronisation. */ - std::mutex send_goal_mutex; - std::condition_variable send_goal_cv; + std::condition_variable cv_; /** - * @brief boolean flag for thread completion. + * @brief flag for threadpool synchronisation. */ - bool action_complete; + 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..d744cf6 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) { @@ -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 != "") @@ -102,8 +103,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 +115,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); } @@ -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/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_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/capability_get_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp new file mode 100644 index 0000000..89b03f1 --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/capability_get_runner.hpp @@ -0,0 +1,118 @@ +#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; + } + + /** + * @brief Update on_success event parameters with new data from an CapabilitySpecs message if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_success trigger event + * + * + * 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); + + // 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_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/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/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..32fd1e9 --- /dev/null +++ b/capabilities2_runner_capabilities/plugins.xml @@ -0,0 +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_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp new file mode 100644 index 0000000..39c18de --- /dev/null +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -0,0 +1,10 @@ +#include +#include +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricCompletionRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::FabricSetPlanRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 1ce3c58..2c109bd 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -15,8 +15,9 @@ 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(lifecycle_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) @@ -44,8 +45,9 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib nav2_msgs + tf2 + tf2_ros geometry_msgs - lifecycle_msgs capabilities2_msgs capabilities2_runner ) 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..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,17 +32,29 @@ 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: /** - * @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 + +
+ +
+ + + + + + + + 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 @@ -55,105 +67,66 @@ 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); + 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..db90d11 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,124 @@ 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* map; + const char* odom; + const char* robot; + + 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("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 mapFrame(map); + std::string odomFrame(odom); + std::string robotFrame(robot); + + // Try to use map -> robot first + try + { + transform_ = tf_buffer_->lookupTransform(mapFrame, 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); + return; + } + catch (tf2::TransformException& ex) + { + 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 != "") + { + 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_("Transformation not received. 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: @@ -42,7 +163,10 @@ class RobotPoseRunner : public TopicRunner + + + * * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters @@ -55,48 +179,31 @@ 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", transform_.transform.translation.x); + positionElement->SetAttribute("y", transform_.transform.translation.y); + positionElement->SetAttribute("z", transform_.transform.translation.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", 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); - // 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); + output_("on_success trigger parameter", result); + return result; }; + + std::shared_ptr tf_listener_{ nullptr }; + std::unique_ptr tf_buffer_; + geometry_msgs::msg::TransformStamped transform_; }; } // namespace capabilities2_runner \ No newline at end of file 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 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_capability_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp new file mode 100644 index 0000000..4bb4673 --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_capability_runner.hpp @@ -0,0 +1,44 @@ +#pragma once +#include +#include +#include +#include + +namespace capabilities2_runner +{ + +/** + * @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 PromptCapabilityRunner : public PromptServiceRunner +{ +public: + PromptCapabilityRunner() : PromptServiceRunner() + { + } + + /** + * @brief generate the prompt used for prompting the capabilities. + * + * @param parameters tinyXML2 parameters + * @return std::string + */ + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override + { + tinyxml2::XMLElement* capabilitySpecsElement = parameters->FirstChildElement("CapabilitySpecs"); + + tinyxml2::XMLPrinter printer; + capabilitySpecsElement->Accept(&printer); + + std::string data(printer.CStr()); + + prompt = "The capabilities of the robot are given as follows" + data; + flush = false; + } +}; + +} // 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..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 @@ -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 * 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 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 void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); @@ -54,26 +35,12 @@ 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 occupancy_grid 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); + 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; - return request; + info_("prompting with : " + prompt, id); } }; 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_request_runner.hpp deleted file mode 100644 index 62935ac..0000000 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ /dev/null @@ -1,116 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include - -namespace capabilities2_runner -{ -/** - * @brief prompt capability 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 - * capabilitie plans values - */ -class PromptPlanRequestRunner : public ServiceRunner -{ -public: - PromptPlanRequestRunner() : 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", 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 replan; - parameters->QueryBoolAttribute("replan", &replan); - - 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"; - } - 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. " + - 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; - } - - /** - * @brief Update on_success event parameters with new data 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 - * - * @param parameters pointer to the XMLElement containing parameters - * @return pointer to the XMLElement containing updated parameters - */ - virtual std::string update_on_success(std::string& parameters) - { - tinyxml2::XMLElement* element = convert_to_xml(parameters); - - std::string document_string = response_->response.response; - - // Create the plan element as a child of the existing parameters element - tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("ReceievdPlan"); - element->InsertEndChild(textElement); - textElement->SetText(document_string.c_str()); - - // Return the updated parameters element with Pose added - std::string result = convert_to_string(element); - - return result; - }; -}; - -} // namespace capabilities2_runner 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_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp new file mode 100644 index 0000000..65b0138 --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_runner.hpp @@ -0,0 +1,93 @@ +#pragma once +#include +#include +#include +#include + +namespace capabilities2_runner +{ +/** + * @brief prompt capability 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 + * capabilitie plans values + */ +class PromptPlanRunner : public PromptServiceRunner +{ +public: + PromptPlanRunner() : PromptServiceRunner() + { + } + + /** + * @brief generate the prompt used for prompting the occupancy grids. + * + * @param parameters tinyXML2 parameters + * @return std::string + */ + virtual void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override + { + bool replan; + const char* task; + std::string taskString; + + parameters->QueryBoolAttribute("replan", &replan); + parameters->QueryStringAttribute("task", &task); + + if (task) + taskString = task; + else + taskString = ""; + + if (!replan) + { + 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 + { + tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); + + 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"; + flush = true; + } + + info_("prompting with : " + prompt, id); + } + + /** + * @brief Update on_success event parameters with new data 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 + * + * @param parameters pointer to the XMLElement containing parameters + * @return pointer to the XMLElement containing updated parameters + */ + virtual std::string update_on_success(std::string& parameters) + { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + + std::string document_string = response_->response.response; + + // Create the plan element as a child of the existing parameters element + tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("ReceievdPlan"); + element->InsertEndChild(textElement); + textElement->SetText(document_string.c_str()); + + // Return the updated parameters element with Pose added + std::string result = convert_to_string(element); + + return result; + }; +}; + +} // namespace capabilities2_runner 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..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 @@ -2,50 +2,31 @@ #include #include #include -#include -#include -#include +#include 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 +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 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 void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); @@ -54,25 +35,11 @@ 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 message 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); + prompt = "The position of the robot is given as a standard ros2 geometry_msgs::msg::Pose of which the content " + "are " + data; + flush = false; - return request; + info_("prompting with : " + prompt, id); } }; 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..d5a7aad --- /dev/null +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_service_runner.hpp @@ -0,0 +1,92 @@ +#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_bridge/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 + { + prompt_msgs::srv::Prompt::Request request; + + prompt_msgs::msg::ModelOption modelOption1; + modelOption1.key = "model"; + modelOption1.value = "llama3.2"; + + 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); + + generate_prompt(parameters, id, request.prompt.prompt, request.prompt.flush); + + return request; + } + + /** + * @brief generate the prompt used for prompting the data. + * + * @param parameters tinyXML2 parameters + * @return std::string + */ + 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) + { + if (response->response.buffered) + { + info_("information buffered", id); + } + else + { + 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..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 @@ -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 void generate_prompt(tinyxml2::XMLElement* parameters, int id, std::string& prompt, bool& flush) override { tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); @@ -54,24 +35,10 @@ 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); + prompt = "The response was " + data; + flush = true; - return request; + info_("prompting with : " + prompt, id); } }; 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 diff --git a/capabilities2_runner_prompt/src/prompt_runners.cpp b/capabilities2_runner_prompt/src/prompt_runners.cpp index 361733f..1ebcd4d 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 +#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::PromptPlanResponseRunner, capabilities2_runner::RunnerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptPlanRunner, capabilities2_runner::RunnerBase) +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::PromptCapabilityRunner, capabilities2_runner::RunnerBase) \ No newline at end of file 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); } 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