diff --git a/.devcontainer/humble/Dockerfile b/.devcontainer/humble/Dockerfile new file mode 100644 index 0000000..a71f968 --- /dev/null +++ b/.devcontainer/humble/Dockerfile @@ -0,0 +1,30 @@ +FROM ros:humble + +# Add vscode user with same UID and GID as your host system +# (modified from https://code.visualstudio.com/remote/advancedcontainers/add-nonroot-user#_creating-a-nonroot-user) +# ubuntu now has a 1000 user ubuntu +ARG USERNAME=ubuntu +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME + +RUN apt-get update && apt-get upgrade -y \ + && apt-get install -y \ + sudo \ + git \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ + && chmod 0440 /etc/sudoers.d/$USERNAME + +# Switch from root to user +USER $USERNAME + +# Rosdep update +RUN rosdep update + +# Source the ROS setup file +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc + +# make sure folders exist +RUN mkdir -p ~/colcon_ws/src diff --git a/.devcontainer/devcontainer.json b/.devcontainer/humble/devcontainer.json similarity index 95% rename from .devcontainer/devcontainer.json rename to .devcontainer/humble/devcontainer.json index 9148ff5..02e20ce 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/humble/devcontainer.json @@ -1,5 +1,5 @@ { - "name": "jazzy base", + "name": "humble base", "dockerFile": "Dockerfile", "remoteUser": "ubuntu", "containerEnv": { diff --git a/.devcontainer/Dockerfile b/.devcontainer/jazzy/Dockerfile similarity index 100% rename from .devcontainer/Dockerfile rename to .devcontainer/jazzy/Dockerfile diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json new file mode 100644 index 0000000..e1f8dd5 --- /dev/null +++ b/.devcontainer/jazzy/devcontainer.json @@ -0,0 +1,17 @@ +{ + "name": "jazzy base", + "dockerFile": "Dockerfile", + "remoteUser": "ubuntu", + "containerEnv": { + "DISPLAY": "${localEnv:DISPLAY}" + }, + "runArgs": [ + "--privileged", + "--network=host" + ], + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/colcon_ws/${localWorkspaceFolderBasename},type=bind", + "workspaceFolder": "/home/ubuntu/colcon_ws", + "mounts": [ + "source=${localEnv:HOME}${localEnv:USERPROFILE}/.bash_history,target=/home/ubuntu/.bash_history,type=bind" + ] +} diff --git a/.github/workflows/amd64_jazzy.yml b/.github/workflows/amd64_jazzy.yml new file mode 100644 index 0000000..17a2287 --- /dev/null +++ b/.github/workflows/amd64_jazzy.yml @@ -0,0 +1,70 @@ +name: ROS Jazzy Capabilities2 + +on: + workflow_dispatch: + + push: + branches: + - capabilities2-server-fabric + paths-ignore: + - README.md + + pull_request: + branches: + - capabilities2-server-fabric + paths-ignore: + - README.md + +env: + REGISTRY: ghcr.io + OWNER: collaborativeroboticslab + IMAGE_NAME: capabilities2 + + +# https://docs.github.com/en/actions/using-jobs/using-concurrency +concurrency: + # only cancel in-progress jobs or runs for the current workflow - matches against branch & tags + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + + build-and-push-image: + runs-on: ubuntu-latest + + permissions: + contents: write + packages: write + + steps: + - name: Remove unnecessary files + run: | + sudo rm -rf /usr/share/dotnet + sudo rm -rf "$AGENT_TOOLSDIRECTORY" + + - name: Check out the repository + uses: actions/checkout@v4 + + - name: Set up QEMU + uses: docker/setup-qemu-action@v3 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Log in to the Container registry + run: | + docker login --username ${{ env.OWNER }} --password ${{ secrets.GH_PAT }} ghcr.io + + - name: Build and Push + uses: docker/build-push-action@v5 + with: + context: . + file: docker/Dockerfile + platforms: linux/amd64 + push: true + tags: | + ${{ env.REGISTRY }}/${{ env.OWNER }}/${{ env.IMAGE_NAME }}:latest + ${{ env.REGISTRY }}/${{ env.OWNER }}/${{ env.IMAGE_NAME }}:jazzy + build-args: | + GITHUB_USERNAME=${{ github.actor }} + GITHUB_PAT=${{ secrets.GH_PAT }} \ No newline at end of file diff --git a/AUTHORS b/AUTHORS index 848e7ac..c86be4e 100644 --- a/AUTHORS +++ b/AUTHORS @@ -1,4 +1,4 @@ Michael Pritchard Thomas Muller-dardelin -Kalana Rathnayake +Kalana Ratnayake Buddhi Gamage diff --git a/TODO.md b/TODO.md index 0a7e405..0808968 100644 --- a/TODO.md +++ b/TODO.md @@ -4,14 +4,16 @@ - [x] names need to be in package/name format everywhere - [x] better docs -- [ ] BUG: escape db function variables +- [x] BUG: handle "'" in db queries +- [x] BUG: escape db function variables - [ ] close or change communication to launch proxy so that it can't be accessed from ros network -- [ ] BUG: handle "'" in db queries +- [ ] BUG: fix issue with connecting to services and actions started using launch proxy + ## Features +- [x] try using ros package to find exports automatically +- [x] improve the event system - [ ] implement provider definition handling in runner -- [ ] try using ros package to find exports automatically -- [ ] improve the event system - [ ] move to established db handler lib - [ ] better bt runner impl diff --git a/capabilities2/package.xml b/capabilities2/package.xml index 691851b..a012c4d 100644 --- a/capabilities2/package.xml +++ b/capabilities2/package.xml @@ -8,6 +8,7 @@ Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard diff --git a/capabilities2_executor/CMakeLists.txt b/capabilities2_executor/CMakeLists.txt deleted file mode 100644 index ccec815..0000000 --- a/capabilities2_executor/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(capabilities2_executor) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(capabilities2_msgs REQUIRED) - -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) - -include_directories( - include - ${TINYXML2_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} SHARED - src/capabilities_executor.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - capabilities2_msgs - TINYXML2 -) - -add_library(${PROJECT_NAME}_file SHARED - src/capabilities_file_parser.cpp -) - -target_link_libraries(${PROJECT_NAME}_file - ${TINYXML2_LIBRARIES} -) - -ament_target_dependencies(${PROJECT_NAME}_file - rclcpp - rclcpp_action - capabilities2_msgs - TINYXML2 -) - -install(DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/capabilities2_executor/config/.gitkeep b/capabilities2_executor/config/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp deleted file mode 100644 index 5624987..0000000 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_executor.hpp +++ /dev/null @@ -1,531 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include -#include - -#include -#include -#include -#include - -/** - * @brief Capabilities Executor - * - * Capabilities executor node that provides a ROS client for the capabilities server. - * Able to receive a XML file that implements a plan via action server that it exposes - * or via a file read. - * - */ - -class CapabilitiesExecutor : public rclcpp::Node -{ -public: - CapabilitiesExecutor(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : Node("Capabilities2_Executor", options) - { - control_tag_list.push_back("sequential"); - control_tag_list.push_back("parallel"); - control_tag_list.push_back("recovery"); - - this->planner_server_ = rclcpp_action::create_server( - this, "~/capabilities", - std::bind(&CapabilitiesExecutor::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&CapabilitiesExecutor::handle_cancel, this, std::placeholders::_1), - std::bind(&CapabilitiesExecutor::handle_accepted, this, std::placeholders::_1)); - - this->client_capabilities_ = rclcpp_action::create_client(this, "~/capabilities_fabric"); - - get_interfaces_client_ = this->create_client("~/get_interfaces"); - get_sem_interf_client_ = this->create_client("~/get_semantic_interfaces"); - - establish_bond_client_ = this->create_client("~/establish_bond"); - } - -private: - /** - * @brief Handle the goal request that comes in from client. returns whether goal is accepted or rejected - * - * - * @param uuid uuid of the goal - * @param goal pointer to the action goal message - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received the goal request with the plan"); - (void)uuid; - - // try to parse the std::string plan from capabilities_msgs/Plan to the to a XMLDocument file - tinyxml2::XMLError xml_status = document.Parse(goal->plan.c_str()); - - // check if the file parsing failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Parsing the plan from goal message failed"); - return rclcpp_action::GoalResponse::REJECT; - } - - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - /** - * @brief Handle the goal cancel request that comes in from client. - * - * @param goal_handle pointer to the action goal handle - * @return rclcpp_action::GoalResponse - */ - rclcpp_action::CancelResponse - handle_cancel(const std::shared_ptr> goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received the request to cancel the plan"); - (void)goal_handle; - - return rclcpp_action::CancelResponse::ACCEPT; - } - - /** - * @brief Handle the goal accept event originating from handle_goal. - * - * @param goal_handle pointer to the action goal handle - */ - void - handle_accepted(const std::shared_ptr> goal_handle) - { - execution_thread = std::make_unique( - std::bind(&CapabilitiesExecutor::execute_plan, this, std::placeholders::_1), goal_handle); - } - - /** - * @brief request the interfaces, semantic_interfaces and providers from the capabilities2 server - * - * @return `true` if interface retreival is successful,`false` otherwise - */ - bool request_information() - { - // create request messages - auto request_interface = std::make_shared(); - auto request_sematic = std::make_shared(); - auto request_providers = std::make_shared(); - - // request data from the server - auto result_future = get_interfaces_client_->async_send_request(request_interface); - - RCLCPP_INFO(this->get_logger(), "Requesting Interface information from Capabilities2 Server"); - - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to receive Interface information from Capabilities2 Server"); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Received Interface information from Capabilities2 Server"); - - // request semantic interfaces available for each and every interface got from the server - for (const auto& interface : result_future.get()->interfaces) - { - request_sematic->interface = interface; - - // request semantic interface from the server - auto result_semantic_future = get_sem_interf_client_->async_send_request(request_sematic); - - RCLCPP_INFO(this->get_logger(), "Requesting Semantic Interface information from Capabilities2 Server for %s", - interface.c_str()); - - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_semantic_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), - "Failed to receive Semantic Interface information from Capabilities2 Server for %s", - interface.c_str()); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Received Semantic Interface information from Capabilities2 Server for %s", - interface.c_str()); - - // add sematic interfaces to the list if available - if (result_semantic_future.get()->semantic_interfaces.size() > 0) - { - for (const auto& semantic_interface : result_semantic_future.get()->semantic_interfaces) - { - interface_list.push_back(semantic_interface); - - // request providers of the semantic interface - request_providers->interface = semantic_interface; - request_providers->include_semantic = true; - - auto result_providers_future = get_providers_client_->async_send_request(request_providers); - - RCLCPP_INFO(this->get_logger(), - "Requesting Providers information from Capabilities2 Server for semantic interface %s", - semantic_interface.c_str()); - - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", - semantic_interface.c_str()); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", - semantic_interface.c_str()); - - // add defualt provider to the list - providers_list.push_back(result_providers_future.get()->default_provider); - - // add additional providers to the list if available - if (result_providers_future.get()->providers.size() > 0) - { - for (const auto& provider : result_providers_future.get()->providers) - { - providers_list.push_back(provider); - } - } - } - } - // if no semantic interfaces are availble for a given interface, add the interface instead - else - { - interface_list.push_back(interface); - - // request providers of the semantic interface - request_providers->interface = interface; - request_providers->include_semantic = false; - - auto result_providers_future = get_providers_client_->async_send_request(request_providers); - - RCLCPP_INFO(this->get_logger(), - "Requesting Providers information from Capabilities2 Server for semantic interface %s", - interface.c_str()); - - // wait until data is received - if (rclcpp::spin_until_future_complete(shared_from_this(), result_providers_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to receive Providers information from Capabilities2 Server for %s", - interface.c_str()); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Received Providers information from Capabilities2 Server for %s", - interface.c_str()); - - providers_list.push_back(result_providers_future.get()->default_provider); - - // add sematic interfaces to the list if available - if (result_providers_future.get()->providers.size() > 0) - { - for (const auto& provider : result_providers_future.get()->providers) - { - providers_list.push_back(provider); - } - } - } - } - - return true; - } - - /** - * @brief verify the plan using received interfaces - * - * @return `true` if interface retreival is successful,`false` otherwise - */ - bool verify_plan() - { - // intialize a vector to accomodate elements from both - std::vector tag_list(interface_list.size() + control_tag_list.size()); - std::merge(interface_list.begin(), interface_list.end(), control_tag_list.begin(), control_tag_list.end(), - tag_list.begin()); - - // verify whether document got 'plan' tags - if (!capabilities2_xml_parser::check_plan_tag(document)) - { - RCLCPP_INFO(this->get_logger(), "Execution plan is not compatible. Please recheck and update"); - return false; - } - - // extract the components within the 'plan' tags - tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - - // verify whether the plan is valid - if (!capabilities2_xml_parser::check_tags(plan, interface_list, providers_list, control_tag_list)) - { - RCLCPP_INFO(this->get_logger(), "Execution plan is faulty. Please recheck and update"); - return false; - } - - return true; - } - - /** - * @brief establish the bond with capabilities2 server - * - * @return `true` if bond establishing is successful,`false` otherwise - */ - bool establish_bond() - { - // create bond establishing server request - auto request_bond = std::make_shared(); - - // send the request - auto result_future = establish_bond_client_->async_send_request(request_bond); - - RCLCPP_INFO(this->get_logger(), "Requesting to establish bond with Capabilities2 Server"); - - // wait for the result - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Failed to establish bond with Capabilities2 Server"); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Established bond with Capabilities2 Server"); - - bond_id = result_future.get()->bond_id; - - return true; - } - - /** - * @brief execute the plan - * - * @param server_goal_handle goal handle of the server - */ - void execute_plan( - const std::shared_ptr> server_goal_handle) - { - auto result = std::make_shared(); - - // verify the plan - if (!request_information()) - { - RCLCPP_INFO(this->get_logger(), "Interface retreival failed"); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - // verify the plan - if (!verify_plan()) - { - RCLCPP_INFO(this->get_logger(), "Plan verification failed"); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - // extract the plan from the XMLDocument - tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - - // Extract the connections from the plan - capabilities2_xml_parser::extract_connections(plan, connection_list); - - // estasblish the bond with the server - if (!establish_bond()) - { - RCLCPP_INFO(this->get_logger(), "Establishing bond failed"); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - - auto connection_goal_msg = capabilities2_msgs::action::Connections::Goal(); - connection_goal_msg.bond_id = bond_id; - connection_goal_msg.header.stamp = this->get_clock()->now(); - - capabilities2_msgs::msg::CapabilityConnection connection_msg; - - for (const auto& connection : connection_list) - { - connection_msg.source.capability = connection.source_event; - connection_msg.source.provider = connection.source_provider; - connection_msg.target.capability = connection.target_event; - connection_msg.target.provider = connection.target_provider; - - if (connection.connection == capabilities2_executor::connection_type_t::ON_SUCCESS_START) - connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_SUCCESS_START; - - if (connection.connection == capabilities2_executor::connection_type_t::ON_START_START) - connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_START_START; - - if (connection.connection == capabilities2_executor::connection_type_t::ON_FAILURE_START) - connection_msg.connection_type = capabilities2_msgs::msg::CapabilityConnection::ON_FAILURE_START; - - connection_msg.target_parameters = capabilities2_xml_parser::convert_to_string(connection.event_element); - - connection_goal_msg.connections.push_back(connection_msg); - } - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // send goal options - // goal response callback - send_goal_options.goal_response_callback = - [this, server_goal_handle]( - const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - if (!goal_handle) - { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - - auto result = std::make_shared(); - - // TODO: improve with error codes - result->success = false; - server_goal_handle->canceled(result); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - } - else - { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; - - // result callback - send_goal_options.result_callback = - [this, server_goal_handle]( - const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - auto result_out = std::make_shared(); - - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - - // TODO: improve with error codes - result_out->success = false; - server_goal_handle->canceled(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - - // TODO: improve with error codes - result_out->success = false; - server_goal_handle->canceled(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - - // TODO: improve with error codes - result_out->success = false; - server_goal_handle->canceled(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); - return; - } - - if (result.result->failed_connections.size() > 0) - { - // TODO: improve with error codes - result_out->success = false; - - for (const auto& failed_connection : result.result->failed_connections) - { - RCLCPP_ERROR(this->get_logger(), "Failed Events : %s", failed_connection.target_parameters.c_str()); - - result_out->failed_elements.push_back(failed_connection.target_parameters); - } - - server_goal_handle->canceled(result_out); - - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - } - else - { - // TODO: improve with error codes - result_out->success = true; - server_goal_handle->succeed(result_out); - - RCLCPP_INFO(this->get_logger(), "Server Execution Succeeded"); - } - - rclcpp::shutdown(); - }; - - this->client_capabilities_->async_send_goal(connection_goal_msg, send_goal_options); - } - -private: - /** File Path link */ - std::string plan_file_path; - - /** flag to select loading from file or accepting via action server */ - bool read_file; - - /** Bond ID between capabilities server and this client */ - std::string bond_id; - - /** XML Document */ - tinyxml2::XMLDocument document; - - /** vector of connections */ - std::vector connection_list; - - /** Execution Thread */ - std::shared_ptr execution_thread; - - /** Interface List */ - std::vector interface_list; - - /** Providers List */ - std::vector providers_list; - - /** Control flow List */ - std::vector control_tag_list; - - /** action client for connecting with capabilities server*/ - rclcpp_action::Client::SharedPtr client_capabilities_; - - /** action server that exposes executor*/ - std::shared_ptr> planner_server_; - - /** action server goal handle*/ - std::shared_ptr> goal_handle; - - /** Get interfaces from capabilities server */ - rclcpp::Client::SharedPtr get_interfaces_client_; - - /** Get semantic interfaces from capabilities server */ - rclcpp::Client::SharedPtr get_sem_interf_client_; - - /** Get providers from capabilities server */ - rclcpp::Client::SharedPtr get_providers_client_; - - /** establish bond */ - rclcpp::Client::SharedPtr establish_bond_client_; -}; \ No newline at end of file diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp deleted file mode 100644 index 5164897..0000000 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_file_parser.hpp +++ /dev/null @@ -1,133 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -/** - * @brief Capabilities Executor File Parser - * - * Capabilities Executor File Parser node that provides a ROS client for the capabilities executor. - * Will read an XML file that implements a plan and send it to the server - */ - -class CapabilitiesFileParser : public rclcpp::Node -{ -public: - CapabilitiesFileParser(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) - : Node("Capabilities2_File_Parser", options) - { - declare_parameter("plan_file_path", "plan.xml"); - plan_file_path = get_parameter("plan_file_path").as_string(); - - this->client_ptr_ = rclcpp_action::create_client(this, "~/capabilities"); - - this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); - } - - void send_goal() - { - this->timer_->cancel(); - - // try to load the file - tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); - - // check if the file loading failed - if (xml_status != tinyxml2::XMLError::XML_SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Loading the file from path : %s failed", plan_file_path.c_str()); - rclcpp::shutdown(); - } - - if (!this->client_ptr_->wait_for_action_server()) - { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - } - - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); - goal_msg.plan = capabilities2_xml_parser::convert_to_string(document); - - RCLCPP_INFO(this->get_logger(), "Sending goal"); - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - // send goal options - // goal response callback - send_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr &goal_handle) - { - if (!goal_handle) - { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } - else - { - RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); - } - }; - - // result callback - send_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult &result) - { - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; - } - - if (result.result->success) - { - RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Plan failed to complete"); - - if (result.result->failed_elements.size() > 0) - { - RCLCPP_ERROR(this->get_logger(), "Plan failed due to incompatible XMLElements in the plan"); - - for (const auto &failed_element : result.result->failed_elements) - RCLCPP_ERROR(this->get_logger(), "Failed Elements : %s", failed_element.c_str()); - } - } - - rclcpp::shutdown(); - }; - - this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - } - -private: - /** File Path link */ - std::string plan_file_path; - - /** XML Document */ - tinyxml2::XMLDocument document; - - /** action client */ - rclcpp_action::Client::SharedPtr client_ptr_; - - /** action server */ - rclcpp::TimerBase::SharedPtr timer_; -}; \ No newline at end of file diff --git a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp deleted file mode 100644 index 102d345..0000000 --- a/capabilities2_executor/include/capabilities2_executor/capabilities_xml_parser.hpp +++ /dev/null @@ -1,265 +0,0 @@ -#include -#include -#include -#include - -namespace capabilities2_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"); - } - - /** - * @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 element to be converted - * - * @return std::string converted element - */ - std::string convert_to_string(tinyxml2::XMLElement *element) - { - tinyxml2::XMLPrinter printer; - - element->Accept(&printer); - - std::string value = printer.CStr(); - - return value; - } - - /** - * @brief convert XMLDocument to std::string - * - * @param document element to be converted - * - * @return std::string converted document - */ - std::string convert_to_string(tinyxml2::XMLDocument &document) - { - tinyxml2::XMLPrinter printer; - - document.Accept(&printer); - - std::string value = printer.CStr(); - - return value; - } - - /** - * @brief check the plan for invalid/unsupported control and event tags - * uses recursive approach to go through the plan - * - * @param element XML Element to be evaluated - * @param events_list list containing valid event tags - * @param providers_list list containing providers - * @param control_list list containing valid control tags - * - * @return `true` if element valid and supported and `false` otherwise - */ - bool check_tags(tinyxml2::XMLElement *element, - std::vector &events_list, - std::vector &providers_list, - std::vector &control_list) - { - const char **name; - const char **provider; - - element->QueryStringAttribute("name", name); - element->QueryStringAttribute("provider", provider); - - std::string typetag(element->Name()); - std::string nametag(*name); - std::string providertag(*provider); - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); - bool foundInControl = capabilities2_xml_parser::search(control_list, nametag); - bool foundInEvents = capabilities2_xml_parser::search(events_list, nametag); - bool foundInProviders = capabilities2_xml_parser::search(providers_list, providertag); - bool returnValue = true; - - if (typetag == "Control") - { - if (foundInControl and hasChildren) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->FirstChildElement(), events_list, providers_list, control_list); - - if (foundInControl and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); - - if (foundInControl and !hasSiblings) - returnValue = returnValue; - - if (!foundInControl) - return false; - } - else if (typetag == "Event") - { - if (foundInEvents and foundInProviders and hasSiblings) - returnValue = returnValue and capabilities2_xml_parser::check_tags(element->NextSiblingElement(), events_list, providers_list, control_list); - - if (foundInEvents and foundInProviders and !hasSiblings) - returnValue = returnValue; - - if (!foundInEvents) - return false; - - if (!foundInProviders) - return false; - } - else - { - return false; - } - - return returnValue; - } - - /** - * @brief Adds an event connection to the capabilities2 fabric - * - * @param source_event the source event from which the connection would start - * @param source_provider provider of the source event - * @param target_event the target event which will be connected via the connection - * @param target_provider provider of the target event - * @param connection the type of connection - * @param event_element the tinyxml2::XMLElement which contains runtime parameters - * - * @return The connection object - */ - capabilities2_executor::connection_t create_connection(std::string source_event, std::string source_provider, - std::string target_event, std::string target_provider, - capabilities2_executor::connection_type_t connection, tinyxml2::XMLElement *event_element) - { - capabilities2_executor::connection_t connect; - - connect.source_event = source_event; - connect.source_provider = source_provider; - connect.target_event = target_event; - connect.target_provider = target_provider; - connect.connection = connection; - connect.event_element = event_element; - - return connect; - } - - /** - * @brief parse through the plan and extract the connections - * - * @param element XML Element to be evaluated - * @param connection_list std::vector containing extracted connections - * @param connection the type of connection - * @param source_event source event name. if not provided, "start" is taken as default - * @param source_provider provider of the source event, if not provided "" is taken as default - * @param target_event target event name. if not provided, "stop" is taken as default - * @param target_provider provider of the target event. if not provided, "stop" is taken as default - */ - void extract_connections(tinyxml2::XMLElement *element, std::vector &connection_list, - capabilities2_executor::connection_type_t connection = capabilities2_executor::connection_type_t::ON_SUCCESS_START, - std::string source_event = "start", std::string source_provider = "", - std::string target_event = "stop", std::string target_provider = "") - { - const char **name; - const char **provider; - - element->QueryStringAttribute("name", name); - element->QueryStringAttribute("provider", provider); - - std::string typetag(element->Name()); - std::string nametag(*name); - std::string providertag(*provider); - - bool hasChildren = !element->NoChildren(); - bool hasSiblings = !capabilities2_xml_parser::isLastElement(element); - - if ((typetag == "Control") and (nametag == "sequential")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_SUCCESS_START, - source_event, source_provider, target_event, target_provider); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - source_event, source_provider, target_event, target_provider); - } - else if ((typetag == "Control") and (nametag == "parallel")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_START_START, - source_event, source_provider, target_event, target_provider); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - source_event, source_provider, target_event, target_provider); - } - else if ((typetag == "Control") and (nametag == "recovery")) - { - if (hasChildren) - capabilities2_xml_parser::extract_connections(element->FirstChildElement(), connection_list, capabilities2_executor::connection_type_t::ON_FAILURE_START, - source_event, source_provider, target_event, target_provider); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - source_event, source_provider, target_event, target_provider); - } - else if (typetag == "Event") - { - capabilities2_executor::connection_t connection_obj = create_connection(source_event, source_provider, nametag, providertag, connection, element); - connection_list.push_back(connection_obj); - - if (hasSiblings) - capabilities2_xml_parser::extract_connections(element->NextSiblingElement(), connection_list, connection, - nametag, providertag, target_event, target_provider); - } - } - -} // namespace capabilities2_xml_parser diff --git a/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp b/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp deleted file mode 100644 index 30b8a21..0000000 --- a/capabilities2_executor/include/capabilities2_executor/structs/connection.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#include -#include - -namespace capabilities2_executor -{ - enum connection_type_t - { - ON_START_START, - ON_START_STOP, - ON_SUCCESS_START, - ON_SUCCESS_STOP, - ON_FAILURE_START, - ON_FAILURE_STOP, - ON_TERMINATE_START, - ON_TERMINATE_STOP - }; - - struct connection_t { - std::string source_event; - std::string source_provider; - std::string target_event; - std::string target_provider; - connection_type_t connection; - tinyxml2::XMLElement* event_element; - }; - -} // namespace capabilities2_executor diff --git a/capabilities2_executor/launch/.gitkeep b/capabilities2_executor/launch/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_executor/readme.md b/capabilities2_executor/readme.md deleted file mode 100644 index 999186c..0000000 --- a/capabilities2_executor/readme.md +++ /dev/null @@ -1,5 +0,0 @@ -# Capabilities2 Executor - -This is a simple executor that demonstrates how to use the capabilities2 library to execute multi-skill tasks. - -## Usage diff --git a/capabilities2_executor/src/capabilities_executor.cpp b/capabilities2_executor/src/capabilities_executor.cpp deleted file mode 100644 index 98c5a43..0000000 --- a/capabilities2_executor/src/capabilities_executor.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/capabilities2_executor/src/capabilities_file_parser.cpp b/capabilities2_executor/src/capabilities_file_parser.cpp deleted file mode 100644 index 00c2410..0000000 --- a/capabilities2_executor/src/capabilities_file_parser.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/capabilities2_launch_proxy/capabilities2_launch_proxy/__init__.py b/capabilities2_launch_proxy/capabilities2_launch_proxy/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py b/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py deleted file mode 100755 index 26a5845..0000000 --- a/capabilities2_launch_proxy/capabilities2_launch_proxy/capabilities_launch_proxy.py +++ /dev/null @@ -1,287 +0,0 @@ -''' -capabilities launch proxy server - -implements a launch server responding to messages to launch and shutdown launch files -use in conjunction with capabilities2_server to launch capabilities - -acts like ros2 launch command but from an action interface -''' - -import os -import threading -from typing import Set -from typing import Text -from typing import Optional -import rclpy -from rclpy.executors import MultiThreadedExecutor -from rclpy.executors import ExternalShutdownException -import rclpy.logging -from rclpy.node import Node -from rclpy.action import ActionServer -from rclpy.action import CancelResponse -from rclpy.action.server import ServerGoalHandle -from launch import LaunchService -from launch import LaunchContext -from launch.event import Event -from launch.events.process import ShutdownProcess -from launch.event_handler import EventHandler -from launch.event_handlers import OnProcessStart -from launch.actions import ExecuteProcess -from launch.actions import EmitEvent -from launch.actions import RegisterEventHandler -from launch.launch_description_sources import AnyLaunchDescriptionSource -# from launch.some_entities_type import SomeEntitiesType -from capabilities2_msgs.action import Launch -from capabilities2_msgs.msg import CapabilityEvent - - -class CancelLaunchGoalEvent(Event): - """ - Launch goal cancel event - - used when a launch goal is canceled - the result should be that the launch description associated with a goal - is shutdown in the running LaunchService - other goals should not be affected - """ - - name = 'launch.event.CancelLaunchGoal' - - def __init__(self, *, goal_handle: ServerGoalHandle) -> None: - """ - Create a LaunchGoalCancelEvent - """ - self.goal_handle = goal_handle - - -class CancelEventHandler(EventHandler): - """ - Cancel event handler - - handle cancel launch goal events - cancels a launch goal if it matches the goal handle passed in - """ - - def __init__(self, goal_handle: ServerGoalHandle, pids: Set[int]): - super().__init__(matcher=lambda event: isinstance(event, CancelLaunchGoalEvent)) - self.goal_handle = goal_handle - self.pids = pids - - def handle(self, event: Event, context: LaunchContext): - """ - handle event - """ - - super().handle(event, context) - - # check if event is a cancel launch goal event - if not isinstance(event, CancelLaunchGoalEvent): - return None - - # if the goals match then cancel - if event.goal_handle == self.goal_handle: - return EmitEvent(event=ShutdownProcess( - process_matcher=lambda action: action.process_details['pid'] in self.pids - )) - - return None - - -class CapabilitiesLaunchProxy(Node): - """ - Capabilities Launch proxy server - """ - - def __init__(self, node_name='capabilities_launch_proxy'): - # super class init - super().__init__(node_name) - - # active files set - self.active_launch_files: Set[Text] = set() - self.launch_set_lock = threading.Lock() - - # create launch action server - self.launch_sub = ActionServer( - self, - Launch, - '~/launch', - handle_accepted_callback=self.handle_accepted_cb, - execute_callback=self.execute_cb, - cancel_callback=self.cancel_cb - ) - - # cap event pub - self.event_pub = self.create_publisher( - CapabilityEvent, - '~/events', - 10 - ) - - # create launch service - self.launch_service = LaunchService() - - # log start - self.get_logger().info('capabilities launch proxy started') - - # service interface - def get_active_launch_files_cb(self): - """ - get active launch files - """ - return self.active_launch_files - - # action interface - def handle_accepted_cb(self, goal_handle: ServerGoalHandle) -> None: - """ - handle accepted callback - """ - # start execution and detach - threading.Thread(target=goal_handle.execute).start() - - # execute callback is main launch feature - def execute_cb(self, goal_handle: ServerGoalHandle) -> Launch.Result: - """ - launch execute callback - """ - - # check if file exists - if not os.path.isfile(goal_handle.request.launch_file_path): - # file does not exist - self.get_logger().error("file does not exist: '{}'".format( - goal_handle.request.launch_file_path)) - - # abort goal - goal_handle.abort() - return Launch.Result() - - with self.launch_set_lock: - # launch context already exists guard - if goal_handle.request.launch_file_path in self.active_launch_files: - self.get_logger().error("launch already exists for source '{}'".format( - goal_handle.request.launch_file_path)) - - goal_handle.abort() - return Launch.Result() - - # add context to active files - self.active_launch_files.add(goal_handle.request.launch_file_path) - - # get launch description from file - description = AnyLaunchDescriptionSource( - goal_handle.request.launch_file_path).get_launch_description(self.launch_service.context) - - # set up process id get event handler - description_process_ids: Set[int] = set() - - # go through all actions and collect all processes (mostly nodes) in the description - for e in description.entities: - # check the type of entity is an execute process action - if isinstance(e, ExecuteProcess): - # register event handler for process start event - # on process start store the pid - description.add_action( - RegisterEventHandler(OnProcessStart( - target_action=e, - on_start=lambda event, context: description_process_ids.add( - event.pid) - )) - ) - - # add shutdown event to launch description - # add shutdown event handler to launch description - # this uses the cancel goal event handler and cancel goal event - description.add_action(RegisterEventHandler(CancelEventHandler( - goal_handle, - description_process_ids - ))) - - # add to launch service - self.launch_service.include_launch_description(description) - - # bind request to this thread - self.get_logger().info('launching: {}'.format( - goal_handle.request.launch_file_path - )) - - # sleep rate for holding this action open - rate = self.create_rate(1.0) - - # loop while not canceled - while rclpy.ok(): - # if cancelled - if goal_handle.is_cancel_requested: - self.get_logger().warn("launch canceled: '{}'".format( - goal_handle.request.launch_file_path - )) - - # shutdown context - self.launch_service.emit_event( - CancelLaunchGoalEvent(goal_handle=goal_handle)) - - # delete context from active files - with self.launch_set_lock: - self.active_launch_files.remove( - goal_handle.request.launch_file_path) - - goal_handle.canceled() - return Launch.Result() - - # send event feedback - feedback = Launch.Feedback() - feedback.event.type = 'launched' - goal_handle.publish_feedback(feedback) - - # spin indefinitely (sleep loop this thread) - rate.sleep() - - goal_handle.succeed() - return Launch.Result() - - def cancel_cb(self, goal_handle: ServerGoalHandle) -> CancelResponse: - """ - accept all cancellations - """ - return CancelResponse.ACCEPT - - -# main function -def main(): - """ - main function - """ - - try: - # init node - rclpy.init() - - # instantiate the capabilities launch server - capabilities_launch_proxy = CapabilitiesLaunchProxy() - - # spin node in new thread - executor = MultiThreadedExecutor() - executor.add_node(capabilities_launch_proxy) - executor_thread = threading.Thread(target=executor.spin) - executor_thread.start() - - # run the launch service - capabilities_launch_proxy.get_logger().info('running LaunchService') - capabilities_launch_proxy.launch_service.run( - shutdown_when_idle=False) - - # cancel the launch services - capabilities_launch_proxy.launch_service.shutdown() - - executor.shutdown() - executor_thread.join() - capabilities_launch_proxy.destroy_node() - - # rclpy.shutdown() - - except ExternalShutdownException: - pass - - -# main -if __name__ == '__main__': - # run the proxy - main() diff --git a/capabilities2_launch_proxy/package.xml b/capabilities2_launch_proxy/package.xml deleted file mode 100644 index ab70338..0000000 --- a/capabilities2_launch_proxy/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - capabilities2_launch_proxy - 0.0.1 - ros2 launch proxy to support launching capabilities similar to ros1 capabilities - mik-p - - MIT - - rclpy - launch - std_msgs - capabilities2_msgs - - - ament_python - - diff --git a/capabilities2_launch_proxy/readme.md b/capabilities2_launch_proxy/readme.md deleted file mode 100644 index e749a41..0000000 --- a/capabilities2_launch_proxy/readme.md +++ /dev/null @@ -1,46 +0,0 @@ -# capabilities2_launch_proxy - -Provides an Action API proxy to the ros2 launch framework. Implements features that are only available in python since the `launch` API is not available in C++. - -Contains the essential features to implement the ros1 [`capabilities`](https://wiki.ros.org/capabilities) package launch functionality in ros2. - -Use in conjunction with `capabilities2_server` package, to provide a full capabilities server. - -## how it works - -Implements three main functions: - -| Function | Description | -| --- | --- | -| `launch` | Run a launch file requested a runtime via a network request | -| `shutdown` | shutdown a launch file that has been started | -| `events` | send capability events during operation | - -The lifecylce of these functions are typically handled by the capabilities server but could be used for other things. - -## Use without capabilities2 server - -The proxy provides an action called `~/launch`. The `goal` is a file path to launch from. The feedback provides events about the launch status. - -### Add to a launch file - -```python -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - # create launch proxy node - Node( - package='capabilities2_launch_proxy', - executable='capabilities_launch_proxy', - name='capabilities_launch_proxy' - ) - ]) -``` - -### Run standalone - -```bash -ros2 run capabilities2_launch_proxy capabilities_launch_proxy -``` diff --git a/capabilities2_launch_proxy/resource/capabilities2_launch_proxy b/capabilities2_launch_proxy/resource/capabilities2_launch_proxy deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_launch_proxy/setup.cfg b/capabilities2_launch_proxy/setup.cfg deleted file mode 100644 index d3f4af5..0000000 --- a/capabilities2_launch_proxy/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/capabilities2_launch_proxy -[install] -install_scripts=$base/lib/capabilities2_launch_proxy diff --git a/capabilities2_launch_proxy/setup.py b/capabilities2_launch_proxy/setup.py deleted file mode 100644 index 1707673..0000000 --- a/capabilities2_launch_proxy/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'capabilities2_launch_proxy' - -setup( - name=package_name, - version='0.0.1', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='mik-p', - maintainer_email='mppritchard3@hotmail.com', - description='ros2 launch proxy to support launching capabilities similar to ros1 capabilities', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'capabilities_launch_proxy = capabilities2_launch_proxy.capabilities_launch_proxy:main', - ], - }, -) diff --git a/capabilities2_launch_proxy/test/call_use_launch_runner.py b/capabilities2_launch_proxy/test/call_use_launch_runner.py deleted file mode 100644 index f8d3f9c..0000000 --- a/capabilities2_launch_proxy/test/call_use_launch_runner.py +++ /dev/null @@ -1,94 +0,0 @@ -''' test call cap services ''' - -from bondpy import bondpy -import rclpy -from capabilities2_msgs.srv import EstablishBond -from capabilities2_msgs.srv import GetRunningCapabilities -from capabilities2_msgs.srv import UseCapability - - -def test_get_running_capabilities(n): - # get running capabilities - client = n.create_client( - GetRunningCapabilities, - '/capabilities/get_running_capabilities' - ) - - # wait for service - client.wait_for_service() - - # send request - request = GetRunningCapabilities.Request() - future = client.call_async(request) - rclpy.spin_until_future_complete(n, future) - - # print result - for s in future.result().running_capabilities: - print(s) - - -# main -if __name__ == '__main__': - - rclpy.init() - - node = rclpy.create_node('test_call_cap_srvs') - - # do tests - # test_get_interfaces_srv(node) - # test_get_semantic_interfaces_srv(node) - # test_get_providers_srv(node) - # test_get_capability_specs_srv(node) - - print("test use capability") - # get a bond - bond_cli = node.create_client( - EstablishBond, - '/capabilities/establish_bond' - ) - - # wait for service - bond_cli.wait_for_service() - - # send request - request = EstablishBond.Request() - future = bond_cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - - # print result - print(future.result()) - # keep bond id - bond_id = future.result().bond_id - - # create a use capability request - use_client = node.create_client( - UseCapability, - '/capabilities/use_capability' - ) - - # wait for service - use_client.wait_for_service() - - # send request - request = UseCapability.Request() - request.capability = 'std_capabilities/empty' - request.preferred_provider = 'std_capabilities/empty' - request.bond_id = future.result().bond_id - future = use_client.call_async(request) - rclpy.spin_until_future_complete(node, future) - - # print result - print(future.result()) - - # get running capabilities - test_get_running_capabilities(node) - - # keep bond alive - bond = bondpy.Bond(node, "/capabilities/bonds", bond_id) - bond.start() - rclpy.spin(node) - - node.destroy_node() - rclpy.shutdown() - - exit(0) diff --git a/capabilities2_msgs/CMakeLists.txt b/capabilities2_msgs/CMakeLists.txt index 669eb9c..529c0ce 100644 --- a/capabilities2_msgs/CMakeLists.txt +++ b/capabilities2_msgs/CMakeLists.txt @@ -17,14 +17,12 @@ find_package(std_msgs REQUIRED) set(msg_files "msg/Capability.msg" - "msg/CapabilityEvent.msg" "msg/CapabilitySpec.msg" "msg/Remapping.msg" "msg/RunningCapability.msg" "msg/NaturalCapability.msg" "msg/CapabilityCommand.msg" "msg/CapabilityResponse.msg" - "msg/CapabilityConnection.msg" ) set(srv_files @@ -41,13 +39,14 @@ set(srv_files "srv/StartCapability.srv" "srv/StopCapability.srv" "srv/UseCapability.srv" + "srv/ConfigureCapability.srv" + "srv/TriggerCapability.srv" + "srv/Launch.srv" ) set(action_files "action/Capability.action" "action/Launch.action" - "action/Plan.action" - "action/Connections.action" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/capabilities2_msgs/action/Connections.action b/capabilities2_msgs/action/Connections.action deleted file mode 100644 index 80c885d..0000000 --- a/capabilities2_msgs/action/Connections.action +++ /dev/null @@ -1,14 +0,0 @@ -# action to send a list of interface connections to capabilities server -# goal -std_msgs/Header header -string bond_id -CapabilityConnection[] connections ---- -# result -std_msgs/Header header -CapabilityConnection[] failed_connections ---- -# feedback -std_msgs/Header header -int32 success_count -int32 total_count diff --git a/capabilities2_msgs/action/Launch.action b/capabilities2_msgs/action/Launch.action index d4c26d4..c6e8c95 100644 --- a/capabilities2_msgs/action/Launch.action +++ b/capabilities2_msgs/action/Launch.action @@ -8,4 +8,3 @@ std_msgs/Header header --- # feedback std_msgs/Header header -capabilities2_msgs/CapabilityEvent event diff --git a/capabilities2_msgs/action/Plan.action b/capabilities2_msgs/action/Plan.action deleted file mode 100644 index ad84fa9..0000000 --- a/capabilities2_msgs/action/Plan.action +++ /dev/null @@ -1,12 +0,0 @@ -# action to transfer a plan for execution -# goal -std_msgs/Header header -string plan ---- -# result -std_msgs/Header header -bool success -string[] failed_elements ---- -# feedback -std_msgs/Header header diff --git a/capabilities2_msgs/msg/Capability.msg b/capabilities2_msgs/msg/Capability.msg index bab67a8..5bf97a1 100644 --- a/capabilities2_msgs/msg/Capability.msg +++ b/capabilities2_msgs/msg/Capability.msg @@ -1,4 +1,8 @@ # Capability string capability + # Used provider string provider + +# trigger parameters +string parameters diff --git a/capabilities2_msgs/msg/CapabilityConnection.msg b/capabilities2_msgs/msg/CapabilityConnection.msg deleted file mode 100644 index 04b538b..0000000 --- a/capabilities2_msgs/msg/CapabilityConnection.msg +++ /dev/null @@ -1,14 +0,0 @@ -# Declare types of connections -uint8 ON_START_START = 0 -uint8 ON_START_STOP = 1 -uint8 ON_SUCCESS_START = 2 -uint8 ON_SUCCESS_STOP = 3 -uint8 ON_FAILURE_START = 4 -uint8 ON_FAILURE_STOP = 5 -uint8 ON_TERMINATE_START = 6 -uint8 ON_TERMINATE_STOP = 7 - -Capability source -Capability target -uint8 connection_type -string target_parameters \ No newline at end of file diff --git a/capabilities2_msgs/msg/CapabilityEvent.msg b/capabilities2_msgs/msg/CapabilityEvent.msg deleted file mode 100644 index 1a52c94..0000000 --- a/capabilities2_msgs/msg/CapabilityEvent.msg +++ /dev/null @@ -1,16 +0,0 @@ -std_msgs/Header header -# Capability which this event pertains to -string capability -# Capability provider which this event pertains to -string provider - -# Event types -string LAUNCHED="launched" -string STOPPED="stopped" -string TERMINATED="terminated" -string SERVER_READY="server_ready" -# Event type -string type - -# PID of the related process -int32 pid diff --git a/capabilities2_msgs/package.xml b/capabilities2_msgs/package.xml index 3ac0ada..9130417 100644 --- a/capabilities2_msgs/package.xml +++ b/capabilities2_msgs/package.xml @@ -8,6 +8,7 @@ Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard diff --git a/capabilities2_msgs/srv/ConfigureCapability.srv b/capabilities2_msgs/srv/ConfigureCapability.srv new file mode 100644 index 0000000..b9e6905 --- /dev/null +++ b/capabilities2_msgs/srv/ConfigureCapability.srv @@ -0,0 +1,8 @@ +Capability source +Capability target_on_start +Capability target_on_stop +Capability target_on_success +Capability target_on_failure +string connection_description +int32 trigger_id +--- \ No newline at end of file diff --git a/capabilities2_msgs/srv/Launch.srv b/capabilities2_msgs/srv/Launch.srv new file mode 100644 index 0000000..a1938ed --- /dev/null +++ b/capabilities2_msgs/srv/Launch.srv @@ -0,0 +1,6 @@ +std_msgs/Header header +string package_name +string launch_file_name +--- +std_msgs/Header header +string status \ No newline at end of file diff --git a/capabilities2_msgs/srv/TriggerCapability.srv b/capabilities2_msgs/srv/TriggerCapability.srv new file mode 100644 index 0000000..b9160a7 --- /dev/null +++ b/capabilities2_msgs/srv/TriggerCapability.srv @@ -0,0 +1,3 @@ +string capability +string parameters +--- diff --git a/capabilities2_runner/CMakeLists.txt b/capabilities2_runner/CMakeLists.txt index 02d553d..6021b30 100644 --- a/capabilities2_runner/CMakeLists.txt +++ b/capabilities2_runner/CMakeLists.txt @@ -15,21 +15,18 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(capabilities2_msgs REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) - -include_directories(include -${TINYXML2_INCLUDE_DIRS} +find_package(capabilities2_utils REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor + +include_directories( + include ) add_library(${PROJECT_NAME} SHARED src/standard_runners.cpp - # src/launch_runner.cpp - # src/action_runner.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} ) ament_target_dependencies(${PROJECT_NAME} @@ -37,7 +34,10 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action pluginlib capabilities2_msgs - TINYXML2 + capabilities2_utils + event_logger + event_logger_msgs + TinyXML2 ) pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 4b63d53..7c768df 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -1,10 +1,12 @@ #pragma once #include +#include #include #include #include +#include #include #include #include @@ -36,60 +38,25 @@ class ActionRunner : public RunnerBase * @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 action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param on_started function pointer to trigger at the start of the action client in the runner - * @param on_terminated function pointer to trigger at the termination of the action client in the runner - * @param on_stopped function pointer to trigger at the termination of the action client by the server */ - virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) + virtual void init_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name) { // initialize the runner base by storing node pointer and run config - init_base(node, run_config, on_started, on_terminated, on_stopped); + init_base(node, run_config); // create an action client action_client_ = rclcpp_action::create_client(node_, action_name); // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); + info_("waiting for action: " + action_name); - if (!action_client_->wait_for_action_server(std::chrono::seconds(3))) + if (!action_client_->wait_for_action_server(std::chrono::seconds(1000))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action: %s", run_config_.interface.c_str(), - action_name.c_str()); + error_("failed to connect to action: " + action_name); throw runner_exception("failed to connect to action server"); } - // send goal options - // goal response callback - send_goal_options_.goal_response_callback = - [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - // publish event - if (goal_handle) - if (on_started_) - on_started_(run_config_.interface); - - // store goal handle to be used with stop funtion - goal_handle_ = goal_handle; - }; - - // result callback - send_goal_options_.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - // Do something - } - else - { - // send terminated event - if (on_terminated_) - { - on_terminated_(run_config_.interface); - } - } - }; + info_("connected with action: " + action_name); } /** @@ -115,106 +82,143 @@ class ActionRunner : public RunnerBase { try { - std::shared_future cancel_future = - action_client_->async_cancel_goal( - goal_handle_, [this](action_msgs::srv::CancelGoal_Response::SharedPtr response) { - if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) - { - // throw runner_exception("failed to cancel runner"); - } - - // publish event - if (on_stopped_) - { - on_stopped_(run_config_.interface); - } - }); + auto cancel_future = action_client_->async_cancel_goal( + goal_handle_, [this](action_msgs::srv::CancelGoal_Response::SharedPtr response) { + if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) + { + // throw runner_exception("failed to cancel runner"); + error_("Runner cancellation failed."); + } + + // Trigger on_stopped event if defined + if (events[runner_id].on_stopped.interface != "") + { + event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, + events[runner_id].on_stopped.provider); + triggerFunction_(events[runner_id].on_stopped.interface, + update_on_stopped(events[runner_id].on_stopped.parameters)); + } + }); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope // BUG: the line below does not work in jazzy build, so a workaround is used - // rclcpp::spin_until_future_complete(node_->get_node_base_interface(), cancel_future, std::chrono::seconds(2)); auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(2); while (std::chrono::steady_clock::now() < timeout) { + // Check if the cancel operation is complete if (cancel_future.wait_for(std::chrono::milliseconds(100)) == std::future_status::ready) break; } } catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) { - RCLCPP_ERROR(node_->get_logger(), "failed to cancel goal: %s", e.what()); + error_("failed to cancel goal: " + std::string(e.what())); throw runner_exception(e.what()); } } + + info_("removing event options"); + + // remove all event options for this runner instance + const auto n = events.size(); + events.clear(); + info_("removed event options for " + std::to_string(n) + " runner ids"); + + info_("runner cleaned. stopping.."); } /** - * @brief the trigger function on the action runner is used to trigger an action. - * this method provides a mechanism for injecting parameters or a goal into the action - * and then trigger the action + * @brief Trigger process to be executed. * - * @param parameters - * @return std::optional)>> + * This method utilizes paramters set via the trigger() function + * + * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) override + virtual void execution(int id) override { - // the action is being triggered out of order - if (!goal_handle_) - throw runner_exception("cannot trigger action that was not started"); - // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_[id]) throw runner_exception("cannot trigger action without parameters"); // generate a goal from parameters if provided - typename ActionT::Goal goal_msg = generate_goal(parameters); + goal_msg_ = generate_goal(parameters_[id], id); - // trigger the action client with goal - auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); + info_("goal generated for event ", id); - // spin until send future completes - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed"); - return std::nullopt; - } + std::unique_lock lock(mutex_); + completed_ = false; + + // trigger the action client with goal + send_goal_options_.goal_response_callback = + [this, id](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + if (goal_handle) + { + info_("goal accepted. Waiting for result", id); - // get result future - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle; - auto result_future = action_client_->async_get_result(goal_handle); - - // create a function to call for the result - // the future will be returned to the caller - // and the caller can provide a conversion function - // to handle the result - std::function)> result_callback = - [this, result_future](std::shared_ptr result) { - // wait for result - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) + // trigger the events related to on_started state + if (events[id].on_started.interface != "") + { + event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); + triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); + } + } + else { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - return; + error_("goal rejected", id); } - // get wrapped result - typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + // store goal handle to be used with stop funtion + goal_handle_ = goal_handle; + }; + + send_goal_options_.feedback_callback = + [this, id](typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + const typename ActionT::Feedback::ConstSharedPtr feedback_msg) { + std::string feedback = generate_feedback(feedback_msg); - // convert the result + if (feedback != "") + { + info_("received feedback: " + feedback, id); + } + }; + + send_goal_options_.result_callback = + [this, id](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { + info_("received result", id); if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { - // publish event - if (on_result_) + info_("action succeeded.", id); + + // trigger the events related to on_success state + if (events[id].on_success.interface != "") { - on_result_(run_config_.interface); + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); } + } + else + { + error_("action failed", id); - result = generate_result(wrapped_result.result); - return; + // trigger the events related to on_failure state + if (events[id].on_failure.interface != "") + { + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); + } } + + result_ = wrapped_result.result; + completed_ = true; + cv_.notify_all(); }; - return result_callback; + goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); + info_("goal sent. Waiting for acceptance.", id); + + // Conditional wait + cv_.wait(lock, [this] { return completed_; }); + info_("action complete. Thread closing.", id); } protected: @@ -229,20 +233,20 @@ class ActionRunner : public RunnerBase * @param parameters * @return ActionT::Goal the generated goal */ - virtual typename ActionT::Goal generate_goal(std::shared_ptr parameters) = 0; + virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters, int id) = 0; /** - * @brief generate a typed erased goal result + * @brief Generate a std::string from feedback message * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the action the result can be passed by the caller or ignored + * This function is used to convert feedback messages into generic strings * - * The pattern needs to be implemented in the derived class + * A pattern needs to be implemented in the derived class. If the feedback string + * is empty, nothing will be printed on the screen * - * @param wrapped_result - * @return std::shared_ptr + * @param parameters + * @return ActionT::Feedback the received feedback */ - virtual std::shared_ptr generate_result(const typename ActionT::Result::SharedPtr& result) = 0; + virtual std::string generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg) = 0; /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; @@ -253,6 +257,21 @@ class ActionRunner : public RunnerBase /** goal handle parameter to capture goal response from goal_response_callback */ typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + + /** Wrapped Result */ + typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result_; + + /** Result */ + typename ActionT::Result::SharedPtr result_; + + /** Goal message */ + typename ActionT::Goal goal_msg_; + + /** Goal Handle Future message */ + std::shared_future::SharedPtr> goal_handle_future_; + + /** Result Future*/ + std::shared_future::WrappedResult> result_future_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp index 0e45708..931454e 100644 --- a/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/encap_runner.hpp @@ -33,21 +33,15 @@ class EnCapRunner : public NoTriggerActionRunner * this is deferred since the action client topic name is not known at this level * of abstraction * - * @param node - * @param run_config - * @param action_name - * @param on_started - * @param on_terminated - * @param on_stopped + * @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 action_name action name used in the yaml file, used to load specific configuration from the run_config */ virtual void init_encapsulated_action(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - const std::string& action_name, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) + const std::string& action_name, std::function print) { // init the base action runner - init_action(node, run_config, action_name, on_started, on_terminated, on_stopped); + init_action(node, run_config, action_name, print); // create an encapsulating action server encap_action_ = rclcpp_action::create_server( diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 9323d9a..b4c101d 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -1,10 +1,7 @@ #pragma once -#include -#include -#include -#include -#include +#include +#include namespace capabilities2_runner { @@ -14,54 +11,140 @@ namespace capabilities2_runner * * Create a launch file runner to run a launch file based capability */ -class LaunchRunner : public NoTriggerActionRunner +class LaunchRunner : public RunnerBase { public: - LaunchRunner() : NoTriggerActionRunner() + using Launch = capabilities2_msgs::srv::Launch; + + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + LaunchRunner() : RunnerBase() { } - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) override + /** + * @brief Starter function for starting the launch runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { - // store node pointer and run_config - init_action(node, run_config, "capabilities_launch_proxy/launch", on_started, on_terminated, on_stopped); + init_base(node, run_config); + + package_name = run_config_.runner.substr(0, run_config_.runner.find("/")); + launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); + + // create an service client + start_service_client_ = node_->create_client("/capabilities/launch/start"); + + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch/start", + run_config_.interface.c_str()); - // get the package path from environment variable - std::string package_path; - try + if (!start_service_client_->wait_for_service(std::chrono::seconds(3))) { - package_path = ament_index_cpp::get_package_share_directory(get_package_name()); + RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch/start", + run_config_.interface.c_str()); + throw runner_exception("Failed to connect to server: /capabilities/launch/start"); } - catch (const std::exception& e) + + RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch/start", + run_config_.interface.c_str()); + + // create an service client + stop_service_client_ = node_->create_client("/capabilities/launch/stop"); + + // wait for action server + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch/stop", + run_config_.interface.c_str()); + + if (!stop_service_client_->wait_for_service(std::chrono::seconds(3))) { - RCLCPP_ERROR(node_->get_logger(), "Failed to get package share directory: %s", e.what()); - throw runner_exception("failed to get package share directory"); + RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch/stop", + run_config_.interface.c_str()); + throw runner_exception("Failed to connect to server: /capabilities/launch/stop"); } - // resolve launch path - // get full path to launch file - // join package path with package name using path functions - std::string launch_file_path = std::filesystem::path(package_path).append(run_config_.runner).string(); + RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch/stop", + run_config_.interface.c_str()); + + // generate a reequest from launch_name and package_name + auto request_msg = std::make_shared(); + + request_msg->package_name = package_name; + request_msg->launch_file_name = launch_name; + + RCLCPP_INFO(node_->get_logger(), "Requesting to launch %s from %s", launch_name.c_str(), package_name.c_str()); + + auto result_future = start_service_client_->async_send_request( + request_msg, [this](typename rclcpp::Client::SharedFuture future) { + if (!future.valid()) + { + RCLCPP_ERROR(node_->get_logger(), "Request to launch %s from %s failed", launch_name.c_str(), + package_name.c_str()); + return; + } + + RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded", launch_name.c_str(), + package_name.c_str()); + }); + } + + /** + * @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"); - // the launch file path - RCLCPP_DEBUG(node_->get_logger(), "launch file path: %s", launch_file_path.c_str()); + // generate a reequest from launch_name and package_name + auto request_msg = std::make_shared(); - // create a launch goal - capabilities2_msgs::action::Launch::Goal goal; - goal.launch_file_path = launch_file_path; + request_msg->package_name = package_name; + request_msg->launch_file_name = launch_name; - send_goal_options_.result_callback = nullptr; + RCLCPP_INFO(node_->get_logger(), "Requesting to stop %s from %s", launch_name.c_str(), package_name.c_str()); - // launch runner using action client - action_client_->async_send_goal(goal, send_goal_options_); + auto result_future = stop_service_client_->async_send_request( + request_msg, [this](typename rclcpp::Client::SharedFuture future) { + if (!future.valid()) + { + RCLCPP_ERROR(node_->get_logger(), "Request to stop %s from %s failed ", launch_name.c_str(), + package_name.c_str()); + return; + } + + RCLCPP_INFO(node_->get_logger(), "Request to launch %s from %s succeeded ", launch_name.c_str(), + package_name.c_str()); + }); + + info_("stopping runner"); } -private: - /** launch file path */ - std::string launch_file_path; + // throw on trigger function + void trigger(const std::string& parameters) override + { + throw runner_exception("No Trigger as this is launch runner"); + } + +protected: + // throw on triggerExecution function + void execution(int id) override + { + throw runner_exception("no triggerExecution() this is a no-trigger action runner"); + } + + std::string launch_name; + std::string package_name; + + rclcpp::Client::SharedPtr start_service_client_; + rclcpp::Client::SharedPtr stop_service_client_; }; -} // namespace capabilities2_runner +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp deleted file mode 100644 index 1cbb1c1..0000000 --- a/capabilities2_runner/include/capabilities2_runner/multi_action_runner.hpp +++ /dev/null @@ -1,266 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -namespace capabilities2_runner -{ - -/** - * @brief templated struct to handle Action clients and their respective goal_handles - * - * @tparam ActionT action type - */ -template -struct ActionClientBundle -{ - std::shared_ptr> action_client; - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle; - typename rclcpp_action::Client::SendGoalOptions send_goal_options; -}; - -/** - * @brief Multi action runner base class - * - * Create action clients to run multiple action based capabilities - * this class provides helpers to set up sequential and concurrent actions - */ -class MultiActionRunner : public RunnerBase -{ -public: - /** - * @brief Constructor which needs to be empty due to plugin semantics - */ - MultiActionRunner() : RunnerBase() - { - } - - /** - * @brief create action clients - * - * Create a new action client and store it in the map - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - */ - template - void init_action(const std::string& action_name) - { - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; - typename rclcpp_action::Client::SendGoalOptions send_goal_options_; - - auto client_ = rclcpp_action::create_client(node_, action_name); - - // wait for action server - RCLCPP_INFO(node_->get_logger(), "%s waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); - - if (!client_->wait_for_action_server(std::chrono::seconds(3))) - { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to action: %s", run_config_.interface.c_str(), - action_name.c_str()); - throw runner_exception("failed to connect to action server"); - } - - ActionClientBundle bundle{ client_, goal_handle_, send_goal_options_ }; - - action_clients_map_[action_name] = std::make_any>(bundle); - } - - /** - * @brief Deinitializer function for stopping an the action - * - * Stop the named action and delete the action client - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - */ - template - void deinit_action(const std::string& action_name) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - // 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 action client is null - // this can happen if the runner is not able to find the action resource - if (!bundle.action_client) - throw runner_exception("cannot stop runner action that was not started"); - - // stop runner using action client - if (bundle.goal_handle) - { - try - { - auto cancel_future = bundle.action_client->async_cancel_goal( - bundle.goal_handle, [this](action_msgs::srv::CancelGoal_Response::SharedPtr response) { - if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) - { - // throw runner_exception("failed to cancel runner"); - } - - // publish event - if (on_stopped_) - { - on_stopped_(run_config_.interface); - } - }); - - // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope - rclcpp::spin_until_future_complete(node_, cancel_future, std::chrono::seconds(2)); - } - catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) - { - RCLCPP_ERROR(node_->get_logger(), "failed to cancel goal: %s", e.what()); - throw runner_exception(e.what()); - } - } - - // delete action client - bundle.action_client.reset(); - action_clients_map_.erase(action_name); - } - - /** - * @brief Trigger function for calling and triggering an action. Non blocking implementation so result will not - * be returned. Use when action triggering is required and result message of the action is not required. - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param goal_msg goal message to be sent to the action server - * - * @returns True for success of launching an action. False for failure to launching the action. - */ - template - bool trigger_action(const std::string& action_name, typename ActionT::Goal& goal_msg) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - // result callback - bundle.send_goal_options.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - if (wrapped_result.code != rclcpp_action::ResultCode::SUCCEEDED) - { - // send terminated event - if (on_terminated_) - { - on_terminated_(run_config_.interface); - } - } - }; - - auto goal_handle_future = bundle.action_client->async_send_goal(goal_msg, bundle.send_goal_options); - - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); - return false; - } - - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - - if (!goal_handle) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return false; - } - else - { - // publish event - if (on_started_) - on_started_(run_config_.interface); - - // store goal handle to be used with stop funtion - bundle.goal_handle = goal_handle; - return true; - } - - action_clients_map_[action_name] = std::make_any>(bundle); - } - - /** - * @brief Trigger function for calling and triggering an action. Blocking implementation so result will be returned. - * Use when result message of the action is required. - * - * @param action_name action name used in the yaml file, used to load specific configuration from the run_config - * @param goal_msg goal message to be sent to the action server - * @param result_msg result message returned by the action server upon completion - * - * @returns True for success of completing an action. False for failure to complete the action. - */ - template - bool trigger_action_wait(const std::string& action_name, typename ActionT::Goal& goal_msg, - typename ActionT::Result::SharedPtr result_msg) - { - auto bundle = std::any_cast>(action_clients_map_[action_name]); - - auto goal_handle_future = bundle.action_client->async_send_goal(goal_msg, bundle.send_goal_options); - - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); - return false; - } - - typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - - if (!goal_handle) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return false; - } - else - { - // publish event - if (on_started_) - on_started_(run_config_.interface); - - // store goal handle to be used with stop funtion - bundle.goal_handle = goal_handle; - } - - // Wait for the server to be done with the goal - auto result_future = bundle.action_client->async_get_result(goal_handle); - - RCLCPP_INFO(node_->get_logger(), "Waiting for result"); - - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); - return false; - } - - typename rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); - - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - result_msg = wrapped_result.result; - return true; - } - else - { - // send terminated event - if (on_terminated_) - on_terminated_(run_config_.interface); - return false; - } - } - -protected: - /** - * Dictionary to hold action client bundle. The key is a string, and the value is a - * polymorphic bundle. - * */ - std::map action_clients_map_; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp index edc07eb..1287105 100644 --- a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp @@ -18,22 +18,23 @@ class NoTriggerActionRunner : public ActionRunner { public: // throw on trigger function - std::optional)>> - trigger(std::shared_ptr parameters) override + void trigger(tinyxml2::XMLElement* parameters) override { throw runner_exception("cannot trigger this is a no-trigger action runner"); } protected: - // throw on xml conversion functions - typename ActionT::Goal generate_goal(std::shared_ptr) override + + // throw on triggerExecution function + void execution() override { - throw runner_exception("cannot generate goal this is a no-trigger action runner"); + throw runner_exception("no triggerExecution() this is a no-trigger action runner"); } - std::shared_ptr generate_result(const typename ActionT::Result::SharedPtr& result) override + // throw on xml conversion functions + typename ActionT::Goal generate_goal(tinyxml2::XMLElement*) override { - throw runner_exception("cannot generate result this is a no-trigger action runner"); + throw runner_exception("cannot generate goal this is a no-trigger action runner"); } }; diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index 1c12a84..b0e20ae 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -3,10 +3,17 @@ #include #include #include -#include +#include +#include #include #include +#include + +#include +#include +#include + namespace capabilities2_runner { /** @@ -56,11 +63,15 @@ struct runner_opts std::string runner; std::string started_by; std::string pid; + int input_count; }; class RunnerBase { public: + using Event = event_logger_msgs::msg::Event; + using EventType = capabilities2::event_t; + RunnerBase() : run_config_() { } @@ -76,14 +87,9 @@ class RunnerBase * * @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 on_started pointer to function to execute on starting the runner - * @param on_terminated pointer to function to execute on terminating the runner - * @param on_stopped pointer to function to execute on stopping the runner + * @param print_ */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) = 0; + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) = 0; /** * @brief stop the runner @@ -92,38 +98,64 @@ class RunnerBase virtual void stop() = 0; /** - * @brief trigger the runner + * @brief Trigger the runner * - * this method allows insertion of parameters in a runner after it has been initialized - * it is an approach to parameterise capabilities + * This method allows insertion of parameters in a runner after it has been initialized. it is an approach + * to parameterise capabilities. Internally starts up RunnerBase::triggerExecution in a thread * * @param parameters pointer to tinyxml2::XMLElement that contains parameters * - * @return std::optional)>> function pointer to invoke - * elsewhere such as an event callback */ - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) = 0; + virtual void trigger(const std::string& parameters) + { + // extract the unique id for the runner and use that as the thread id + tinyxml2::XMLElement * element = nullptr; + element = convert_to_xml(parameters); + element->QueryIntAttribute("id", &runner_id); + + parameters_[runner_id] = element; + + info_("received new parameters with event id : " + std::to_string(runner_id), runner_id); + + executionThreadPool[runner_id] = std::thread(&RunnerBase::execution, this, runner_id); + + info_("started execution", runner_id); + } /** * @brief Initializer function for initializing the base runner in place of constructor due to plugin semantics * * @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 on_started pointer to function to execute on starting the runner - * @param on_terminated pointer to function to execute on terminating the runner */ - void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) + void init_base(rclcpp::Node::SharedPtr node, const runner_opts& run_config) { // store node pointer and opts node_ = node; run_config_ = run_config; - on_started_ = on_started; - on_terminated_ = on_terminated; - on_stopped_ = on_stopped; + + current_inputs_ = 0; + runner_id = 0; + + event_client_ = std::make_shared(node_, "runner", "/events"); + } + + /** + * @brief attach events to the runner + * + * @param event_option event_options related for the action + * @param triggerFunction external function that triggers capability runners + * + * @return number of attached events + */ + virtual void attach_events(capabilities2::event_opts& event_option, + std::function triggerFunction) + { + info_("accepted event options with ID : " + std::to_string(event_option.event_id)); + + triggerFunction_ = triggerFunction; + + events[event_option.event_id] = event_option; } /** @@ -166,7 +198,136 @@ class RunnerBase return run_config_.pid; } + /** + * @brief Get the execution status of runner. + * + * @return `true` if execution is complete, `false` otherwise. + */ + const bool get_completion_status() const + { + return execution_complete_; + } + protected: + /** + * @brief Trigger process to be executed. + * + * This method utilizes paramters set via the trigger() function + * + * @param id unique identifier for the runner id. used to track the correct + * triggers and subsequent events. + * + */ + virtual void execution(int id) = 0; + + /** + * @brief Update on_started event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_started 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_started(std::string& parameters) + { + return parameters; + }; + + /** + * @brief Update on_stopped event parameters with new data if avaible. + * + * This function is used to inject new data into the XMLElement containing + * parameters related to the on_stopped 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_stopped(std::string& parameters) + { + return parameters; + }; + + /** + * @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) + { + return parameters; + }; + + /** + * @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) + { + return parameters; + }; + + /** + * @brief convert an 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 + */ + std::string convert_to_string(tinyxml2::XMLElement* element) + { + if (element) + { + element->Accept(&printer); + std::string parameters = printer.CStr(); + return parameters; + } + else + { + std::string parameters = ""; + return parameters; + } + } + + /** + * @brief convert an 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 + */ + tinyxml2::XMLElement* convert_to_xml(const std::string& parameters) + { + if (parameters != "") + { + doc.Parse(parameters.c_str()); + tinyxml2::XMLElement* element = doc.FirstChildElement(); + return element; + } + else + { + return nullptr; + } + } // run config getters /** @@ -301,35 +462,177 @@ class RunnerBase } protected: + void info_(const std::string text, int thread_id = -1) + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = "runners"; + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = ""; + message.target.provider = ""; + message.thread_id = thread_id; + message.type = Event::INFO; + message.content = text; + message.pid = -1; + message.event = Event::UNDEFINED; + + event_client_->publish(message); + } + + void error_(const std::string text, int thread_id = -1) + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = "runners"; + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = ""; + message.target.provider = ""; + message.thread_id = thread_id; + message.type = Event::ERROR; + message.content = text; + message.pid = -1; + message.event = Event::UNDEFINED; + + event_client_->publish(message); + } + + void output_(const std::string text, const std::string element, int thread_id = -1) + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = "runners"; + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = ""; + message.target.provider = ""; + message.thread_id = thread_id; + message.type = Event::INFO; + message.content = text + " : " + element; + message.pid = -1; + message.event = Event::UNDEFINED; + + event_client_->publish(message); + } + + void event_(EventType event = EventType::IDLE, int thread_id = -1, const std::string& target_capability = "", + const std::string& target_provider = "") + { + auto message = Event(); + + message.header.stamp = node_->now(); + message.origin_node = "runners"; + message.source.capability = run_config_.interface; + message.source.provider = run_config_.provider; + message.target.capability = target_capability; + message.target.provider = target_provider; + message.thread_id = thread_id; + message.type = Event::RUNNER_EVENT; + message.pid = -1; + + switch (event) + { + case EventType::IDLE: + message.event = Event::IDLE; + break; + case EventType::STARTED: + message.event = Event::STARTED; + break; + case EventType::STOPPED: + message.event = Event::STOPPED; + break; + case EventType::FAILED: + message.event = Event::FAILED; + break; + case EventType::SUCCEEDED: + message.event = Event::SUCCEEDED; + break; + default: + message.event = Event::UNDEFINED; + break; + } + + event_client_->publish(message); + } + /** - * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @brief shared pointer to the capabilities node. Allows to use ros node related functionalities */ rclcpp::Node::SharedPtr node_; /** - * @param node runner configuration + * @brief run_config_ runner configuration */ runner_opts run_config_; /** - * @param node pointer to function to execute on starting the runner + * @brief dictionary of events + */ + std::map events; + + /** + * @brief unique id for the runner + */ + int runner_id; + + /** + * @brief curent number of trigger signals received + */ + int current_inputs_; + + /** + * @brief system runner completion tracking + */ + bool execution_complete_; + + /** + * @brief pointer to XMLElement which contain parameters + */ + std::map parameters_; + + /** + * @brief dictionary of threads that executes the triggerExecution functionality + */ + std::map executionThreadPool; + + /** + * @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_; + + /** + * @brief external function that triggers capability runners */ - std::function on_started_; + std::function triggerFunction_; /** - * @param node pointer to function to execute on terminating the runner + * @brief XMLElement that is used to convert xml strings to std::string */ - std::function on_terminated_; + tinyxml2::XMLPrinter printer; /** - * @param node pointer to function to execute on stopping the runner + * @brief XMLElement that is used to convert std::string to xml strings */ - std::function on_stopped_; + tinyxml2::XMLDocument doc; /** - * @param node pointer to function to execute on result + * @brief client for publishing events */ - std::function on_result_; + std::shared_ptr event_client_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp new file mode 100644 index 0000000..d28f6be --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -0,0 +1,183 @@ +#pragma once + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief service runner base class + * + * Create an server client to run an service based capability + */ +template +class ServiceRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + ServiceRunner() : RunnerBase() + { + } + + /** + * @brief Initializer function for initializing the service runner in place of constructor due to plugin semantics + * + * @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 service_name action name used in the yaml file, used to load specific configuration from the run_config + */ + virtual void init_service(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + const std::string& service_name) + { + // initialize the runner base by storing node pointer and run config + init_base(node, run_config); + + // create an service client + service_client_ = node_->create_client(service_name); + + // wait for action server + info_("waiting for service: " + service_name); + + if (!service_client_->wait_for_service(std::chrono::seconds(3))) + { + error_("failed to connect to service: " + service_name); + throw runner_exception("failed to connect to server"); + } + + info_("connected with service: " + service_name); + } + + /** + * @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 + { + // if parameters are not provided then cannot proceed + if (!parameters_[id]) + throw runner_exception("cannot trigger service without parameters"); + + // generate a goal from parameters if provided + auto request_msg = std::make_shared(generate_request(parameters_[id], id)); + + info_("request generated for event :", id); + + std::unique_lock lock(mutex_); + completed_ = false; + + auto result_future = service_client_->async_send_request( + request_msg, [this, id](typename rclcpp::Client::SharedFuture future) { + if (!future.valid()) + { + error_("get result call failed"); + + // trigger the events related to on_failure state + if (events[id].on_failure.interface != "") + { + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); + } + } + else + { + info_("get result call succeeded", id); + + response_ = future.get(); + process_response(response_, id); + + // trigger the events related to on_success state + if (events[id].on_success.interface != "") + { + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); + } + } + + completed_ = true; + cv_.notify_all(); + }); + + // trigger the events related to on_started state + if (events[id].on_started.interface != "") + { + event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); + triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); + } + + // Conditional wait + cv_.wait(lock, [this] { return completed_; }); + info_("Service request complete. 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 (!service_client_) + throw runner_exception("cannot stop runner action that was not started"); + + // Trigger on_stopped event if defined + if (events[runner_id].on_stopped.interface != "") + { + event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); + triggerFunction_(events[runner_id].on_stopped.interface, + update_on_stopped(events[runner_id].on_stopped.parameters)); + } + + info_("removing event options"); + + // remove all event options for this runner instance + const auto n = events.size(); + events.clear(); + info_("removed event options for " + std::to_string(n) + " runner ids"); + + info_("runner cleaned. stopping.."); + } + +protected: + /** + * @brief Generate a request from parameters + * + * This function is used in conjunction with the trigger function to inject type erased parameters + * into the typed service + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return ServiceT::Request the generated request + */ + 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_; +}; +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp new file mode 100644 index 0000000..5cf2138 --- /dev/null +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -0,0 +1,155 @@ +#pragma once +#include + +#include "rclcpp/rclcpp.hpp" +#include +#include + +namespace capabilities2_runner +{ + +/** + * @brief Topic runner base class + * + * Create an topic subsriber for data grabbing capability + */ +template +class TopicRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + TopicRunner() : RunnerBase() + { + } + + /** + * @brief Initializer function for initializing the topic runner in place of constructor due to plugin semantics + * + * @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 topic_name topic name used in the yaml file, used to load specific configuration from the run_config + */ + virtual void init_subscriber(rclcpp::Node::SharedPtr node, const runner_opts& run_config, + const std::string& topic_name) + { + // initialize the runner base by storing node pointer and run config + init_base(node, run_config); + + // create an service client + subscription_ = node_->create_subscription( + topic_name, 10, [this](const typename TopicT::SharedPtr msg) { this->callback(msg); }); + } + + /** + * @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 + { + // 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[id].on_started.interface != "") + { + event_(EventType::STARTED, id, events[id].on_started.interface, events[id].on_started.provider); + triggerFunction_(events[id].on_started.interface, update_on_started(events[id].on_started.parameters)); + } + + 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 + if (events[id].on_success.interface != "") + { + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); + } + } + else + { + error_("Message receving failed."); + + // trigger the events related to on_failure state + if (events[id].on_failure.interface != "") + { + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); + } + } + + info_("Thread closing.", id); + } + + /** + * @brief stop function to cease functionality and shutdown + * + */ + virtual void stop() override + { + // if the node pointer is empty then throw an error + // this means that the runner was not started and is being used out of order + + if (!node_) + throw runner_exception("cannot stop runner that was not started"); + + // throw an error if the service client is null + // this can happen if the runner is not able to find the action resource + + if (!subscription_) + throw runner_exception("cannot stop runner subscriber that was not started"); + + // Trigger on_stopped event if defined + if (events[runner_id].on_stopped.interface != "") + { + event_(EventType::STOPPED, -1, events[runner_id].on_stopped.interface, events[runner_id].on_stopped.provider); + triggerFunction_(events[runner_id].on_stopped.interface, + update_on_stopped(events[runner_id].on_stopped.parameters)); + } + + info_("removing event options"); + + // remove all event options for this runner instance + const auto n = events.size(); + events.clear(); + info_("removed event options for " + std::to_string(n) + " runner ids"); + + info_("runner cleaned. stopping.."); + } + +protected: + /** + * @brief Callback to be executed when receiving a message + * + * This function is used grab the messages from the callback queue into a class + * parameter so that it can be used later on dering trigger + * + * @param msg message parameter + */ + 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_; +}; +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner/package.xml b/capabilities2_runner/package.xml index e010e22..f33f924 100644 --- a/capabilities2_runner/package.xml +++ b/capabilities2_runner/package.xml @@ -8,6 +8,7 @@ Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard @@ -20,10 +21,13 @@ pluginlib std_msgs capabilities2_msgs + capabilities2_utils + event_logger + event_logger_msgs + tinyxml2_vendor uuid - tinyxml2 ament_cmake diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index 69f3745..58ca354 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -7,13 +7,10 @@ namespace capabilities2_runner class DummyRunner : public RunnerBase { public: - void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) override + void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override { // init the base runner - init_base(node, run_config, on_started, on_terminated, on_stopped); + init_base(node, run_config); // do nothing RCLCPP_INFO(node_->get_logger(), "Dummy runner started"); @@ -29,11 +26,16 @@ class DummyRunner : public RunnerBase RCLCPP_INFO(node_->get_logger(), "Dummy runner stopped"); } - std::optional)>> - trigger(std::shared_ptr parameters) override + void trigger(const std::string& parameters) override { RCLCPP_INFO(node_->get_logger(), "Dummy runner cannot trigger"); - return std::nullopt; + } + +protected: + // throw on triggerExecution function + void execution(int id) override + { + RCLCPP_INFO(node_->get_logger(), "Dummy runner does not have triggerExecution()"); } }; diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp deleted file mode 100644 index 0df18c1..0000000 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief action runner base class - * - * Class to run VoiceListener action based capability - * - */ -class VoiceListenerRunner : public MultiActionRunner -{ -public: - VoiceListenerRunner() : MultiActionRunner() - { - } - - /** - * @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 - * @param on_started function pointer to trigger at the start of the action client in the runner - * @param on_terminated function pointer to trigger at the termination of the action client in the runner - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) override - { - init_base(node, run_config, on_started, on_terminated, on_stopped); - - init_action("speech_to_text"); - } - - /** - * @brief stop function to cease functionality and shutdown - * - */ - virtual void stop() override - { - deinit_action("speech_to_text"); - } - - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) override - { - hri_audio_msgs::action::SpeechToText::Goal goal_msg; - hri_audio_msgs::action::SpeechToText::Result::SharedPtr result_msg; - - goal_msg.header.stamp = node_->get_clock()->now(); - - return [this, &goal_msg, &result_msg](std::shared_ptr result) { - bool action_result = - trigger_action_wait("speech_to_text", goal_msg, result_msg); - result->BoolAttribute("result", action_result); - }; - } - -protected: -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/capabilities2_runner_audio/package.xml b/capabilities2_runner_audio/package.xml deleted file mode 100644 index 915904c..0000000 --- a/capabilities2_runner_audio/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - capabilities2_runner_audio - 0.0.0 - TODO: Package description - kalana - - MIT - - ament_cmake - - rclcpp - rclcpp_action - pluginlib - std_msgs - hri_audio_msgs - capabilities2_msgs - capabilities2_runner - - - tinyxml2 - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/capabilities2_runner_audio/plugins.xml b/capabilities2_runner_audio/plugins.xml deleted file mode 100644 index d43e9c3..0000000 --- a/capabilities2_runner_audio/plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - A plugin that provide speech to text capability - - - \ No newline at end of file diff --git a/capabilities2_runner_audio/src/audio_runners.cpp b/capabilities2_runner_audio/src/audio_runners.cpp deleted file mode 100644 index f7b73c5..0000000 --- a/capabilities2_runner_audio/src/audio_runners.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include -#include -#include - -namespace capabilities2_runner -{ - -} - -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::VoiceListenerRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_bt/.clang-format b/capabilities2_runner_bt/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_bt/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -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_bt/CMakeLists.txt b/capabilities2_runner_bt/CMakeLists.txt deleted file mode 100644 index aae09e2..0000000 --- a/capabilities2_runner_bt/CMakeLists.txt +++ /dev/null @@ -1,60 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(capabilities2_runner_bt) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(pluginlib REQUIRED) -find_package(capabilities2_msgs REQUIRED) -find_package(capabilities2_runner REQUIRED) -find_package(behaviortree_cpp REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) - -include_directories(include -${TINYXML2_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} SHARED - src/bt_runners.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_action - pluginlib - capabilities2_msgs - capabilities2_runner - behaviortree_cpp - TINYXML2 -) - -pluginlib_export_plugin_description_file(${PROJECT_NAME} 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_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp deleted file mode 100644 index b26ebc0..0000000 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ /dev/null @@ -1,89 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief base runner class for a behaviortree factory - * - * The runner implements a behaviour defined by a behaviourtree - * and executes it in a behaviortree - * - */ -class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory -{ -public: - BTRunnerBase() : RunnerBase(), BT::BehaviorTreeFactory() - { - } - - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) - { - // init the runner base - init_base(node, run_config, on_started, on_terminated, on_stopped); - - // register (bt)actions from ROS plugins - try - { - this->registerFromROSPlugins(); - } - catch (const std::exception& e) // TODO: add more specific exception - { - throw runner_exception(e.what()); - } - } - - virtual void stop() - { - // reset the tree (this destroys the nodes) - tree_.reset(); - } - - /** - * @brief trigger the behaviour factory with the input data - * - * @param parameters - * @return std::optional)>> - */ - virtual std::optional)>> - trigger(std::shared_ptr parameters) - { - // if parameters are not provided then cannot proceed - if (!parameters) - throw runner_exception("cannot trigger action without parameters"); - - // create the tree (ptr) - tree_ = std::make_shared(this->createTreeFromText(parameters->GetText())); - - // return the tick function - // the caller can call this function to tick the tree - std::function)> result_callback = - [this](std::shared_ptr result) { - // tick the tree - BT::NodeStatus status = tree_->tickWhileRunning(); - - // fill the result - if (status == BT::NodeStatus::SUCCESS) - result->SetText("OK"); - else - result->SetText("FAIL"); - - return; - }; - - return result_callback; - } - -protected: - // the tree - std::shared_ptr tree_; -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp deleted file mode 100644 index 8bf4dea..0000000 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_runner_base.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#pragma once - -/** - * @file bt_runner_base.hpp - * @brief This file is heavily inspired by the nav2_behavior_tree::BtActionNode implementation - * as it performs almost the same functionality - * - * The nav2_behavior_tree::BtActionNode is part of navigation2 - * which is licensed under the Apache 2.0 license. see the following - * license file for more details: - * - */ - -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -namespace capabilities2_runner -{ - -/** - * @brief base class for a behaviortree action - * - * This class is heavily inspired by the nav2_behavior_tree::BtActionNode - * - * The action implements a action runner and an action node to combine the two - * domain definitions of 'action' - * - * Through this class, an action can be run in a behaviortree which implements a ROS action - * - * @tparam ActionT - */ -template -class BTRunnerBase : public ActionRunner, public BT::ActionNodeBase -{ -public: - BTRunnerBase() : ActionRunner(), BT::ActionNodeBase() - { - } - - // init pattern for bt and runner - /** - * @brief initializer function for initializing the action runner in place of constructor due to plugin semantics - * - * TODO: this is getting pretty messy - * - * @param node - * @param run_config - * @param action_name - * @param on_started - * @param on_terminated - * @param on_stopped - * @param bt_tag_name - * @param conf - */ - virtual void init_bt(rclcpp::Node::SharedPtr node, const runner_opts& run_config, const std::string& action_name, - const std::string& bt_tag_name, const BT::NodeConfiguration& conf, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) - { - ActionRunner::init_action(node, run_config, action_name, on_started, on_terminated, on_stopped); - - // FIXME: no init in action base - // BT::ActionNodeBase::init(bt_tag_name, conf); - } - - // bt methods that need to be overridden - BT::NodeStatus tick() override - { - // TODO: run the action client? - } - - void halt() override - { - // cancel the action client if it is running - // can be implemented with the stop virtual method - // from action runner - ActionRunner::stop(); - } - - // the action runner methods that need to be overridden - // these can be overridden when this class is inherited and the action runner - // action template is resolved - // - // see runner_base.hpp for more details - // - // virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - // std::function on_started = nullptr, - // std::function on_terminated = nullptr, - // std::function on_stopped = nullptr) override - // - // virtual void trigger(std::shared_ptr parameters = nullptr) override -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_bt/package.xml b/capabilities2_runner_bt/package.xml deleted file mode 100644 index 2c84b98..0000000 --- a/capabilities2_runner_bt/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - capabilities2_runner_bt - 0.0.1 - - Package for behaviour tree based capabilities2 runners - - - Michael Pritchard - mik-p - - Michael Pritchard - - MIT - - ament_cmake - - rclcpp - rclcpp_action - pluginlib - std_msgs - capabilities2_msgs - capabilities2_runner - behaviortree_cpp - - - uuid - tinyxml2 - - - ament_cmake - - diff --git a/capabilities2_runner_bt/plugins.xml b/capabilities2_runner_bt/plugins.xml deleted file mode 100644 index 1289f20..0000000 --- a/capabilities2_runner_bt/plugins.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/capabilities2_runner_bt/readme.md b/capabilities2_runner_bt/readme.md deleted file mode 100644 index 16d8543..0000000 --- a/capabilities2_runner_bt/readme.md +++ /dev/null @@ -1,35 +0,0 @@ -# capabilities2_runner_bt - -Behavior tree runners for [capabilities2](../readme.md). This package provides a runner for the capabilities2 package that uses behavior trees to combine and execute composite runners in the capabilities framework. - -## Runners - -| Runner | Description | -| ------ | ----------- | -| `BTRunnerBase` | A base class for behavior tree runners. This class is used to implement the behavior tree runner for the capabilities2 package. It inherits from the [`ActionRunner`](../capabilities2_runner/readme.md) to join ROS action and behavior tree action concepts. | -| `BTFactoryRunner` | Load a behavior tree and run it as a capabilities2 runner (this just means that it runs in the capabilities2_server node). | - -### Provider Example - -The following example demonstrates how to use the `BTFactoryRunner` to load a behavior tree from a file and run it as a capabilities2 runner. - -```yaml -name: bt_ask_help -spec_version: "1.1" -spec_type: provider -implements: std_capabilities/ask_help -runner: capabilities2_runner_bt/BTFactoryRunner - -# the tree to load -definition: | - - - - - - - - - - -``` diff --git a/capabilities2_runner_bt/src/bt_runners.cpp b/capabilities2_runner_bt/src/bt_runners.cpp deleted file mode 100644 index 3a630db..0000000 --- a/capabilities2_runner_bt/src/bt_runners.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include -#include - -namespace capabilities2_runner -{ - -} // namespace capabilities2_runner - -// PLUGINLIB_EXPORT_CLASS(capabilities2_runner::BTRunnerBase, capabilities2_runner::RunnerBase) diff --git a/capabilities2_executor/.clang-format b/capabilities2_runner_capabilities/.clang-format similarity index 100% rename from capabilities2_executor/.clang-format rename to capabilities2_runner_capabilities/.clang-format diff --git a/capabilities2_runner_audio/CMakeLists.txt b/capabilities2_runner_capabilities/CMakeLists.txt similarity index 59% rename from capabilities2_runner_audio/CMakeLists.txt rename to capabilities2_runner_capabilities/CMakeLists.txt index 15ba918..6aa29c9 100644 --- a/capabilities2_runner_audio/CMakeLists.txt +++ b/capabilities2_runner_capabilities/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(capabilities2_runner_audio) +project(capabilities2_runner_capabilities) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -10,40 +10,39 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) -find_package(hri_audio_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) +find_package(capabilities2_utils REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -include_directories(include -${TINYXML2_INCLUDE_DIRS} +include_directories( + include ) add_library(${PROJECT_NAME} SHARED - src/audio_runners.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} + src/capabilities2_runner.cpp ) ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action pluginlib - hri_audio_msgs capabilities2_msgs capabilities2_runner - TINYXML2 + capabilities2_utils + event_logger + event_logger_msgs + TinyXML2 ) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) +pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} @@ -56,6 +55,14 @@ install(DIRECTORY include/ DESTINATION include ) +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) + ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp new file mode 100644 index 0000000..e6d70ad --- /dev/null +++ b/capabilities2_runner_capabilities/include/capabilities2_runner_capabilities/get_runner.hpp @@ -0,0 +1,117 @@ +#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) override + { + init_service(node, run_config, "/capabilities/get_capability_specs"); + } + +protected: + /** + * @brief This generate goal function overrides the generate_goal() function from ActionRunner() + * @param parameters XMLElement that contains parameters in the format + '' + * @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/interfaces/CapabilityGetRunner.yaml b/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml new file mode 100644 index 0000000..c50bbbb --- /dev/null +++ b/capabilities2_runner_capabilities/interfaces/CapabilityGetRunner.yaml @@ -0,0 +1,34 @@ +%YAML 1.1 +--- +name: CapabilityGetRunner +spec_version: 1.1 +spec_type: interface +description: "This capability depends on the capabilities2 server functionalities and allows an decision making authority such as an LLM extract + capabilities of the robot. The capability can be trigged with an command such as + ''. + This is included in the default starting plan and the decision making authority such as an LLM does not need to include this in any + generated plans. The details for the Capaiblities are stored in a format as follows, + ' + + + name: Navigation + dependencies: + - MapServer + - PathPlanner + + + + + name: ObjectDetection + dependencies: + - Camera + - ImageProcessor + + + ' " + +interface: + services: + "/capabilities/get_capability_specs": + type: "capabilities2_msgs::srv::GetCapabilitySpecs" + description: "This capability focuses on extracting robot's capabilities and transfering them to decision making authority." \ No newline at end of file diff --git a/capabilities2_runner_capabilities/package.xml b/capabilities2_runner_capabilities/package.xml new file mode 100644 index 0000000..53bff7d --- /dev/null +++ b/capabilities2_runner_capabilities/package.xml @@ -0,0 +1,43 @@ + + + + capabilities2_runner_capabilities + 0.0.0 + TODO: Package description + + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake + + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_action + pluginlib + std_msgs + capabilities2_msgs + capabilities2_runner + event_logger + event_logger_msgs + tinyxml2_vendor + + + ament_cmake + + + + interfaces/CapabilityGetRunner.yaml + + + + providers/CapabilityGetRunner.yaml + + + diff --git a/capabilities2_runner_capabilities/plugins.xml b/capabilities2_runner_capabilities/plugins.xml new file mode 100644 index 0000000..f4ff0f8 --- /dev/null +++ b/capabilities2_runner_capabilities/plugins.xml @@ -0,0 +1,7 @@ + + + + A plugin that request capabilities from the capabilities server and transfers to an LLM + + + diff --git a/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml b/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml new file mode 100644 index 0000000..640e3fc --- /dev/null +++ b/capabilities2_runner_capabilities/providers/CapabilityGetRunner.yaml @@ -0,0 +1,9 @@ +# the empty provider +%YAML 1.1 +--- +name: CapabilityGetRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for extracting capabilities from server. This is used to identify the capabilities of the robot" +implements: capabilities2_runner_capabilities/CapabilityGetRunner +runner: capabilities2_runner::CapabilityGetRunner \ No newline at end of file diff --git a/capabilities2_runner_capabilities/src/capabilities2_runner.cpp b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp new file mode 100644 index 0000000..bcdf964 --- /dev/null +++ b/capabilities2_runner_capabilities/src/capabilities2_runner.cpp @@ -0,0 +1,6 @@ +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::CapabilityGetRunner, capabilities2_runner::RunnerBase); diff --git a/capabilities2_runner_nav2/.clang-format b/capabilities2_runner_nav2/.clang-format deleted file mode 100644 index d36804f..0000000 --- a/capabilities2_runner_nav2/.clang-format +++ /dev/null @@ -1,64 +0,0 @@ - -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_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp deleted file mode 100644 index 36b5310..0000000 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ /dev/null @@ -1,123 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include - -#include - -namespace capabilities2_runner -{ - -/** - * @brief action runner base class - * - * Class to run waypointfollower action based capability - * - */ -class WayPointRunner : public ActionRunner -{ -public: - WayPointRunner() : 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 - * @param on_started function pointer to trigger at the start of the action client in the runner - * @param on_terminated function pointer to trigger at the termination of the action client in the runner - */ - virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config, - std::function on_started = nullptr, - std::function on_terminated = nullptr, - std::function on_stopped = nullptr) override - { - init_action(node, run_config, "follow_waypoints", on_started, on_terminated, on_stopped); - } - - /** - * @brief trigger the runner - * - @param parameters XMLElement that contains parameters in the format '' - */ - virtual std::optional)>> - trigger(std::shared_ptr parameters = nullptr) - { - tinyxml2::XMLElement* parametersElement = parameters->FirstChildElement("waypointfollower"); - - parametersElement->QueryDoubleAttribute("x", &x); - parametersElement->QueryDoubleAttribute("y", &y); - - nav2_msgs::action::FollowWaypoints::Goal goal_msg; - geometry_msgs::msg::PoseStamped pose_msg; - - global_frame_ = "map"; - robot_base_frame_ = "base_link"; - - pose_msg.header.stamp = node_->get_clock()->now(); - pose_msg.header.frame_id = global_frame_; - pose_msg.pose.position.x = x; - pose_msg.pose.position.y = y; - pose_msg.pose.position.z = 0.0; - - goal_msg.poses.push_back(pose_msg); - - auto goal_handle_future = action_client_->async_send_goal(goal_msg); - - return [this, goal_handle_future](std::shared_ptr result) { - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed"); - return; - } - - auto result_future = action_client_->async_get_result(goal_handle_future.get()); - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - return; - } - - auto wrapped_result = result_future.get(); - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - RCLCPP_INFO(node_->get_logger(), "Waypoint reached"); - result->BoolAttribute("result", true); - } - else - { - RCLCPP_INFO(node_->get_logger(), "Waypoint not reached"); - } - }; - } - -protected: - // not implemented - virtual nav2_msgs::action::FollowWaypoints::Goal - generate_goal(std::shared_ptr parameters) override - { - return nav2_msgs::action::FollowWaypoints::Goal(); - } - - virtual std::shared_ptr - generate_result(const nav2_msgs::action::FollowWaypoints::Result::SharedPtr& result) override - { - return nullptr; - } - -protected: - std::string global_frame_; /**The global frame of the robot*/ - std::string robot_base_frame_; /**The frame of the robot base*/ - - double x, y; /**Coordinate frame parameters*/ -}; - -} // namespace capabilities2_runner diff --git a/capabilities2_runner_nav2/interfaces/nav2.yaml b/capabilities2_runner_nav2/interfaces/nav2.yaml deleted file mode 100644 index 1acd342..0000000 --- a/capabilities2_runner_nav2/interfaces/nav2.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# standardised nav2 interface specification -%YAML 1.1 ---- -name: nav2 -spec_version: 1 -spec_type: interface -description: "Navigational capabilities using Nav2 stack" -interface: - actions: - "follow_waypoints": - type: "nav2_msgs::action::FollowWaypoints" - description: "This system allow the robot to navigate to a given two dimensional coordinate - given via '' command. '$value' represents - a value in meters. As an example ' means the - robot will move 1.2 meters forward and 0.8 meters to the right side." \ No newline at end of file diff --git a/capabilities2_runner_nav2/package.xml b/capabilities2_runner_nav2/package.xml deleted file mode 100644 index 4f32f58..0000000 --- a/capabilities2_runner_nav2/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - capabilities2_runner_nav2 - 0.0.0 - TODO: Package description - kalana - MIT - - ament_cmake - - rclcpp - rclcpp_action - pluginlib - std_msgs - nav2_msgs - geometry_msgs - capabilities2_msgs - capabilities2_runner - - - tinyxml2 - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/capabilities2_runner_nav2/plugins.xml b/capabilities2_runner_nav2/plugins.xml deleted file mode 100644 index a0d5780..0000000 --- a/capabilities2_runner_nav2/plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - A plugin that provide nav2 waypoint navigation capability - - - diff --git a/capabilities2_runner_nav2/src/nav2_runners.cpp b/capabilities2_runner_nav2/src/nav2_runners.cpp deleted file mode 100644 index a457865..0000000 --- a/capabilities2_runner_nav2/src/nav2_runners.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include -#include -#include - -namespace capabilities2_runner -{ - -} - -// register runner plugins -PLUGINLIB_EXPORT_CLASS(capabilities2_runner::WayPointRunner, capabilities2_runner::RunnerBase) diff --git a/capabilities2_runner_audio/.clang-format b/capabilities2_runner_system/.clang-format similarity index 100% rename from capabilities2_runner_audio/.clang-format rename to capabilities2_runner_system/.clang-format diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_system/CMakeLists.txt similarity index 55% rename from capabilities2_runner_nav2/CMakeLists.txt rename to capabilities2_runner_system/CMakeLists.txt index 3057740..aaec417 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_system/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.10) -project(capabilities2_runner_nav2) +cmake_minimum_required(VERSION 3.8) +project(capabilities2_runner_system) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -14,37 +14,37 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) +find_package(capabilities2_utils REQUIRED) +find_package(fabric_msgs REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor -include_directories(include -${TINYXML2_INCLUDE_DIRS} +include_directories( + include ) add_library(${PROJECT_NAME} SHARED - src/nav2_runners.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${TINYXML2_LIBRARIES} + src/capabilities2_runner.cpp ) ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action pluginlib - nav2_msgs - geometry_msgs capabilities2_msgs + fabric_msgs capabilities2_runner - TINYXML2 + capabilities2_utils + event_logger + event_logger_msgs + TinyXML2 ) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) +pluginlib_export_plugin_description_file(capabilities2_runner plugins.xml) install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} @@ -57,6 +57,14 @@ install(DIRECTORY include/ DESTINATION include ) +install(DIRECTORY interfaces + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY providers + DESTINATION share/${PROJECT_NAME} +) + ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp new file mode 100644 index 0000000..5bc8cac --- /dev/null +++ b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_all_runner.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include + +namespace capabilities2_runner +{ +class InputMultiplexAllRunner : public MultiplexBaseRunner +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + InputMultiplexAllRunner() : MultiplexBaseRunner() + { + } + + /** + * @brief trigger function to handle multiplexing of all inputs based on ALL condition + * + * @param parameters not used in this runner + */ + virtual void trigger(const std::string& parameters) override + { + tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters); + + int input_count = 0; + + parameters_->QueryIntAttribute("input_count", &input_count); + parameters_->QueryIntAttribute("id", &runner_id); + + if (input_count_tracker.find(runner_id) == input_count_tracker.end()) + { + input_count_tracker[runner_id] = 1; + expected_input_count[runner_id] = input_count; + + info_("has started the All condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs."); + } + else + { + input_count_tracker[runner_id] += 1; + + info_("has received " + std::to_string(input_count_tracker[runner_id]) + "/" + + std::to_string(expected_input_count[runner_id]) + " inputs for ALL condition."); + } + + if (input_count_tracker[runner_id] == expected_input_count[runner_id]) + { + info_("has fullfilled the All condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs."); + + executionThreadPool[runner_id] = std::thread(&InputMultiplexAllRunner::execution, this, runner_id); + } + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp new file mode 100644 index 0000000..dfc919f --- /dev/null +++ b/capabilities2_runner_system/include/capabilities2_runner_system/input_multiplex_any_runner.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include + +namespace capabilities2_runner +{ +class InputMultiplexAnyRunner : public MultiplexBaseRunner +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + InputMultiplexAnyRunner() : MultiplexBaseRunner() + { + } + + /** + * @brief trigger function to handle multiplexing of all inputs based on ANY condition + * + * @param parameters not used in this runner + */ + virtual void trigger(const std::string& parameters) override + { + info_("received new parameters for InputMultiplexAnyRunner : " + parameters); + + tinyxml2::XMLElement* parameters_ = convert_to_xml(parameters); + + int input_count = -1; + + parameters_->QueryIntAttribute("input_count", &input_count); + parameters_->QueryIntAttribute("id", &runner_id); + + if (runner_id < 0 || input_count < 0) + { + throw runner_exception("UID or input_count not found in parameters"); + } + else + { + info_("triggered with UID: " + std::to_string(runner_id) + " and input_count: " + std::to_string(input_count)); + } + + if (input_count_tracker.find(runner_id) == input_count_tracker.end()) + { + input_count_tracker[runner_id] = 1; + expected_input_count[runner_id] = input_count; + + info_("has started the ANY condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs."); + } + else + { + input_count_tracker[runner_id] += 1; + + info_("has received " + std::to_string(input_count_tracker[runner_id]) + "/" + + std::to_string(expected_input_count[runner_id]) + " inputs for ANY condition."); + } + + if (input_count_tracker[runner_id] > 0) + { + info_("has fullfilled the ANY condition with " + std::to_string(input_count_tracker[runner_id]) + " inputs."); + + executionThreadPool[runner_id] = std::thread(&InputMultiplexAnyRunner::execution, this, runner_id); + } + } +}; + +} // namespace capabilities2_runner diff --git a/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp b/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp new file mode 100644 index 0000000..10d2c61 --- /dev/null +++ b/capabilities2_runner_system/include/capabilities2_runner_system/multiplex_base_runner.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include +#include + +namespace capabilities2_runner +{ +class MultiplexBaseRunner : public RunnerBase +{ +public: + /** + * @brief Constructor which needs to be empty due to plugin semantics + */ + MultiplexBaseRunner() : RunnerBase() + { + } + + /** + * @brief Starter function for starting the action runner + * + * @param node shared pointer to the capabilities node. Allows to use ros node related functionalities + * @param run_config runner configuration loaded from the yaml file + */ + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) override + { + init_base(node, run_config); + } + + /** + * @brief Trigger process to be executed. + * + * @param id unique identifier for the execution + */ + virtual void execution(int id) + { + info_("execution started for id: " + std::to_string(id)); + + // check if the id is already completed + if (completed_executions.find(id) != completed_executions.end() && completed_executions[id]) + { + info_("execution already completed for id: " + std::to_string(id)); + return; + } + else + { + // trigger the events related to on_success state + if (events[id].on_success.interface != "") + { + event_(EventType::SUCCEEDED, id, events[id].on_success.interface, events[id].on_success.provider); + triggerFunction_(events[id].on_success.interface, update_on_success(events[id].on_success.parameters)); + } + // trigger the events related to on_failure state + else if (events[id].on_failure.interface != "") + { + event_(EventType::FAILED, id, events[id].on_failure.interface, events[id].on_failure.provider); + triggerFunction_(events[id].on_failure.interface, update_on_failure(events[id].on_failure.parameters)); + } + } + + // track the execution as completed + completed_executions[id] = true; + + info_("multiplexing complete. 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"); + + info_("stopping runner"); + } + +protected: + // input count tracker + std::map input_count_tracker; + + // expected input count + std::map expected_input_count; + + // completed executions + std::map completed_executions; +}; +} // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_system/interfaces/InputMultiplexAllRunner.yaml b/capabilities2_runner_system/interfaces/InputMultiplexAllRunner.yaml new file mode 100644 index 0000000..0ab62cc --- /dev/null +++ b/capabilities2_runner_system/interfaces/InputMultiplexAllRunner.yaml @@ -0,0 +1,13 @@ +%YAML 1.1 +--- +name: InputMultiplexAllRunner +spec_version: 1.1 +spec_type: interface +description: "This capability combines the results of all input by multiplexing events into a single interface. It allows the robot to wait or + multiple parallel processes at once until completion. This is inserted by the system itself and a decision making authority such + as an LLM does not need to include this in plans generated by it." +interface: + actions: + "empty": + type: std_srvs/action/Empty + description: empty. not used \ No newline at end of file diff --git a/capabilities2_runner_system/interfaces/InputMultiplexAnyRunner.yaml b/capabilities2_runner_system/interfaces/InputMultiplexAnyRunner.yaml new file mode 100644 index 0000000..ff4313b --- /dev/null +++ b/capabilities2_runner_system/interfaces/InputMultiplexAnyRunner.yaml @@ -0,0 +1,13 @@ +%YAML 1.1 +--- +name: InputMultiplexAnyRunner +spec_version: 1.1 +spec_type: interface +description: "This capability combines the results of any (at least one) input by multiplexing events into a single interface. It allows the + robot to wait or multiple parallel processes at once until completion. This is inserted by the system itself and a decision + making authority such as an LLM does not need to include this in plans generated by it." +interface: + actions: + "empty": + type: std_srvs/action/Empty + description: empty. not used \ No newline at end of file diff --git a/capabilities2_runner_system/package.xml b/capabilities2_runner_system/package.xml new file mode 100644 index 0000000..1af9a4e --- /dev/null +++ b/capabilities2_runner_system/package.xml @@ -0,0 +1,55 @@ + + + + capabilities2_runner_system + 0.0.0 + TODO: Package description + + + Kalana Ratnayake + Kalana Ratnayake + + Kalana Ratnayake + + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_action + pluginlib + std_msgs + capabilities2_msgs + capabilities2_runner + capabilities2_utils + fabric_msgs + event_logger + event_logger_msgs + tinyxml2_vendor + + + ament_cmake + + + + interfaces/InputMultiplexAllRunner.yaml + + + + providers/InputMultiplexAllRunner.yaml + + + + + interfaces/InputMultiplexAnyRunner.yaml + + + + providers/InputMultiplexAnyRunner.yaml + + + + diff --git a/capabilities2_runner_system/plugins.xml b/capabilities2_runner_system/plugins.xml new file mode 100644 index 0000000..bc05f3c --- /dev/null +++ b/capabilities2_runner_system/plugins.xml @@ -0,0 +1,14 @@ + + + + A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. + It can handle multiple input streams and route them to the appropriate output. + + + + + A plugin that Acts as a multiplexer for multiple input streams, allowing them to be processed in a single runner. + It can handle multiple input streams and route them to the appropriate output. + + + diff --git a/capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml b/capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml new file mode 100644 index 0000000..1720a2b --- /dev/null +++ b/capabilities2_runner_system/providers/InputMultiplexAllRunner.yaml @@ -0,0 +1,8 @@ +%YAML 1.1 +--- +name: InputMultiplexAllRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the capabilities2_runner_system/InputMultiplexAllRunner interface" +implements: capabilities2_runner_system/InputMultiplexAllRunner +runner: capabilities2_runner::InputMultiplexAllRunner \ No newline at end of file diff --git a/capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml b/capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml new file mode 100644 index 0000000..9e07449 --- /dev/null +++ b/capabilities2_runner_system/providers/InputMultiplexAnyRunner.yaml @@ -0,0 +1,8 @@ +%YAML 1.1 +--- +name: InputMultiplexAnyRunner +spec_type: provider +spec_version: 1.1 +description: "The capability provider for the capabilities2_runner_system/InputMultiplexAnyRunner interface" +implements: capabilities2_runner_system/InputMultiplexAnyRunner +runner: capabilities2_runner::InputMultiplexAnyRunner \ No newline at end of file diff --git a/capabilities2_runner_system/src/capabilities2_runner.cpp b/capabilities2_runner_system/src/capabilities2_runner.cpp new file mode 100644 index 0000000..114133e --- /dev/null +++ b/capabilities2_runner_system/src/capabilities2_runner.cpp @@ -0,0 +1,8 @@ +#include +#include +#include +#include + +// register runner plugins +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAllRunner, capabilities2_runner::RunnerBase); +PLUGINLIB_EXPORT_CLASS(capabilities2_runner::InputMultiplexAnyRunner, capabilities2_runner::RunnerBase); \ No newline at end of file diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index f219f35..bc9f429 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -14,16 +14,19 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(bondcpp REQUIRED) find_package(pluginlib REQUIRED) -find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) -# find_package(uuid REQUIRED) -# find_package(TinyXML2 REQUIRED) +find_package(capabilities2_utils REQUIRED) +find_package(event_logger REQUIRED) +find_package(event_logger_msgs REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) # provided by tinyxml2 upstream, or tinyxml2_vendor +find_package(backward_ros REQUIRED) find_package(PkgConfig) -pkg_check_modules(TINYXML2 tinyxml2 REQUIRED) pkg_check_modules(UUID REQUIRED uuid) # Find SQLite3 @@ -32,26 +35,27 @@ find_package(SQLite3) # find yaml-cpp find_package(yaml-cpp REQUIRED) -include_directories( - include +include_directories(include ${SQLITE3_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} - ${TINYXML2_INCLUDE_DIRS} ${UUID_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} SHARED - src/capabilities_server.cpp +####################################################################### +# Component based implementation that compiles as a library +####################################################################### + +add_library(${PROJECT_NAME}_comp SHARED + src/capabilities_server_component.cpp ) -target_link_libraries(${PROJECT_NAME} +target_link_libraries(${PROJECT_NAME}_comp ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} - ${TINYXML2_LIBRARIES} ${UUID_LIBRARIES} ) -ament_target_dependencies(${PROJECT_NAME} +ament_target_dependencies(${PROJECT_NAME}_comp rclcpp rclcpp_action bondcpp @@ -59,36 +63,78 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_components capabilities2_msgs capabilities2_runner + capabilities2_utils + event_logger + event_logger_msgs + TinyXML2 SQLite3 yaml-cpp - TINYXML2 UUID ) -rclcpp_components_register_node(${PROJECT_NAME} +rclcpp_components_register_node(${PROJECT_NAME}_comp PLUGIN "capabilities2_server::CapabilitiesServer" - EXECUTABLE capabilities2_server_node + EXECUTABLE capabilities2_server_component ) -ament_export_targets(capabilities2_server_node) +ament_export_targets(capabilities2_server_component) -install(TARGETS ${PROJECT_NAME} - EXPORT capabilities2_server_node +install(TARGETS ${PROJECT_NAME}_comp + EXPORT capabilities2_server_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +############################################################################ +# node implementation that compiles as a executable +############################################################################ + +add_executable(${PROJECT_NAME}_node + src/capabilities_server_node.cpp +) + +target_link_libraries(${PROJECT_NAME}_node + ${UUID_LIBRARIES} + ${SQLITE3_LIBRARIES} + ${YAML_CPP_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + rclcpp_action + bondcpp + pluginlib + capabilities2_msgs + capabilities2_runner + capabilities2_utils + event_logger + event_logger_msgs + TinyXML2 + SQLite3 + yaml-cpp + UUID +) + +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + # make test executable + add_executable(test_capabilities_server test/test.cpp) + target_link_libraries(test_capabilities_server - ${PROJECT_NAME} + ${PROJECT_NAME}_comp ${SQLITE3_LIBRARIES} ${YAML_CPP_LIBRARIES} - ${TINYXML2_LIBRARIES} ${UUID_LIBRARIES} ) -add_dependencies(test_capabilities_server ${PROJECT_NAME}) +add_dependencies(test_capabilities_server ${PROJECT_NAME}_comp) + +############################################################################ +# miscellaneous +############################################################################ # install test executable install(TARGETS test_capabilities_server diff --git a/capabilities2_server/config/capabilities.yaml b/capabilities2_server/config/capabilities.yaml index be5d1e5..b0f1a87 100644 --- a/capabilities2_server/config/capabilities.yaml +++ b/capabilities2_server/config/capabilities.yaml @@ -2,11 +2,8 @@ ros__parameters: loop_rate: 5.0 # Hz db_file: ~/.ros/capabilities/capabilities.sqlite3 - rebuild: false # Set to true to rebuild the database + rebuild: true # Set to true to rebuild the database package_paths: # paths to search for capabilities + - install/ - /opt/ros/humble/share - /opt/ros/jazzy/share - - /home/$USER/capabilities_ws/src - - /home/$USER/capabilities_ws/src/capabilities2 - - /home/ubuntu/colcon_ws/src - - /home/ubuntu/colcon_ws/src/capabilities2 diff --git a/capabilities2_server/docs/register.md b/capabilities2_server/docs/register.md new file mode 100644 index 0000000..b5e9578 --- /dev/null +++ b/capabilities2_server/docs/register.md @@ -0,0 +1,22 @@ +## Registering a capability + +Capabilities can be registered by exporting them in a package. The capabilities2 server will read the capabilities from the package and make them available to the user. Basic capability use case needs two components to be registered; `capability_interface` and `capability_provider`. Optionally, a third component can be added as `semantic_capability_interface`. + +```xml + + + + interfaces/cool_cap.yaml + + + + providers/cool_cap.yaml + + + + semantic_interfaces/not_cool_cap.yaml + + +``` + +Capabilities can also be registered through a service call. This is useful for registering capabilities that are not exported in a package. The service call has been implemented as a node in the capabilities2 package. Use the node to register capabilities during runtime. This method can also be used to update capabilities. diff --git a/capabilities2_server/docs/terminal_usage.md b/capabilities2_server/docs/terminal_usage.md new file mode 100644 index 0000000..20751db --- /dev/null +++ b/capabilities2_server/docs/terminal_usage.md @@ -0,0 +1,26 @@ +## Terminal based capability usage + +Using a capability requires the user to establish a bond with the capabilities2 server. The bond is used to maintain a persistent connection with the server. + +first, inspect the available capabilities provided under this server on this robot. + +```bash +ros2 service call /capabilities/get_capability_specs capabilities2_msgs/srv/GetCapabilitySpecs +``` + +then, request a bond id to become a persistent user + +```bash +ros2 service call /capabilities/establish_bond capabilities2_msgs/srv/EstablishBond + +# this bond needs to be updated every second by publishing a heartbeat the bond topic. Replace the ... with value received from above call +ros2 topic pub /capabilities/bonds ... +``` + +then request to use a capability + +```bash +ros2 service call /capabilities/use_capability capabilities2_msgs/srv/UseCapability +``` + +This capability can be freed by calling the `free_capability` service, or just let the bond expire. The capability will be freed automatically. diff --git a/capabilities2_server/include/capabilities2_server/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 296ddcc..4705371 100644 --- a/capabilities2_server/include/capabilities2_server/bond_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/bond_cache.hpp @@ -19,7 +19,7 @@ namespace capabilities2_server class BondCache { public: - BondCache(const std::string& bonds_topic = "~/bonds") : bonds_topic_(bonds_topic) + BondCache(const std::string& bonds_topic = "/capabilities/bond") : bonds_topic_(bonds_topic) { } @@ -71,6 +71,7 @@ class BondCache { // remove bond id from capability entry auto it = std::find(bond_cache_[capability].begin(), bond_cache_[capability].end(), bond_id); + if (it != bond_cache_[capability].end()) { bond_cache_[capability].erase(it); @@ -124,6 +125,8 @@ class BondCache live_bonds_[bond_id] = std::make_unique(bonds_topic_, bond_id, node, on_broken, on_formed); // start the bond + live_bonds_[bond_id]->setHeartbeatPeriod(0.10); + live_bonds_[bond_id]->setHeartbeatTimeout(10.0); live_bonds_[bond_id]->start(); } diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 771a595..e7a246a 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -6,19 +6,24 @@ #include #include +#include #include #include -#include #include + #include #include #include #include +#include + +#include +#include #include +#include #include -#include #include #include @@ -43,36 +48,19 @@ class CapabilitiesAPI * @brief connect with the database file * * @param db_file file path of the database file - * @param node_logging_interface_ptr pointer to the ROS node logging interface + * @param event_client pointer to the event publishing interface */ - void connect(const std::string& db_file, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr) + void connect(const std::string& db_file, std::shared_ptr event_client) { - // set logger - node_logging_interface_ptr_ = node_logging_interface_ptr; + event_ = event_client; + + runner_cache_.connect(event_client); // connect db cap_db_ = std::make_unique(db_file); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "CAPAPI connected to db: %s", db_file.c_str()); - } - - /** - * @brief initialize runner events. THese are called from within runners and passed on to those execution levels - * as function pointers - * - * @param on_started event triggered at the start of the runner - * @param on_stopped event triggered at the shutdown of the runner by the capabilities server - * @param on_terminated event triggered at the failure of the runner by the action or server side. - */ - void init_events(std::function on_started, - std::function on_stopped, - std::function on_terminated) - { - runner_cache_.set_on_started(on_started); - runner_cache_.set_on_stopped(on_stopped); - runner_cache_.set_on_terminated(on_terminated); + event_->info("Capabilities API connected to db: " + db_file); } /** @@ -81,9 +69,14 @@ class CapabilitiesAPI * @param node ros node pointer of the ros server * @param capability capability name to be started * @param provider provider of the capability + * + * @return `true` if capability started successfully. else returns `false` */ - void start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) + bool start_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider) { + // return value + bool value = true; + // get the running model from the db models::running_model_t running = cap_db_->get_running(provider); @@ -91,48 +84,49 @@ class CapabilitiesAPI // go through the running model and start the necessary dependencies for (const auto& run : running.dependencies) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "found dependency: %s", run.interface.c_str()); + event_->info("found dependency: " + run.interface); // make an internal 'use' bond for the capability dependency bind_dependency(run.interface); // add the runner to the cache - start_capability(node, run.interface, run.provider); + value = value and start_capability(node, run.interface, run.provider); } // get the provider specification for the capability models::run_config_model_t run_config = cap_db_->get_run_config(provider); - // create a new runner - // this call implicitly starts the runner + // create a new runner, this call implicitly starts the runner // create a runner id which is the cap name to uniquely identify the runner // this means only one runner per capability name + // // TODO: consider the logic for multiple runners per capability try { runner_cache_.add_runner(node, capability, run_config); - // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "started capability: %s with provider: %s", - capability.c_str(), provider.c_str()); + event_->info("started capability: " + capability + " with provider: " + provider); + + return value and true; } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not start runner: %s", e.what()); + event_->error("could not start runner: " + std::string(e.what())); + + return false; } } /** * @brief trigger a capability * - * This is a new function for a capability provider (runner) - * that is already started but has additional parameters to be triggered - * This function can be used externally. + * This is a new function for a capability provider (runner) that is already started but has + * additional parameters to be triggered. This function can be used externally. * * @param capability * @param parameters */ - void trigger_capability(const std::string& capability, std::shared_ptr parameters = nullptr) + void trigger_capability(const std::string& capability, const std::string& parameters) { // trigger the runner try @@ -141,7 +135,7 @@ class CapabilitiesAPI } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not trigger runner: %s", e.what()); + event_->error("could not trigger runner: " + std::string(e.what())); } } @@ -156,7 +150,7 @@ class CapabilitiesAPI // this can happen if dependencies fail to resolve in the first place if (!runner_cache_.running(capability)) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "could not get provider for: %s", capability.c_str()); + event_->error("could not get provider for: " + capability); return; } @@ -170,7 +164,7 @@ class CapabilitiesAPI // FIXME: this unrolls the dependency tree from the bottom up but should probably be top down for (const auto& run : running.dependencies) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "freeing dependency: %s", run.interface.c_str()); + event_->info("freeing dependency: " + run.interface + "of : " + capability); // remove the internal 'use' bond for the capability dependency unbind_dependency(run.interface); @@ -190,12 +184,12 @@ class CapabilitiesAPI } catch (const capabilities2_runner::runner_exception& e) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "could not stop runner: %s", e.what()); + event_->error("could not stop runner: " + std::string(e.what())); return; } // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "stopped capability: %s", capability.c_str()); + event_->info("stopped capability: " + capability); } /** @@ -214,7 +208,8 @@ class CapabilitiesAPI if (!bond_cache_.exists(capability)) { // stop the capability - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "stopping freed capability: %s", capability.c_str()); + event_->info("stopping freed capability: " + capability); + stop_capability(capability); } } @@ -227,15 +222,40 @@ class CapabilitiesAPI * @param capability capability name to be started * @param provider provider of the capability * @param bond_id bond_id for the capability + * + * @return `true` if capability started successfully. else returns `false` */ - void use_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, + bool use_capability(rclcpp::Node::SharedPtr node, const std::string& capability, const std::string& provider, const std::string& bond_id) { // add bond to cache for capability bond_cache_.add_bond(capability, bond_id); // start the capability with the provider - start_capability(node, capability, provider); + return start_capability(node, capability, provider); + } + + /** + * @brief Set triggers for `on_success`, `on_failure`, `on_start`, `on_stop` events for a given capability + * + * @param capability capability from where the events originate + * @param event_options event options for the capability + */ + void set_triggers(const std::string& capability, capabilities2::event_opts& event_options) + { + try + { + // log + event_->info("Setting triggers for capability: " + capability); + + runner_cache_.set_runner_triggers(capability, event_options); + + event_->info("Successfully set triggers for capability: " + capability); + } + catch (const capabilities2_runner::runner_exception& e) + { + event_->error("could not set triggers for the runner: " + std::string(e.what())); + } } // capability api @@ -252,7 +272,7 @@ class CapabilitiesAPI // exists guard if (cap_db_->exists(header.name)) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "interface already exists"); + event_->info(header.name + " interface already exists"); return; } @@ -264,7 +284,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "failed to convert spec to model: %s", e.what()); + event_->error("failed to convert spec to model: " + std::string(e.what())); return; } @@ -272,8 +292,8 @@ class CapabilitiesAPI model.header.name = spec.package + "/" + model.header.name; cap_db_->insert_interface(model); - // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "interface added to db: %s", model.header.name.c_str()); + event_->info("interface added to db: " + model.header.name); + return; } @@ -281,7 +301,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "semantic interface already exists"); + event_->info(header.name + " semantic interface already exists"); return; } @@ -293,7 +313,7 @@ class CapabilitiesAPI } catch (const std::exception& e) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "failed to convert spec to model: %s", e.what()); + event_->error("failed to convert spec to model: " + std::string(e.what())); return; } @@ -301,8 +321,8 @@ class CapabilitiesAPI cap_db_->insert_semantic_interface(model); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "semantic interface added to db: %s", - model.header.name.c_str()); + event_->info("semantic interface added to db: " + model.header.name); + return; } @@ -310,7 +330,7 @@ class CapabilitiesAPI { if (cap_db_->exists(header.name)) { - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "provider already exists"); + event_->info(header.name + " provider already exists"); return; } @@ -322,19 +342,20 @@ class CapabilitiesAPI } catch (const std::exception& e) { - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "failed to convert spec to model: %s", e.what()); + event_->error("failed to convert spec to model: " + std::string(e.what())); return; } + model.header.name = spec.package + "/" + model.header.name; cap_db_->insert_provider(model); // log - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "provider added to db: %s", model.header.name.c_str()); + event_->info("provider added to db: " + model.header.name); return; } // couldn't parse unknown capability type - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "unknown capability type: %s", spec.type.c_str()); + event_->error("unknown capability type: " + spec.type); } // query api @@ -563,104 +584,15 @@ class CapabilitiesAPI return running_capabilities; } - // event api - // related to runner api - const capabilities2_msgs::msg::CapabilityEvent on_capability_started(const std::string& capability) - { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // started event - event.type = capabilities2_msgs::msg::CapabilityEvent::LAUNCHED; - - // set cap, prov, pid - event.capability = capability; - - // try to get details - try - { - event.provider = runner_cache_.provider(capability); - event.pid = atoi(runner_cache_.pid(capability).c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - event.provider = "unknown"; - event.pid = -1; - } - - return event; - } - - const capabilities2_msgs::msg::CapabilityEvent on_capability_stopped(const std::string& capability) - { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // terminated event - event.type = capabilities2_msgs::msg::CapabilityEvent::STOPPED; - - // set cap, prov, pid - event.capability = capability; - - // try to get details - try - { - event.provider = runner_cache_.provider(capability); - event.pid = atoi(runner_cache_.pid(capability).c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - event.provider = "unknown"; - event.pid = -1; - } - - return event; - } - - const capabilities2_msgs::msg::CapabilityEvent on_capability_terminated(const std::string& capability) - { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // terminated event - event.type = capabilities2_msgs::msg::CapabilityEvent::TERMINATED; - - // set cap, prov, pid - event.capability = capability; - try - { - event.provider = runner_cache_.provider(capability); - event.pid = atoi(runner_cache_.pid(capability).c_str()); - } - catch (const capabilities2_runner::runner_exception& e) - { - event.provider = "unknown"; - event.pid = -1; - } - - // log error - RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "capability terminated: %s", capability.c_str()); - - return event; - } - - const capabilities2_msgs::msg::CapabilityEvent on_server_ready() + /** + * @brief get pid of capability + * + * @param capability capability of which the pid is requested + * @return value of pid + */ + int get_pid(const std::string& capability) { - // create event msg - capabilities2_msgs::msg::CapabilityEvent event; - event.header.frame_id = "capabilities"; - event.header.stamp = rclcpp::Clock().now(); - - // started event - event.type = capabilities2_msgs::msg::CapabilityEvent::SERVER_READY; - - return event; + return atoi(runner_cache_.pid(capability).c_str()); } // bond api @@ -682,13 +614,13 @@ class CapabilitiesAPI void on_bond_established(const std::string& bond_id) { // log bond established event - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "bond established with id: %s", bond_id.c_str()); + event_->info("bond established with id: " + bond_id); } void on_bond_broken(const std::string& bond_id) { // log warning - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "bond broken for id: %s", bond_id.c_str()); + event_->error("bond broken for id: " + bond_id); // get capabilities requested by the bond std::vector capabilities = bond_cache_.get_capabilities(bond_id); @@ -759,12 +691,12 @@ class CapabilitiesAPI } private: - // logger - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ptr_; - // db std::unique_ptr cap_db_; + // for events publishing + std::shared_ptr event_; + // caches BondCache bond_cache_; RunnerCache runner_cache_; diff --git a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp index db0a059..d48e7ec 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_db.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_db.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -475,10 +476,10 @@ class CapabilitiesDB : public DBBase interface.header.name = std::string(reinterpret_cast(sqlite3_column_text(stmt, 0))); interface.header.version = sqlite3_column_int(stmt, 1); interface.header.type = std::string(reinterpret_cast(sqlite3_column_text(stmt, 2))); - interface.header.description = std::string(reinterpret_cast(sqlite3_column_text(stmt, 3))); + interface.header.description = from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 3)))); models::specification_model_t spec; - spec.from_yaml(YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 4))))); + spec.from_yaml(YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 4)))))); interface.interface = spec; return interface; @@ -490,11 +491,11 @@ class CapabilitiesDB : public DBBase semantic_interface.header.name = std::string(reinterpret_cast(sqlite3_column_text(stmt, 0))); semantic_interface.header.version = sqlite3_column_int(stmt, 1); semantic_interface.header.type = std::string(reinterpret_cast(sqlite3_column_text(stmt, 2))); - semantic_interface.header.description = std::string(reinterpret_cast(sqlite3_column_text(stmt, 3))); + semantic_interface.header.description = from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 3)))); semantic_interface.redefines = std::string(reinterpret_cast(sqlite3_column_text(stmt, 4))); semantic_interface.global_namespace = std::string(reinterpret_cast(sqlite3_column_text(stmt, 5))); semantic_interface.remappings.from_yaml( - YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6))))); + YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6)))))); return semantic_interface; } @@ -505,11 +506,11 @@ class CapabilitiesDB : public DBBase provider.header.name = std::string(reinterpret_cast(sqlite3_column_text(stmt, 0))); provider.header.version = sqlite3_column_int(stmt, 1); provider.header.type = std::string(reinterpret_cast(sqlite3_column_text(stmt, 2))); - provider.header.description = std::string(reinterpret_cast(sqlite3_column_text(stmt, 3))); + provider.header.description = from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 3)))); provider.implements = std::string(reinterpret_cast(sqlite3_column_text(stmt, 4))); - provider.depends_on = YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 5)))) + provider.depends_on = YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 5))))) .as>(); - provider.remappings.from_yaml(YAML::Load(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6))))); + provider.remappings.from_yaml(YAML::Load(from_sql_safe(std::string(reinterpret_cast(sqlite3_column_text(stmt, 6)))))); provider.runner = std::string(reinterpret_cast(sqlite3_column_text(stmt, 7))); return provider; diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index 4012142..e5e5b30 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -7,9 +7,7 @@ #include #include #include - #include - #include #include @@ -17,14 +15,14 @@ #include -#include #include -#include #include #include #include #include +#include #include +#include #include #include #include @@ -33,7 +31,11 @@ #include #include #include -#include + +#include + +#include +#include namespace capabilities2_server { @@ -59,8 +61,31 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { public: CapabilitiesServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : Node("capabilities", options), CapabilitiesAPI() + : Node("capabilities2", options), CapabilitiesAPI() { + try + { + // Only call setup if this object is already owned by a shared_ptr + if (shared_from_this()) + { + initialize(); + } + } + catch (const std::bad_weak_ptr&) + { + // Not yet safe — probably standalone without make_shared + } + } + + /** + * @brief Initializes the capabilities server node. + * + */ + void initialize() + { + // pubs + event_ = std::make_shared(shared_from_this(), "server", "/events"); + // params interface // loop rate declare_parameter("loop_rate", 5.0); @@ -85,10 +110,11 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (rebuild) { // remove db file if it exists - RCLCPP_INFO(get_logger(), "Rebuilding capabilities database"); + event_->info("Removing the old capabilities database"); + if (std::remove(db_file.c_str()) != 0) { - RCLCPP_ERROR(get_logger(), "Error deleting file %s", db_file.c_str()); + event_->error("Error deleting database file " + db_file); } } @@ -96,11 +122,17 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI if (!std::filesystem::exists(db_path)) { // create db file path + event_->info("Creating capabilities database"); + std::filesystem::create_directories(db_path.parent_path()); } // init capabilities api - connect(db_file, get_node_logging_interface()); + event_->info("Connecting Capabilities API with Database"); + + connect(db_file, event_); + + event_->info("Loading capabilities"); // load capabilities from package paths for (const auto& package_path : package_paths) @@ -108,11 +140,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI load_capabilities(package_path); } - // pubs - // events - event_pub_ = create_publisher("~/events", 10); - - // subs + event_->info("Starting server interfaces"); // services // establish bond @@ -135,11 +163,21 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI "~/free_capability", std::bind(&CapabilitiesServer::free_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // trigger capability + trigger_capability_srv_ = create_service( + "~/trigger_capability", + std::bind(&CapabilitiesServer::trigger_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // use capability use_capability_srv_ = create_service( "~/use_capability", std::bind(&CapabilitiesServer::use_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // configure capability + configure_capability_srv_ = create_service( + "~/configure_capability", + std::bind(&CapabilitiesServer::configure_capability_cb, this, std::placeholders::_1, std::placeholders::_2)); + // register capability register_capability_srv_ = create_service( "~/register_capability", @@ -174,19 +212,8 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI "~/get_running_capabilities", std::bind(&CapabilitiesServer::get_running_capabilities_cb, this, std::placeholders::_1, std::placeholders::_2)); - // create publishing event callbacks by binding the event publisher and event message callbacks - // handled by the API class and passed around to runners - // on started, stopped, and terminated lambdas binding event_pub_ - // init events system callbacks with lambdas - init_events([this](const std::string& cap) { event_pub_->publish(on_capability_started(cap)); }, - [this](const std::string& cap) { event_pub_->publish(on_capability_stopped(cap)); }, - [this](const std::string& cap) { event_pub_->publish(on_capability_terminated(cap)); }); - - // log ready - RCLCPP_INFO(get_logger(), "capabilities server started"); - // publish ready event - event_pub_->publish(on_server_ready()); + event_->info("capabilities server start up complete"); } // service callbacks @@ -224,6 +251,17 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI res->successful = true; } + // trigger capability + void trigger_capability_cb(const std::shared_ptr req, + std::shared_ptr res) + { + // try stopping capability + // TODO: handle errors + trigger_capability(req->capability, req->parameters); + + // response is empty + } + // free capability void free_capability_cb(const std::shared_ptr req, std::shared_ptr res) @@ -231,13 +269,13 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability.empty()) { - RCLCPP_ERROR(get_logger(), "free_capability: capability is empty"); + event_->error("free_capability: capability is empty"); return; } if (req->bond_id.empty()) { - RCLCPP_ERROR(get_logger(), "free_capability: bond_id is empty"); + event_->error("free_capability: bond_id is empty"); return; } @@ -254,19 +292,19 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability.empty()) { - RCLCPP_ERROR(get_logger(), "use_capability: capability is empty"); + event_->error("use_capability: capability is empty"); return; } if (req->preferred_provider.empty()) { - RCLCPP_ERROR(get_logger(), "use_capability: preferred_provider is empty"); + event_->error("use_capability: preferred_provider is empty"); return; } if (req->bond_id.empty()) { - RCLCPP_ERROR(get_logger(), "use_capability: bond_id is empty"); + event_->error("use_capability: bond_id is empty"); return; } @@ -276,6 +314,53 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // response is empty } + // use capability + void configure_capability_cb(const std::shared_ptr req, + std::shared_ptr res) + { + capabilities2::event_opts event_options; + + event_options.event_id = req->trigger_id; + + event_options.on_started.interface = req->target_on_start.capability; + event_options.on_started.provider = req->target_on_start.provider; + event_options.on_started.parameters = req->target_on_start.parameters; + + event_->runner_define(req->source.capability, req->source.provider, req->target_on_start.capability, + req->target_on_start.provider, event_logger_msgs::msg::Event::STARTED, req->connection_description); + + event_options.on_failure.interface = req->target_on_failure.capability; + event_options.on_failure.provider = req->target_on_failure.provider; + event_options.on_failure.parameters = req->target_on_failure.parameters; + + event_->runner_define(req->source.capability, req->source.provider, req->target_on_failure.capability, + req->target_on_failure.provider, event_logger_msgs::msg::Event::FAILED, req->connection_description); + + event_options.on_success.interface = req->target_on_success.capability; + event_options.on_success.provider = req->target_on_success.provider; + event_options.on_success.parameters = req->target_on_success.parameters; + + event_->runner_define(req->source.capability, req->source.provider, req->target_on_success.capability, + req->target_on_success.provider, event_logger_msgs::msg::Event::SUCCEEDED, req->connection_description); + + event_options.on_stopped.interface = req->target_on_stop.capability; + event_options.on_stopped.provider = req->target_on_stop.provider; + event_options.on_stopped.parameters = req->target_on_stop.parameters; + + event_->runner_define(req->source.capability, req->source.provider, req->target_on_stop.capability, + req->target_on_stop.provider, event_logger_msgs::msg::Event::STOPPED, req->connection_description); + + event_->info("on_started : " + event_options.on_started.parameters); + event_->info("on_failure : " + event_options.on_failure.parameters); + event_->info("on_success : " + event_options.on_success.parameters); + event_->info("on_stopped : " + event_options.on_stopped.parameters); + + // setup triggers between parameters + set_triggers(req->source.capability, event_options); + + // response is empty + } + // register capability void register_capability_cb(const std::shared_ptr req, std::shared_ptr res) @@ -283,7 +368,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // guard empty values if (req->capability_spec.package.empty() || req->capability_spec.content.empty()) { - RCLCPP_ERROR(get_logger(), "register_capability: package or content is empty"); + event_->error("register_capability: package or content is empty"); return; } @@ -333,7 +418,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // if the spec is not empty set response if (capability_spec.content.empty()) { - RCLCPP_ERROR(get_logger(), "capability spec not found for resource: %s", req->capability_spec.c_str()); + event_->error("capability spec not found for resource: " + req->capability_spec); // BUG: throw error causes service to crash, this is a ROS2 bug // std::runtime_error("capability spec not found for resource: " + req->capability_spec); @@ -374,59 +459,74 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI } private: - /** - * @brief Load capabilities from a package path - * - * @param package_path - */ void load_capabilities(const std::string& package_path) { - RCLCPP_DEBUG(get_logger(), "Loading capabilities from package path: %s", package_path.c_str()); + event_->debug("Loading capabilities from package path: " + package_path); + // check if path exists if (!std::filesystem::exists(package_path)) { - RCLCPP_ERROR(get_logger(), "package path does not exist: %s", package_path.c_str()); + event_->error("package path does not exist: " + package_path); return; } + // find packages in path - std::vector packages; + std::vector packages_root, packages_install; + for (const auto& entry : std::filesystem::directory_iterator(package_path)) { if (entry.is_directory()) { - packages.push_back(entry.path().filename()); + std::string name(entry.path().filename()); + + if (std::filesystem::exists(package_path + "/" + name + "/package.xml")) + { + packages_root.push_back(name); + } + else if (std::filesystem::exists(package_path + "/" + name + "/share/" + name + "/package.xml")) + { + packages_install.push_back(name); + } } } - // load capabilities from packages - for (const auto& package : packages) + + // load capabilities from packages in /opt/ros/*/share + for (const auto& package : packages_root) { - RCLCPP_DEBUG(get_logger(), "loading capabilities from package: %s", package.c_str()); + event_->debug("Loading capabilities from package: " + package); + // package.xml exports std::string package_xml = package_path + "/" + package + "/package.xml"; + // check if package.xml exists if (!std::filesystem::exists(package_xml)) { - RCLCPP_DEBUG(get_logger(), "package.xml does not exist: %s", package_xml.c_str()); + event_->error("package.xml does not exist: " + package_xml); continue; } + // parse package.xml tinyxml2::XMLDocument doc; + try { parse_package_xml(package_xml, doc); } catch (const std::runtime_error& e) { - RCLCPP_ERROR(get_logger(), "failed to parse package.xml file: %s", e.what()); + event_->error("failed to parse package.xml file: " + std::string(e.what())); continue; } + // get exports tinyxml2::XMLElement* exports = doc.FirstChildElement("package")->FirstChildElement("export"); + if (exports == nullptr) { - RCLCPP_DEBUG(get_logger(), "No exports found in package.xml file: %s", package_xml.c_str()); + event_->error("No exports found in package.xml file: " + package_xml); continue; } + // get capability specs of each type using a lambda auto get_capability_specs = [&](const std::string& spec_type) { // get capability spec @@ -452,16 +552,102 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { // read spec file load_spec_content(package_path + "/" + package + "/" + spec_path, capability_spec); + // add capability to db - RCLCPP_INFO(get_logger(), "adding capability: %s", (package + "/" + spec_path).c_str()); + event_->info("adding capability: " + package + "/" + spec_path); + add_capability(capability_spec); } catch (const std::runtime_error& e) { - RCLCPP_ERROR(get_logger(), "failed to load spec file: %s", e.what()); + event_->error("failed to load spec file: " + std::string(e.what())); } } }; + + // get interface specs + get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_INTERFACE); + // get semantic interface specs + get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::SEMANTIC_CAPABILITY_INTERFACE); + // get provider specs + get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_PROVIDER); + } + + // load capabilities from packages in workspace install folder + for (const auto& package : packages_install) + { + event_->debug("Loading capabilities from package: " + package); + + // package.xml exports + std::string package_xml = package_path + "/" + package + "/share/" + package + "/package.xml"; + + // check if package.xml exists + if (!std::filesystem::exists(package_xml)) + { + event_->error("package.xml does not exist: " + package_xml); + continue; + } + + // parse package.xml + tinyxml2::XMLDocument doc; + + try + { + parse_package_xml(package_xml, doc); + } + catch (const std::runtime_error& e) + { + event_->error("failed to parse package.xml file: " + std::string(e.what())); + continue; + } + + // get exports + tinyxml2::XMLElement* exports = doc.FirstChildElement("package")->FirstChildElement("export"); + + if (exports == nullptr) + { + event_->error("No exports found in package.xml file: " + package_xml); + continue; + } + + // get capability specs of each type using a lambda + auto get_capability_specs = [&](const std::string& spec_type) { + // get capability spec + for (tinyxml2::XMLElement* spec = exports->FirstChildElement(spec_type.c_str()); spec != nullptr; + spec = spec->NextSiblingElement(spec_type.c_str())) + { + // read spec relative path for spec file from element contents + std::string spec_path = spec->GetText(); + // clear white spaces + spec_path.erase(std::remove_if(spec_path.begin(), spec_path.end(), isspace), spec_path.end()); + // remove leading slash + if (spec_path[0] == '/') + { + spec_path = spec_path.substr(1); + } + // create a spec message + capabilities2_msgs::msg::CapabilitySpec capability_spec; + // add package details + capability_spec.package = package; + capability_spec.type = spec_type; + // try load spec file + try + { + // read spec file + load_spec_content(package_path + "/" + package + "/share/" + package + "/" + spec_path, capability_spec); + + // add capability to db + event_->info("adding capability: " + package + "/" + spec_path); + + add_capability(capability_spec); + } + catch (const std::runtime_error& e) + { + event_->error("failed to load spec file: " + std::string(e.what())); + } + } + }; + // get interface specs get_capability_specs(capabilities2_msgs::msg::CapabilitySpec::CAPABILITY_INTERFACE); // get semantic interface specs @@ -527,12 +713,9 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI // loop hz double loop_hz_; - /** capabilities_fabric launch thread */ - std::shared_ptr fabric_launch_thread; - // publishers - // event publisher - rclcpp::Publisher::SharedPtr event_pub_; + /** Event client for publishing events */ + std::shared_ptr event_; // services // establish bond @@ -543,6 +726,10 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI rclcpp::Service::SharedPtr stop_capability_srv_; // free capability rclcpp::Service::SharedPtr free_capability_srv_; + // free capability + rclcpp::Service::SharedPtr configure_capability_srv_; + // free capability + rclcpp::Service::SharedPtr trigger_capability_srv_; // use capability rclcpp::Service::SharedPtr use_capability_srv_; // register capability @@ -561,9 +748,6 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI rclcpp::Service::SharedPtr get_remappings_srv_; // get running capabilities rclcpp::Service::SharedPtr get_running_capabilities_srv_; - - /** action server that exposes cabapilities fabric*/ - std::shared_ptr> capabilities_fabric_server; }; } // namespace capabilities2_server diff --git a/capabilities2_server/include/capabilities2_server/models/header.hpp b/capabilities2_server/include/capabilities2_server/models/header.hpp index 6edd01d..7dc9738 100644 --- a/capabilities2_server/include/capabilities2_server/models/header.hpp +++ b/capabilities2_server/include/capabilities2_server/models/header.hpp @@ -2,6 +2,7 @@ #include #include +#include namespace capabilities2_server { @@ -70,7 +71,7 @@ struct header_model_t : identifiable_base_t, soft_deleteable_base_t, modifiable_ const std::string to_sql_values() const { - return "'" + name + "', '" + version + "', '" + type + "', '" + description + "'"; + return "'" + name + "', '" + version + "', '" + type + "', '" + to_sql_safe(description) + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/models/interface.hpp b/capabilities2_server/include/capabilities2_server/models/interface.hpp index ba5b847..f6a47b0 100644 --- a/capabilities2_server/include/capabilities2_server/models/interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/interface.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -164,7 +165,7 @@ struct interface_model_t : public predicateable_base_t const std::string to_sql_values() const { - return header.to_sql_values() + ", '" + YAML::Dump(interface.to_yaml()) + "'"; + return header.to_sql_values() + ", '" + to_sql_safe(YAML::Dump(interface.to_yaml())) + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/models/provider.hpp b/capabilities2_server/include/capabilities2_server/models/provider.hpp index 1c20da6..fa1088b 100644 --- a/capabilities2_server/include/capabilities2_server/models/provider.hpp +++ b/capabilities2_server/include/capabilities2_server/models/provider.hpp @@ -6,6 +6,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -75,8 +76,9 @@ struct provider_model_t : public remappable_base_t, predicateable_base_t, public { YAML::Node deps; deps["depends_on"] = depends_on; - return header.to_sql_values() + ", '" + implements + "', '" + YAML::Dump(deps["depends_on"]) + "', '" + - YAML::Dump(remappings.to_yaml()) + "', '" + runner + "', '" + definition_str + "'"; + + return header.to_sql_values() + ", '" + implements + "', '" + to_sql_safe(YAML::Dump(deps["depends_on"])) + "', '" + + to_sql_safe(YAML::Dump(remappings.to_yaml())) + "', '" + runner + "', '" + definition_str + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp index 482ad13..0a727bd 100644 --- a/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp +++ b/capabilities2_server/include/capabilities2_server/models/semantic_interface.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -58,7 +59,7 @@ struct semantic_interface_model_t : public remappable_base_t, predicateable_base std::string to_sql_values() const { return header.to_sql_values() + ", '" + redefines + "', '" + global_namespace + "', '" + - YAML::Dump(remappings.to_yaml()) + "'"; + to_sql_safe(YAML::Dump(remappings.to_yaml())) + "'"; } }; diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index d50b412..db7c9fe 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -9,6 +9,7 @@ #include #include #include +#include namespace capabilities2_server { @@ -29,9 +30,16 @@ class RunnerCache public: RunnerCache() : runner_loader_("capabilities2_runner", "capabilities2_runner::RunnerBase") { - on_started = nullptr; - on_stopped = nullptr; - on_terminated = nullptr; + } + + /** + * @brief connect with event interface + * + * @param event_client pointer to the event client + */ + void connect(std::shared_ptr event_client) + { + event_ = event_client; } /** @@ -44,12 +52,10 @@ class RunnerCache void add_runner(rclcpp::Node::SharedPtr node, const std::string& capability, const models::run_config_model_t& run_config) { - // if the runner exists then throw an error + // if the runner exists then throw an error preserving uniqueness if (running(capability)) { - // already running throw capabilities2_runner::runner_exception("capability is running already: " + capability); - // return; } // check if run config is valid @@ -58,9 +64,8 @@ class RunnerCache throw capabilities2_runner::runner_exception("run config is not valid: " + YAML::Dump(run_config.to_yaml())); } - // create the runner - // add the runner to map - // if the spec runner contains a path to a launch file then use the launch file runner + // create the runner, add the runner to map, and if the spec runner contains a path to a launch file then use the + // launch file runner if (run_config.runner.find(".launch") != std::string::npos || run_config.runner.find("/") != std::string::npos || run_config.runner.find(".py") != std::string::npos) { @@ -68,12 +73,11 @@ class RunnerCache } else { - // use different runner types based on cap and provider specs runner_cache_[capability] = runner_loader_.createSharedInstance(run_config.runner); } // start the runner - runner_cache_[capability]->start(node, run_config.to_runner_opts(), on_started, on_terminated, on_stopped); + runner_cache_[capability]->start(node, run_config.to_runner_opts()); } /** @@ -84,7 +88,7 @@ class RunnerCache * @param capability capability name to be loaded * @param parameters parameters related to the runner in std::string form for compatibility accross various runners */ - void trigger_runner(const std::string& capability, std::shared_ptr parameters = nullptr) + void trigger_runner(const std::string& capability, const std::string& parameters) { // is the runner in the cache if (running(capability)) @@ -93,10 +97,28 @@ class RunnerCache } else { + event_->error("Runner not found for capability: " + capability); throw capabilities2_runner::runner_exception("capability runner not found: " + capability); } } + /** + * @brief Set triggers for `on_success`, `on_failure`, `on_start`, `on_stop` events + * + * + * @param capability capability from where the events originate + * @param on_started on_start event with capability and parameters + * @param on_failure on_failure event with capability and parameters + * @param on_success on_success event with capability and parameters + * @param on_stopped on_stop event with capability and parameters + */ + void set_runner_triggers(const std::string& capability, capabilities2::event_opts& event_options) + { + runner_cache_[capability]->attach_events(event_options, + std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, + std::placeholders::_1, std::placeholders::_2)); + } + /** * @brief Remove a given runner * @@ -104,12 +126,10 @@ class RunnerCache */ void remove_runner(const std::string& capability) { - // find the runner in the cache + // find the runner in the cache and if not found then throw an error if (!running(capability)) { - // not found so nothing to do throw capabilities2_runner::runner_exception("capability runner not found: " + capability); - // return; } // safely stop the runner @@ -124,10 +144,10 @@ class RunnerCache } // reset the runner pointer - runner_cache_[capability].reset(); + // runner_cache_[capability].reset(); // remove the runner from map - runner_cache_.erase(capability); + // runner_cache_.erase(capability); } /** @@ -199,36 +219,6 @@ class RunnerCache return runner_cache_.find(capability) != runner_cache_.end(); } - /** - * @brief Callback function for 'on_started' event - * - * @param cb callback function pointer - */ - void set_on_started(std::function cb) - { - on_started = cb; - } - - /** - * @brief Callback function for 'on_stopped' event - * - * @param cb callback function pointer - */ - void set_on_stopped(std::function cb) - { - on_stopped = cb; - } - - /** - * @brief Callback function for 'on_terminated' event - * - * @param cb callback function pointer - */ - void set_on_terminated(std::function cb) - { - on_terminated = cb; - } - private: // map capability to running model // capability / provider specs -> runner @@ -237,10 +227,11 @@ class RunnerCache // runner plugin loader pluginlib::ClassLoader runner_loader_; - // event callbacks - std::function on_started; - std::function on_stopped; - std::function on_terminated; + // for events publishing + std::shared_ptr event_; + + // event string + std::string event; }; } // namespace capabilities2_server diff --git a/capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp b/capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp new file mode 100644 index 0000000..0f7c2c0 --- /dev/null +++ b/capabilities2_server/include/capabilities2_server/utils/sql_safe.hpp @@ -0,0 +1,47 @@ +#pragma once +#include +#include + +namespace capabilities2_server +{ + +/** + * @brief convert a generic std::string to a sql safe std::string + * + * @param input generic std::string + * @return sql safe std::string + * + */ +std::string to_sql_safe(const std::string& input) +{ + std::string result = input; + + // Escape backslashes (if necessary for your SQL dialect) + result = std::regex_replace(result, std::regex(R"(\\)"), R"(\\\\)"); + + // Escape single quotes by replacing them with two single quotes + result = std::regex_replace(result, std::regex(R"(')"), R"('')"); + + return result; +} + +/** + * @brief convert a sql safe std::string to a generic std::string + * + * @param input sql safe std::string + * @return generic std::string + * + */ +std::string from_sql_safe(const std::string& input) +{ + std::string result = input; + + // Escape backslashes (if necessary for your SQL dialect) + result = std::regex_replace(result, std::regex(R"(\\\\)"), R"(\\)"); + + // Escape single quotes by replacing them with two single quotes + result = std::regex_replace(result, std::regex(R"('')"), R"(')"); + + return result; +} +} // namespace capabilities2_server diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py new file mode 100644 index 0000000..0ef7310 --- /dev/null +++ b/capabilities2_server/launch/server.launch.py @@ -0,0 +1,37 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 server + """ + # load config file + server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') + + # create bridge composition + capabilities2 = Node( + package='capabilities2_server', + executable='capabilities2_server_node', + name='capabilities', + parameters=[server_config], + output='screen', + arguments=['--ros-args', '--log-level', 'info'] + ) + + # return + return LaunchDescription([ + capabilities2 + ]) + + diff --git a/capabilities2_server/launch/capabilities2_server.launch.py b/capabilities2_server/launch/server_composed.launch.py similarity index 68% rename from capabilities2_server/launch/capabilities2_server.launch.py rename to capabilities2_server/launch/server_composed.launch.py index 759947b..498f824 100644 --- a/capabilities2_server/launch/capabilities2_server.launch.py +++ b/capabilities2_server/launch/server_composed.launch.py @@ -17,37 +17,27 @@ def generate_launch_description(): LaunchDescription: The launch description for capabilities2 server """ # load config file - config = os.path.join( - get_package_share_directory('capabilities2_server'), - 'config', - 'capabilities.yaml' - ) + server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') # create bridge composition capabilities = ComposableNodeContainer( name='capabilities2_container', namespace='', package='rclcpp_components', - executable='component_container', + executable='component_container_mt', + # prefix=['xterm -e gdb -ex run --args'], # Add GDB debugging prefix + arguments=['--ros-args', '--log-level', 'info'], composable_node_descriptions=[ ComposableNode( package='capabilities2_server', plugin='capabilities2_server::CapabilitiesServer', name='capabilities', - parameters=[config] + parameters=[server_config] ) ] ) - # create launch proxy node - launch_proxy = Node( - package='capabilities2_launch_proxy', - executable='capabilities_launch_proxy', - name='capabilities_launch_proxy' - ) - # return return LaunchDescription([ - capabilities, - launch_proxy + capabilities ]) diff --git a/capabilities2_server/launch/system.launch.py b/capabilities2_server/launch/system.launch.py new file mode 100644 index 0000000..8b7c0f0 --- /dev/null +++ b/capabilities2_server/launch/system.launch.py @@ -0,0 +1,20 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + + navstack_launch_path = os.path.join(get_package_share_directory('nav_stack'), 'launch', 'system.launch.py') + capabilities_launch_path = os.path.join(get_package_share_directory('capabilities2_server'), 'launch', 'server.launch.py') + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(navstack_launch_path), + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(capabilities_launch_path), + ) + ]) \ No newline at end of file diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index ffa1e48..3efffc8 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -8,6 +8,7 @@ Michael Pritchard mik-p + Kalana Ratnayake Michael Pritchard @@ -22,12 +23,16 @@ rclcpp_components capabilities2_msgs capabilities2_runner + capabilities2_utils + event_logger + event_logger_msgs + tinyxml2_vendor yaml-cpp - tinyxml2 libsqlite3-dev uuid + backward_ros rclpy launch_ros diff --git a/capabilities2_server/readme.md b/capabilities2_server/readme.md index 62cb0db..4b81b3a 100644 --- a/capabilities2_server/readme.md +++ b/capabilities2_server/readme.md @@ -6,6 +6,19 @@ Capabilities2 server allows interaction with the capabilities2 API. It is a reim 2. Capability registration through service calls. 3. Abstracted providers through a *runners* API see [capabilities2_runner](../capabilities2_runner/readme.md) +## Specialising the capabilities2 server + +The capabilities2 server is implemented as a [ROS2 Component](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Composition.html). The server can be specialised by composing it with another Component that adds domain logic. + +## Launch capabilities2 server + +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +Refer `capabilities2_server/config/capabilities.yaml` for launch arguments. + ## System dependencies The capabilities2 server depends on the following `bondcpp` ROS2 package. See the package.xml for the full list of dependencies. Notably, the capabilities2 server depends on the following system dependencies: @@ -17,7 +30,7 @@ The capabilities2 server depends on the following `bondcpp` ROS2 package. See th ## API -The capabilities2 server provides the following ROS API: +The capabilities2 server provides the following ROS2 API: ### Services @@ -38,6 +51,8 @@ The capabilities2 server exposes the following Service API (see [capabilities2_m | `~/start_capability` | `StartCapability.srv` | Start a capability (this is a forceful start, and ignores use and free logic) | | `~/stop_capability` | `StopCapability.srv` | Stop a capability (this is a forceful stop, and ignores use and free logic) | | `~/register_capability` | `RegisterCapability.srv` | Register a capability with the capabilities server | +| `~/trigger_capability` | `TriggerCapability.srv` | Trigger a capability | +| `~/configure_capability` | `ConfigureCapability.srv` | Configure a capability with `on_start`, `on_end`, `on_success`, `on_failure` events| ### Topics @@ -49,84 +64,9 @@ The capabilities2 server exposes the following Topics API: | `~/events` | `GetInterfaces.srv` | Publish capability events | | `~/bonds` | `GetSemanticInterfaces.srv` | Maintain bonds with capability users - [Bond API](https://wiki.ros.org/bond) | -## How to use - -### Register a capability - -Capabilities can be registered by exporting them in a package. The capabilities2 server will read the capabilities from the package and make them available to the user. - -```xml - - - - interfaces/cool_cap.yaml - - -``` - -Following is the content of the cool_cap.yaml - -```yaml -# cool_cap.yaml -name: cool_cap -spec_version: 1 -spec_type: interface -description: "This is a cool capability" -interface: - topics: - "/cool_topic": - type: "std_msgs/msg/String" - description: "This is a cool topic" -``` - -Capabilities can also be registered through a service call. This is useful for registering capabilities that are not exported in a package. The service call has been implemented as a node in the capabilities2 package. Use the node to register capabilities during runtime. This method can also be used to update capabilities. - -### Launch capabilities2 server - -```bash -ros2 launch capabilities2_server capabilities2_server.launch.py -``` - -Can add launch args for path to packages for exported capabilties via terminal as well. refer `./config/capabilities.yaml` for example usage. - - -### Terminal based capability usage +
-Using a capability requires the user to establish a bond with the capabilities2 server. The bond is used to maintain a persistent connection with the server. +## Additional Information -first, inspect the available capabilities provided under this server on this robot. - -```bash -ros2 service call /capabilities/get_capability_specs capabilities2_msgs/srv/GetCapabilitySpecs -``` - -then, request a bond id to become a persistent user - -```bash -ros2 service call /capabilities/establish_bond capabilities2_msgs/srv/EstablishBond - -# this bond needs to be updated every second by publishing a heartbeat the bond topic. Replace the ... with value received from above call -ros2 topic pub /capabilities/bonds ... -``` - -then request to use a capability - -```bash -ros2 service call /capabilities/use_capability capabilities2_msgs/srv/UseCapability -``` - -This capability can be freed by calling the `free_capability` service, or just let the bond expire. The capability will be freed automatically. - - -## Developing - -A `devcontainer` is provided for developing the capabilities2 meta-package. Dependencies need to be installed in the container. Install the dependencies with rosdep: - -```bash -# in the devcontainer -rosdep install --from-paths src --ignore-src -r -y -``` - -### Specialising the capabilities2 server - -The capabilities2 server is implemented as a [ROS2 Component](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Composition.html). The server can be specialised by composing it with another Component that adds domain logic. +- [Registering a capability](../capabilities2_server/docs/register.md) +- [Terminal based capability usage](../capabilities2_server/docs/terminal_usage.md) diff --git a/capabilities2_server/src/capabilities_server.cpp b/capabilities2_server/src/capabilities_server_component.cpp similarity index 94% rename from capabilities2_server/src/capabilities_server.cpp rename to capabilities2_server/src/capabilities_server_component.cpp index 8c8eb6f..efbad0e 100644 --- a/capabilities2_server/src/capabilities_server.cpp +++ b/capabilities2_server/src/capabilities_server_component.cpp @@ -1,4 +1,4 @@ #include #include -RCLCPP_COMPONENTS_REGISTER_NODE(capabilities2_server::CapabilitiesServer) +RCLCPP_COMPONENTS_REGISTER_NODE(capabilities2_server::CapabilitiesServer) \ No newline at end of file diff --git a/capabilities2_server/src/capabilities_server_node.cpp b/capabilities2_server/src/capabilities_server_node.cpp new file mode 100644 index 0000000..2817989 --- /dev/null +++ b/capabilities2_server/src/capabilities_server_node.cpp @@ -0,0 +1,27 @@ +#include + +int main(int argc, char** argv) +{ + // Initialize ROS 2 + rclcpp::init(argc, argv); + + // Create the node object and executor object + auto node = std::make_shared(); + + // Initialize the node + node->initialize(); + + // Create a MultiThreadedExecutor + auto exec = std::make_shared(); + + // Add the node to the executor + exec->add_node(node); + + // Spin the executor + exec->spin(); + + // Shutdown ROS 2 + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/capabilities2_utils/CMakeLists.txt b/capabilities2_utils/CMakeLists.txt new file mode 100644 index 0000000..db84b39 --- /dev/null +++ b/capabilities2_utils/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(capabilities2_utils) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_package() diff --git a/capabilities2_utils/include/capabilities2_utils/bond_client.hpp b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp new file mode 100644 index 0000000..3db48fd --- /dev/null +++ b/capabilities2_utils/include/capabilities2_utils/bond_client.hpp @@ -0,0 +1,95 @@ +#pragma once +#include +#include +#include +#include + +class BondClient +{ +public: + /** + * @brief Construct a new Bond Client object + * + * @param node pointer to the node + * @param event_client pointer to the event client + * @param bond_id Bond id string + * @param bonds_topic Bond topic to be published + */ + BondClient(rclcpp::Node::SharedPtr node, std::shared_ptr event_client, const std::string &bond_id, const std::string &bonds_topic = "/capabilities/bond") + { + topic_ = bonds_topic; + bond_id_ = bond_id; + node_ = node; + event_ = event_client; + } + + /** + * @brief start the bond + * + */ + void start() + { + event_->info("[BondClient] creating bond to capabilities server"); + + bond_ = std::make_unique(topic_, bond_id_, node_, std::bind(&BondClient::on_broken, this), std::bind(&BondClient::on_formed, this)); + + bond_->setHeartbeatPeriod(0.10); + bond_->setHeartbeatTimeout(10.0); + bond_->start(); + } + + /** + * @brief stop the bond + * + */ + void stop() + { + event_->info("[BondClient] destroying bond to capabilities server"); + + if (bond_) + { + bond_.reset(); + } + } + + ~BondClient() + { + stop(); + } + +private: + /** + * @brief callback function for bond formed event + * + */ + void on_formed() + { + // log bond established event + event_->info("[BondClient] bond with capabilities server formed with id: " + bond_id_); + } + + /** + * @brief callback function for bond broken event + * + */ + void on_broken() + { + // log bond established event + event_->info("[BondClient] bond with capabilities server broken with id: " + bond_id_); + } + + /** Ros node pointer */ + rclcpp::Node::SharedPtr node_; + + /** Bond id string */ + std::string bond_id_; + + /** Bond topic to be published */ + std::string topic_; + + /** Heart beat bond with capabilities server */ + std::shared_ptr bond_; + + /** Event client for publishing events */ + std::shared_ptr event_; +}; \ No newline at end of file diff --git a/capabilities2_utils/include/capabilities2_utils/connection.hpp b/capabilities2_utils/include/capabilities2_utils/connection.hpp new file mode 100644 index 0000000..4d8acca --- /dev/null +++ b/capabilities2_utils/include/capabilities2_utils/connection.hpp @@ -0,0 +1,32 @@ +#pragma once +#include +#include + +namespace capabilities2 +{ + enum connection_type_t + { + ON_START, + ON_SUCCESS, + ON_FAILURE, + ON_STOP + }; + + struct connection_t + { + std::string runner = ""; + std::string provider = ""; + tinyxml2::XMLElement* parameters = nullptr; + }; + + struct node_t { + connection_t source; + connection_t target_on_start; + connection_t target_on_stop; + connection_t target_on_success; + connection_t target_on_failure; + std::string connection_description; + int trigger_id = -1; + }; + +} // namespace capabilities2 diff --git a/capabilities2_utils/include/capabilities2_utils/event_types.hpp b/capabilities2_utils/include/capabilities2_utils/event_types.hpp new file mode 100644 index 0000000..9f756ef --- /dev/null +++ b/capabilities2_utils/include/capabilities2_utils/event_types.hpp @@ -0,0 +1,47 @@ +#pragma once +#include + +namespace capabilities2 +{ +enum event_t +{ + IDLE, + STARTED, + STOPPED, + FAILED, + SUCCEEDED +}; + +/** + * @brief event definition + * + * Contains the informations about the event to be executed. It contains the interface, provider and parameters + */ +struct event +{ + std::string interface; + std::string provider; + std::string parameters; +}; + +/** + * @brief event options + * + * keeps track of events that are related to runner instances at various points of the + * plan + * @param event_id unique identifier for the event + * @param on_started information about the capability to execute on start + * @param on_success information about the capability to execute on success + * @param on_failure information about the capability to execute on failure + * @param on_stopped information about the capability to execute on stop + */ +struct event_opts +{ + int event_id = -1; + event on_started; + event on_success; + event on_failure; + event on_stopped; +}; + +} \ No newline at end of file diff --git a/capabilities2_executor/package.xml b/capabilities2_utils/package.xml similarity index 57% rename from capabilities2_executor/package.xml rename to capabilities2_utils/package.xml index d8d8132..f6296bd 100644 --- a/capabilities2_executor/package.xml +++ b/capabilities2_utils/package.xml @@ -1,21 +1,18 @@ - capabilities2_executor + capabilities2_utils 0.0.0 TODO: Package description - kalana - MIT + Kalana Ratnayake + Kalana Ratnayake - ament_cmake - - rclcpp - capabilities2_msgs - rclcpp_action + Kalana Ratnayake + + TODO: License declaration - - tinyxml2 + ament_cmake ament_lint_auto ament_lint_common diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..7fec305 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,100 @@ +FROM ros:jazzy-ros-base-noble as base + +## Parameters +ENV WORKSPACE_ROOT=/capabilities2 + +ARG GITHUB_USERNAME +ARG GITHUB_PAT + +############################################################################################################################# +##### +##### Install Dependencies +##### +############################################################################################################################# + +WORKDIR / + +RUN apt-get update -y +RUN apt-get install -y --no-install-recommends ros-dev-tools \ + ros-$ROS_DISTRO-tf2-eigen \ + ros-$ROS_DISTRO-rmw-cyclonedds-cpp \ + ros-$ROS_DISTRO-navigation2 \ + ros-$ROS_DISTRO-nav2-bringup \ + ros-$ROS_DISTRO-slam-toolbox + +RUN apt-get clean + +RUN git clone https://github.com/leethomason/tinyxml2.git + +WORKDIR /tinyxml2 +RUN make install + + +############################################################################################################################# +##### +##### Clone packages and setup dependencies +##### +############################################################################################################################# + +WORKDIR ${WORKSPACE_ROOT}/src + +RUN git clone -b capabilities2-server-fabric https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/capabilities2.git +RUN git clone -b develop https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/std_capabilities.git +RUN git clone https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/nav_stack.git +RUN git clone -b ros2/devel https://${GITHUB_USERNAME}:${GITHUB_PAT}@github.com/CollaborativeRoboticsLab/prompt_tools.git +RUN git clone https://github.com/CollaborativeRoboticsLab/hri_audio_msgs.git + +RUN rm /etc/ros/rosdep/sources.list.d/20-default.list + +RUN rosdep init && rosdep update && rosdep install --from-paths ${WORKSPACE_ROOT}/src -y --ignore-src + + +############################################################################################################################# +##### +##### Build Capabilities packages +##### +############################################################################################################################# + +WORKDIR ${WORKSPACE_ROOT} + +RUN . /opt/ros/jazzy/setup.sh && colcon build + +WORKDIR / + +############################################################################################################################# +##### +##### Remove workspace source and build files that are not relevent to running the system +##### +############################################################################################################################# + +RUN rm -rf ${WORKSPACE_ROOT}/src +RUN rm -rf ${WORKSPACE_ROOT}/log +RUN rm -rf ${WORKSPACE_ROOT}/build + +RUN rm -rf /var/lib/apt/lists/* +RUN rm -rf /tmp/* +RUN apt-get clean + + +#--------------------------------------------------------------------------------------------------------------------------- +#---- +#---- Start final release image +#---- +#--------------------------------------------------------------------------------------------------------------------------- + + +FROM ros:jazzy-ros-base-noble as final + +## Parameters +ENV WORKSPACE_ROOT=/capabilities2 +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +COPY --from=base / / + +COPY docker/workspace_entrypoint.sh /workspace_entrypoint.sh + +RUN chmod +x /workspace_entrypoint.sh + +ENTRYPOINT [ "/workspace_entrypoint.sh" ] + +CMD ["/bin/sh", "-c", "bash"] \ No newline at end of file diff --git a/docker/compose.yaml b/docker/compose.yaml new file mode 100644 index 0000000..0ee4eea --- /dev/null +++ b/docker/compose.yaml @@ -0,0 +1,8 @@ +services: + capabilities2: + image: ghcr.io/collaborativeroboticslab/capabilities2:jazzy + restart: unless-stopped + privileged: true + network_mode: host + volumes: + - /dev:/dev \ No newline at end of file diff --git a/docker/docs/startup.md b/docker/docs/startup.md new file mode 100644 index 0000000..7557655 --- /dev/null +++ b/docker/docs/startup.md @@ -0,0 +1,29 @@ +## Docker based startup information + +### Setup the Capabilities2 container + +Pull and start the docker container + +```bash +cd src/capabilities2/docker +docker compose pull +docker compose up -d +``` + +### Starting the Capabilities2 server + +On the host computer, same terminal or a new one + +```bash +docker run -it capabilities2 bash +ros2 launch capabilities2_server server.launch.py +``` + +### Starting the Capabilities2 Fabric + +On the host computer, iIn a new terminal + +```bash +source install/setup.bash +ros2 launch capabilities2_fabric fabric.launch.py +``` diff --git a/docker/workspace_entrypoint.sh b/docker/workspace_entrypoint.sh new file mode 100644 index 0000000..fb3f777 --- /dev/null +++ b/docker/workspace_entrypoint.sh @@ -0,0 +1,8 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source "/opt/ros/$ROS_DISTRO/setup.bash" +source "$WORKSPACE_ROOT/install/setup.bash" + +exec "$@" \ No newline at end of file diff --git a/docs/design.md b/docs/design.md new file mode 100644 index 0000000..4366e1b --- /dev/null +++ b/docs/design.md @@ -0,0 +1,15 @@ +## Design + +The new capabilities package is designed to be more efficient and extensible. The functions are implemented as plugins, which can be loaded at runtime. The execution of providers is abstracted using an API called runners. The runners can manage more arbitrary provider operation which can include performing actions or services, or even running other capabilities. The capability models are stored in a database, This will allow various feature improvements such as hot reloading, state persistence, and model extension. Another possible feature is to create more complex ontological relationships between capabilities such as prerequisites, conflicts, or RDF triples (for example, `grasp` results in `holding`, or `pick` is a type of `manipulation`, or `grasp` is incompatible with `push`). + +### Bonds + +The bonds feature as implemented in `capabilities` and reimplemented in `capabilities2` allows applications to overlap functions by requesting use of the same resources. When all references to a resource are released, the resource is stopped. This has the added benefit of creating an idle state for the robot in which the robot computing resources are not being used. As a result of this, the robot could be more energy-efficient, and the robot could be more responsive to new tasks. + +### Runners + +The runners feature which is new in `capabilities2` allows capabilities to be executed in a more diverse manner. This could include running actions, services, or even other capabilities. This allows capabilities to be used like actions and not just started and stopped by the capabilities service. The Runner API is designed to be extensible, so that specific runners can be created on a per provider basis. Another feature that can be implemented is cross-runner communication, which could allow capabilities to be combined at runtime to perform more complex tasks. This could overcome limitations in robot programming in which there is a common need for extensive pre-definition of tasks. + +#### launch runner + +To enable the launch functionality from `capabilities` in `capabilities2`, a `launch runner` has been implemented. Due to the design of the launch system in ROS2, it was necessary to create a `launch_proxy` node which uses the `python` launch API to start and stop launch files. The runner allows uses an action to start and stop launch files, and keep track of running launch files. diff --git a/docs/fabric_setup.md b/docs/fabric_setup.md new file mode 100644 index 0000000..ae5f63a --- /dev/null +++ b/docs/fabric_setup.md @@ -0,0 +1,25 @@ +# Dependency installation for Fabric Runners + +## Clone packages + +Clone the fabric package same workspace if its not already availabe in the workspace. Fabric Runners are dependent on this package. + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/fabric.git +``` + +## Clone Capabilities2 plugin for Fabric stack + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/capabilities2_runner_fabric.git +``` + +## 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/foxglove_studio.md b/docs/foxglove_studio.md new file mode 100644 index 0000000..341ea13 --- /dev/null +++ b/docs/foxglove_studio.md @@ -0,0 +1,7 @@ +# Dependency Installation with Foxglove Studio + +We utilize foxglove studio to visalize information about the Capabilities2 system. + +Event system has been defined in capabilities2_events package and foxglove-bridge has been set as a dependency of capabilities2_events for ease of installation. We have selected foxglove-bridge over rosbridge due to performance improvements foxglove-bridge has over rosbridge due to former being written in C++ and latter being in Python. + +To visualize data, download foxglove-studio from [website](https://foxglove.dev/download) and create a free account when signing in. \ No newline at end of file diff --git a/docs/nav2_setup.md b/docs/nav2_setup.md new file mode 100644 index 0000000..b8eaab5 --- /dev/null +++ b/docs/nav2_setup.md @@ -0,0 +1,41 @@ +# Dependency installation for Nav2 Runners + +## Install nav2 stack and slam-toolbox + +Run the following commands to install nav2 stack + +```bash +sudo apt install ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-slam-toolbox +``` + +## Clone Nav2 configuration + +Clone the nav_stack default configuration and launch files to the same workspace if its not already availabe in the workspace. Capabilities2 Nav2 Runners are dependent on this package. + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/nav_stack.git +``` + +## Clone Capabilities2 plugin for Nav2 stack + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/capabilities2_runner_nav2.git +``` + +## Turtlebot3 Simulation (Optional) + +If using a simulated turtlebot3 for testing, install using following commands. + +```bash +sudo apt install ros-$ROS_DISTRO-turtlebot3* +``` + +## 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/prompt_tools_setup.md b/docs/prompt_tools_setup.md new file mode 100644 index 0000000..dd71076 --- /dev/null +++ b/docs/prompt_tools_setup.md @@ -0,0 +1,25 @@ +# 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 +``` + +## Clone Capabilities2 plugin for Prompt tools stack + +```bash +cd src +git clone https://github.com/CollaborativeRoboticsLab/capabilities2_runner_prompt.git +``` + +## 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/run_test_scripts.md b/docs/run_test_scripts.md index 791200c..77044e3 100644 --- a/docs/run_test_scripts.md +++ b/docs/run_test_scripts.md @@ -1,14 +1,23 @@ # Run test scripts -Capabilities features can be tested using the provided test scripts. The test scripts primarily test the capabilities2 server using service clients. Launch the capabilities2 server before running the test scripts [information here](../capabilities2_server/readme.md). The test scripts are written to test against the `std_capabilities` package. Make sure that the `std_capabilities` package is copied along with the `capabilities2` package to the `capabilities2_ws/src` folder and is built and sourced before running the test scripts. +Capabilities features can be tested using the provided test scripts. The test scripts primarily test the capabilities2 server using service clients. Launch the capabilities2 server before running the test scripts using -> **Note**: The test scripts use the `bondby` package. Make sure to install the `bondpy` package before running the test scripts. -> sudo apt install ros-$ROS_DISTRO-bondy +```bash +source install/setup.bash +ros2 launch capabilities2_server server.launch.py +``` + +The test scripts are written to test against the `std_capabilities` package. Make sure that the `std_capabilities` package is copied along with the `capabilities2` package to the `capabilities2_ws/src` folder and is built and sourced before running the test scripts. + +> **Note**: The test scripts use the `bondby` package. Make sure to install the `bondpy` package before running the test scripts. \ +> `sudo apt install ros-$ROS_DISTRO-bondy` Run the tests with python3. The test scripts are located in the `capabilities2_server/test` directory. Make sure to source the workspace before running the test. ```bash # example +source install/setup.bash +cd src/capabilities2/capabilities2_server/test python3 call_establish_bond.py ``` @@ -23,6 +32,13 @@ python3 call_establish_bond.py There is another test script in the `capabilities2_launch_proxy` package that tests using a capability. +```bash +# example +source install/setup.bash +cd src/capabilities2/capabilities2_launch_proxy/test +python3 call_establish_bond.py +``` + | Test Script | Description | | --- | --- | | [call_use_launch_runner.py](../capabilities2_launch_proxy/test/call_use_launch_runner.py) | Test using a launch runner based capability. This tests the bond, use, and get running features of the capabilities server | diff --git a/docs/setup.md b/docs/setup.md new file mode 100644 index 0000000..05ac7d6 --- /dev/null +++ b/docs/setup.md @@ -0,0 +1,49 @@ +## Using Capabilities package (Development and Deployment) + +The current package has been tested on Ubuntu and ROS2 Humble, Rolling and Jazzy. Complete following sections in order. + +### Base folder creation + +Create the workspace folder by running following commands in the terminal. + +```bash +mkdir -p /home/$USER/capabilities_ws/src +cd /home/$USER/capabilities_ws/src +``` + +### Cloning the Packages + +Clone the package using Git + +```bash +git clone https://github.com/CollaborativeRoboticsLab/capabilities2.git +``` + +Optionally you can clone +```bash +git https://github.com/CollaborativeRoboticsLab/std_capabilities.git +``` +### Dependency installation + +Move the terminal to workspace root and install dependencies. + +```bash +cd /home/$USER/capabilities_ws +``` +```bash +rosdep install --from-paths src --ignore-src -r -y +``` + +### Compile + +Use colcon to build the packages: + +```bash +colcon build +``` + +### Additional installtions + +[Fabric](fabric_setup.md) \ +[Prompt Tools](./prompt_tools_setup.md) \ +[Nav2 stack](./nav2_setup.md) \ No newline at end of file diff --git a/docs/setup_with_dev.md b/docs/setup_with_dev.md new file mode 100644 index 0000000..88972ee --- /dev/null +++ b/docs/setup_with_dev.md @@ -0,0 +1,53 @@ +## Using Capabilities package (Development and Deployment) + +The current package has been tested on Ubuntu and ROS2 Humble, Rolling and Jazzy. Complete following sections in order. + +### Base folder creation + +Create the workspace folder by running following commands in the terminal. + +```bash +mkdir -p /home/$USER/capabilities_ws/src +cd /home/$USER/capabilities_ws/src +``` + +### Cloning the Package + +Clone the package using Git + +```bash +git clone -b capabilities2-server-fabric https://github.com/CollaborativeRoboticsLab/capabilities2.git +git clone -b develop https://github.com/CollaborativeRoboticsLab/std_capabilities.git +git clone https://github.com/AIResearchLab/nav_stack.git +``` + +### Devcontainer + +A `devcontainer` is provided for developing the capabilities2 meta-package. The container can be used with [Microsoft Visual Studio Code](https://code.visualstudio.com/) and [Remote Development Extention](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.vscode-remote-extensionpack). + +If there are multiple packages in the workspace as with the usual case, move the `.devcontainer` folder into the parent folder such that it is located as follows, +```txt +colcon_ws +--> build +--> install +--> log +--> src + --> .devcontainer + --> capabilities2 + --> pakcage .. +``` + +Close and reopen the vs code editor so that devcontainer is automatically detected. After that, Dependencies need to be installed in the container. Install the dependencies with rosdep: + +```bash +# in the devcontainer +rosdep install --from-paths src --ignore-src -r -y +``` + +### Compile + +Use colcon to build the packages: + +```bash +colcon build +``` diff --git a/docs/testing.md b/docs/testing.md new file mode 100644 index 0000000..5adeead --- /dev/null +++ b/docs/testing.md @@ -0,0 +1,5 @@ + + +### + +Read more about this [here]() \ No newline at end of file diff --git a/readme.md b/readme.md index 91714cd..e7bacbd 100644 --- a/readme.md +++ b/readme.md @@ -1,14 +1,19 @@ -# capabilities2 +# Capabilities2 [![ROS2 Jazzy](https://img.shields.io/badge/ROS2-Jazzy-blue)](https://index.ros.org/doc/ros2/Releases/) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![Open in Visual Studio Code](https://img.shields.io/badge/vscode-dev-blue)](https://open.vscode.dev/airesearchlab/capabilities2) ![C++](https://img.shields.io/badge/Code-C++-informational?&logo=c%2b%2b) -![ROS](https://img.shields.io/badge/Framework-ROS-informational?&logo=ROS) +![ROS](https://img.shields.io/badge/Framework-ROS2-informational?&logo=ROS) -A reimplementation of the [capabilities](https://github.com/osrf/capabilities) package. This package is implemented using C++17 and extends the capabilities package features. See the [capabilities2_server](./capabilities2_server/readme.md) package for the main system component. +A reimplementation of the [capabilities](https://github.com/osrf/capabilities) package. This package is implemented using C++17 and extends the capabilities package features. +- [capabilities2_server](./capabilities2_server/readme.md) package contains the core of the system. +- [capabilities2_runner](./capabilities2_server/readme.md) package contains base and template classes for capability implementations. + + +## Extentions +- [Fabric](https://github.com/CollaborativeRoboticsLab/fabric) package implements a behaviour planning framework that utlizes capabilities2 system. -[More Information about Motivation and Example Use Cases](./docs/motivation_and_examples.md) ## System structure @@ -16,7 +21,7 @@ A reimplementation of the [capabilities](https://github.com/osrf/capabilities) p ## Entities -The main usage of `capabilities2` will typically involve creating capabilities through providers, interfaces and semantic interfaces. Which are represented in specification file stored as YAML, see the definitions and examples for each: +The main usage of `capabilities2` will typically involve creating or customizing capabilities through providers, interfaces and semantic interfaces. These are stored as YAML, and for more information about definitions and examples, click the links: | Entity | Description | | --- | --- | @@ -24,103 +29,33 @@ The main usage of `capabilities2` will typically involve creating capabilities t | [Providers](./docs/providers.md) | The capability provider specification file provides a mechanism to operate the capability | | [Semantic Interfaces](./docs/semantic_interfaces.md) | The semantic interface specification file provides a mechanism to redefine a capability with semantic information | -See [docs](./docs/) for the format of these entities or click the links above. Runners can be created using the runner API parent classes [here](./capabilities2_runner/readme.md). The capabilities service can be started using the [capabilities2_server](./capabilities2_server/readme.md) package. - -## Design - -The new capabilities package is designed to be more efficient and extensible. The functions are implemented as plugins, which can be loaded at runtime. The execution of providers is abstracted using an API called runners. The runners can manage more arbitrary provider operation which can include performing actions or services, or even running other capabilities. The capability models are stored in a database, This will allow various feature improvements such as hot reloading, state persistence, and model extension. Another possible feature is to create more complex ontological relationships between capabilities such as prerequisites, conflicts, or RDF triples (for example, `grasp` results in `holding`, or `pick` is a type of `manipulation`, or `grasp` is incompatible with `push`). - -### Bonds - -The bonds feature as implemented in `capabilities` and reimplemented in `capabilities2` allows applications to overlap functions by requesting use of the same resources. When all references to a resource are released, the resource is stopped. This has the added benefit of creating an idle state for the robot in which the robot computing resources are not being used. As a result of this, the robot could be more energy-efficient, and the robot could be more responsive to new tasks. - -### Runners +Runners can be created using the runner API parent classes [here](./capabilities2_runner/readme.md). The capabilities service can be started using the [capabilities2_server](./capabilities2_server/readme.md) package. -The runners feature which is new in `capabilities2` allows capabilities to be executed in a more diverse manner. This could include running actions, services, or even other capabilities. This allows capabilities to be used like actions and not just started and stopped by the capabilities service. The Runner API is designed to be extensible, so that specific runners can be created on a per provider basis. Another feature that can be implemented is cross-runner communication, which could allow capabilities to be combined at runtime to perform more complex tasks. This could overcome limitations in robot programming in which there is a common need for extensive pre-definition of tasks. -#### launch runner +## Setup Information +- [Installation](./docs/setup.md) +- [Setup Instructions with devcontainer](./docs/setup_with_dev.md) +- [Dependency installation for Fabric Runners](./docs/fabric_setup.md) +- [Dependency installation for Nav2 Runners](./docs/nav2_setup.md) +- [Dependency installation for Prompt Runners](./docs/prompt_tools_setup.md) +- [Dependency installation for Foxglove-studio](./docs/foxglove_studio.md) -To enable the launch functionality from `capabilities` in `capabilities2`, a `launch runner` has been implemented. Due to the design of the launch system in ROS2, it was necessary to create a `launch_proxy` node which uses the `python` launch API to start and stop launch files. The runner allows uses an action to start and stop launch files, and keep track of running launch files. - - -## Using Capabilities package (Development and Deployment) - -The current package has been tested on Ubuntu and ROS2 Rolling and Jazzy. - -### Basic setup - -Create the workspace folder by running following commands in the terminal. - -```bash -mkdir -p /home/$USER/capabilities_ws/src -cd /home/$USER/capabilities_ws/src -``` - -Unzip and copy the `capabilities2` folder into the `/home/$USER/capabilities_ws/src` folder. (Final path should be like `/home/$USER/capabilities_ws/src/capabilities2`) - -### When using devcontainer - -A `devcontainer` is provided for developing the capabilities2 meta-package. The container can be used with [Microsoft Visual Studio Code](https://code.visualstudio.com/) and [Remote Development Extention](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.vscode-remote-extensionpack). Dependencies need to be installed in the container. Install the dependencies with rosdep: - -```bash -# in the devcontainer -rosdep install --from-paths src --ignore-src -r -y -``` - -### When using without the devcontainer - -On the terminal run the following command to identify the $USER and note down the value - -```bash -echo $USER -``` - -Then open the `/home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml` with an available text editor. Either gedit or nano can be used. - -```sh -nano /home/$USER/ROS/capabilities_ws/src/capabilities2/capabilities2_server/config/capabilities.yaml -``` - -In the opened file, replace $USER value in lines 9 & 10 with above identified value for $USER. if the $USER is ubuntu, those lines should be - -```yaml - - /home/ubuntu/capabilities_ws/src - - /home/ubuntu/capabilities_ws/src/capabilities2 -``` - -Save the file and close. - -Move the terminal to workspace root and install dependencies. - -```bash -cd /home/$USER/capabilities_ws -``` -```bash -rosdep install --from-paths src --ignore-src -r -y -``` - -### Building - -Use Colcon to build the package: - -```bash -colcon build -``` +## Quick Startup information ### Starting the Capabilities2 server ```bash source install/setup.bash -ros2 launch capabilities2_server capabilities2_server.launch.py +ros2 launch capabilities2_server server.launch.py ``` -### Terminal based bond creation - -Read more about this [here](../capabilities2/capabilities2_server/readme.md) - -### Running Test cases - -Read more about this [here](../capabilities2/docs/run_test_scripts.md) +## Additional Information +- [Motivation and Example Use Cases](./docs/motivation_and_examples.md) +- [Design Information](./docs/design.md) +- [Registering a capability](./capabilities2_server/docs/register.md) +- [Terminal based capability usage](./capabilities2_server/docs/terminal_usage.md) +- [Running test scripts](./docs/run_test_scripts.md) +- [Using with Docker](./docker/docs/startup.md) ## Acknowledgements @@ -129,3 +64,17 @@ This work is based on the capabilities package developed by the Open Source Robo ## Citation If you use this work in an academic context, please cite the following publication(s): + +[Capabilities2 for ROS2: Advanced Skill-Based Control for Human-Robot Interaction](https://dl.acm.org/doi/10.5555/3721488.3721623) +```latex +@inproceedings{10.5555/3721488.3721623, + author = {Pritchard, Michael and Ratnayake, Kalana and Gamage, Buddhi and Jayasuriya, Maleen and Herath, Damith}, + title = {Capabilities2 for ROS2: Advanced Skill-Based Control for Human-Robot Interaction}, + year = {2025}, + publisher = {IEEE Press}, + booktitle = {Proceedings of the 2025 ACM/IEEE International Conference on Human-Robot Interaction}, + pages = {1067–1071}, + location = {Melbourne, Australia}, + series = {HRI '25} +} +```