From d02a4abd5489ee9470cf80b2351b9030926c66d2 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 7 Jan 2025 14:15:14 +1100 Subject: [PATCH 01/11] updated codebase for review. still in progress --- .../docs/waypoint_runner_ex1.md | 2 +- .../docs/waypoint_runner_ex2.md | 2 +- .../capabilities_fabric.hpp | 8 +- .../capabilities_xml_parser.hpp | 6 +- capabilities2_fabric/plans/navigation_1.xml | 2 +- capabilities2_fabric/plans/navigation_2.xml | 4 +- .../capabilities2_runner/action_runner.hpp | 108 +++++------ .../capabilities2_runner/launch_runner.hpp | 9 +- .../notrigger_action_runner.hpp | 15 +- .../capabilities2_runner/runner_base.hpp | 146 +++++++++++---- .../capabilities2_runner/service_runner.hpp | 78 ++++---- .../capabilities2_runner/topic_runner.hpp | 61 +++--- capabilities2_runner/src/standard_runners.cpp | 11 +- .../listen_runner.hpp | 33 +--- .../speak_runner.hpp | 13 -- .../bt_factory_runner.hpp | 38 ++-- .../occupancygrid_runner.hpp | 174 ++++-------------- .../robotpose_runner.hpp | 88 ++------- .../waypoint_runner.hpp | 14 +- .../prompt_occupancy_runner.hpp | 17 -- .../prompt_plan_request_runner.hpp | 46 ++--- .../prompt_plan_response_runner.hpp | 26 +-- .../prompt_pose_runner.hpp | 17 -- .../prompt_text_runner.hpp | 17 -- .../capabilities2_server/capabilities_api.hpp | 2 +- .../capabilities2_server/runner_cache.hpp | 122 +++--------- capabilities2_server/launch/server.launch.py | 2 +- 27 files changed, 388 insertions(+), 673 deletions(-) diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 555c773..76ef73a 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -39,7 +39,7 @@ colcon build ```bash export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +ros2 launch turtlebot3_gazebo empty_world.launch.py ``` ### Start the Navigation2 stack diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 01121e9..6cf251a 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -39,7 +39,7 @@ colcon build ```bash export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py +ros2 launch turtlebot3_gazebo turtlebot3_empty_world.launch.py ``` ### Start the Navigation2 stack diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index ef5bb30..a1396a7 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -588,13 +588,13 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id = response->bond_id; - feedback->progress = "Received the bond id : " + bond_id.c_str() ; + feedback->progress = "Received the bond id : " + bond_id ; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); expected_capabilities_ = connection_map.size(); - RCLCPP_INFO(this->get_logger(), "Requsting use (start) of %d capabilities", expected_capabilities_); + RCLCPP_INFO(this->get_logger(), "Requsting start of %d capabilities", expected_capabilities_); use_capability(connection_map, goal_handle); }); @@ -620,7 +620,7 @@ class CapabilitiesFabric : public rclcpp::Node request_use->bond_id = bond_id; feedback->progress = - "Using (starting) capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; + "Starting capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), ""); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -655,7 +655,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability use (start) succeessful"; + feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability start succeessful"; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp index cd43df5..fedbc53 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp @@ -68,18 +68,18 @@ bool check_plan_tag(tinyxml2::XMLDocument& document) * * @return `true` if element is not nullptr and conversion successful, `false` if element is nullptr */ -bool convert_to_string(tinyxml2::XMLElement* element, std::string& paramters) +bool convert_to_string(tinyxml2::XMLElement* element, std::string& parameters) { if (element) { tinyxml2::XMLPrinter printer; element->Accept(&printer); - paramters = printer.CStr(); + parameters = printer.CStr(); return true; } else { - paramters = ""; + parameters = ""; return false; } } diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index 7e48c39..3b6de79 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml index e87805d..a2569f7 100644 --- a/capabilities2_fabric/plans/navigation_2.xml +++ b/capabilities2_fabric/plans/navigation_2.xml @@ -1,7 +1,7 @@ - - + + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index f438e52..54429ee 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -88,11 +88,17 @@ class ActionRunner : public RunnerBase if (response->return_code != action_msgs::srv::CancelGoal_Response::ERROR_NONE) { // throw runner_exception("failed to cancel runner"); - RCLCPP_WARN(node_->get_logger(), "Runner cancelation failed."); + RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); } // Trigger on_stopped event if defined - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + if (events[execute_id].on_stopped != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_stopped, + update_on_stopped(events[execute_id].on_stopped_param)); + } }); // wait for action to be stopped. hold the thread for 2 seconds to help keep callbacks in scope @@ -118,34 +124,42 @@ class ActionRunner : public RunnerBase } /** - * @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(tinyxml2::XMLElement* parameters = nullptr) override + virtual void triggerExecution() override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_) throw runner_exception("cannot trigger action without parameters"); // generate a goal from parameters if provided - typename ActionT::Goal goal_msg = generate_goal(parameters); + typename ActionT::Goal goal_msg = generate_goal(parameters_); + + RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); // send goal options // goal response callback send_goal_options_.goal_response_callback = [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (goal_handle) - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + { + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } + } else { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); } // store goal handle to be used with stop funtion @@ -155,10 +169,26 @@ class ActionRunner : public RunnerBase // result callback send_goal_options_.result_callback = [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); // Trigger on_success event if defined - else - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); // Trigger on_failure event if defined + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) + { + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } + } + else + { + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } + } result_ = wrapped_result.result; }; @@ -166,33 +196,20 @@ class ActionRunner : public RunnerBase // trigger the action client with goal auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); + RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + // 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, - goal_handle_future](tinyxml2::XMLElement* result) { - auto goal_handle = goal_handle_future.get(); - - if (!goal_handle) - { - RCLCPP_ERROR(node_->get_logger(), "Failed to send goal"); - return; - } + auto goal_handle = goal_handle_future.get(); - // Get the result asynchronously - auto result_future = action_client_->async_get_result(goal_handle); + RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - // Wait for result future - auto wrapped_result = result_future.get(); - - // convert the result - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - result = generate_result(wrapped_result.result); - } - }; - - return result_callback; + if (!goal_handle) + { + RCLCPP_INFO(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); + return; + } } protected: @@ -209,19 +226,6 @@ class ActionRunner : public RunnerBase */ virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters) = 0; - /** - * @brief generate a typed erased goal result - * - * 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 - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* generate_result(const typename ActionT::Result::SharedPtr& result) = 0; - /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 82b32f4..22ff606 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -122,11 +122,18 @@ class LaunchRunner : public RunnerBase } // throw on trigger function - std::optional> trigger(tinyxml2::XMLElement* parameters) override + void trigger(const std::string& parameters) override { throw runner_exception("No Trigger as this is launch runner"); } +protected: + // throw on triggerExecution function + void triggerExecution() override + { + throw runner_exception("no triggerExecution() this is a no-trigger action runner"); + } + std::string launch_name; std::string package_name; diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp index 0dc8a8b..de9c10b 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(tinyxml2::XMLElement* 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(tinyxml2::XMLElement*) override + + // throw on triggerExecution function + void triggerExecution() 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"); } - tinyxml2::XMLElement* 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 4027760..5a0db5e 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include @@ -63,25 +63,25 @@ struct runner_opts * * keeps track of events that are related to runner instances at various points of the * plan - * @param on_started pointer to function to execute on start - * @param on_success pointer to function to execute on success - * @param on_failure pointer to function to execute on failure - * @param on_stopped pointer to function to execute on stop - * @param on_started_param parameters for the function to execute on start - * @param on_success_param parameters for the function to execute on success - * @param on_failure_param parameters for the function to execute on failure - * @param on_stopped_param parameters for the function to execute on stop + * @param on_started name of the capability to execute on start + * @param on_success name of the capability to execute on success + * @param on_failure name of the capability to execute on failure + * @param on_stopped name of the capability to execute on stop + * @param on_started_param parameters for the capability to execute on start + * @param on_success_param parameters for the capability to execute on success + * @param on_failure_param parameters for the capability to execute on failure + * @param on_stopped_param parameters for the capability to execute on stop */ struct event_opts { - std::function on_started; - std::function on_success; - std::function on_failure; - std::function on_stopped; - tinyxml2::XMLElement* on_started_param; - tinyxml2::XMLElement* on_success_param; - tinyxml2::XMLElement* on_failure_param; - tinyxml2::XMLElement* on_stopped_param; + std::string on_started; + std::string on_success; + std::string on_failure; + std::string on_stopped; + std::string on_started_param; + std::string on_success_param; + std::string on_failure_param; + std::string on_stopped_param; }; class RunnerBase @@ -112,18 +112,20 @@ 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(tinyxml2::XMLElement* parameters = nullptr) = 0; + virtual void trigger(const std::string& parameters) + { + parameters_ = convert_to_xml(parameters); + + executionThread = std::thread(&RunnerBase::triggerExecution, this); + } /** * @brief Initializer function for initializing the base runner in place of constructor due to plugin semantics @@ -144,13 +146,17 @@ class RunnerBase * @brief attach events to the runner * * @param event_option event_options related for the action - * + * * @return number of attached events */ - int attach_events(capabilities2_runner::event_opts& event_option) + int attach_events(capabilities2_runner::event_opts& event_option, + std::function triggerFunction) { - RCLCPP_INFO(node_->get_logger(), "%s accepted event options with ID : %d ", run_config_.interface.c_str(), insert_id); - + RCLCPP_INFO(node_->get_logger(), "%s accepted event options with ID : %d ", run_config_.interface.c_str(), + insert_id); + + triggerFunction_ = triggerFunction; + events[insert_id] = event_option; insert_id += 1; @@ -198,6 +204,16 @@ class RunnerBase } protected: + /** + * @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 triggerExecution() = 0; + /** * @brief Update on_started event parameters with new data if avaible. * @@ -209,7 +225,7 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_started(tinyxml2::XMLElement* parameters) + virtual std::string update_on_started(std::string& parameters) { return parameters; }; @@ -225,7 +241,7 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_stopped(tinyxml2::XMLElement* parameters) + virtual std::string update_on_stopped(std::string& parameters) { return parameters; }; @@ -241,7 +257,7 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_failure(tinyxml2::XMLElement* parameters) + virtual std::string update_on_failure(std::string& parameters) { return parameters; }; @@ -257,11 +273,55 @@ class RunnerBase * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* 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 /** @@ -423,8 +483,28 @@ class RunnerBase /** * @brief pointer to XMLElement which contain parameters - * */ + */ tinyxml2::XMLElement* parameters_; + + /** + * @brief thread that executes the triggerExecution functionality + */ + std::thread executionThread; + + /** + * @brief external function that triggers capability runners + */ + std::function triggerFunction_; + + /** + * @brief XMLElement that is used to convert xml strings to std::string + */ + tinyxml2::XMLPrinter printer; + + /** + * @brief XMLElement that is used to convert std::string to xml strings + */ + tinyxml2::XMLDocument doc; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 73201d8..5058784 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -52,61 +52,54 @@ class ServiceRunner : public RunnerBase } /** - * @brief the trigger function on the service runner is used to trigger an service. - * this method provides a mechanism for injecting parameters or a goal into a service - * and then trigger the service + * @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(tinyxml2::XMLElement* parameters = nullptr) override + virtual void triggerExecution() override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_) throw runner_exception("cannot trigger service without parameters"); // generate a goal from parameters if provided - auto request_msg = std::make_shared(generate_request(parameters)); + auto request_msg = std::make_shared(generate_request(parameters_)); auto result_future = service_client_->async_send_request( request_msg, [this](typename rclcpp::Client::SharedFuture future) { if (!future.valid()) { RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } } else { RCLCPP_INFO(node_->get_logger(), "get result call succeeded"); - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); - } - }); - - // Trigger started event if defined - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); - // Define a callback function to handle the result once it's ready - std::function result_callback = - [this, &result_future](tinyxml2::XMLElement* result) mutable { - auto response = result_future.get(); - result = generate_response(response); + response_ = future.get(); - // Ensure the future is ready before accessing the result - if (result_future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) - { - auto response = result_future.get(); - result = generate_response(response); + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } } - else - { - RCLCPP_WARN(node_->get_logger(), "Result is not ready yet."); - } - }; + }); - return result_callback; + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } } /** @@ -127,8 +120,11 @@ class ServiceRunner : public RunnerBase if (!service_client_) throw runner_exception("cannot stop runner action that was not started"); - // publish event - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } } protected: @@ -145,19 +141,7 @@ class ServiceRunner : public RunnerBase */ virtual typename ServiceT::Request generate_request(tinyxml2::XMLElement* parameters) = 0; - /** - * @brief generate a typed erased response - * - * This method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* generate_response(const typename ServiceT::Response::SharedPtr& result) const = 0; - 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 index 266d22c..64c26c3 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -44,41 +44,44 @@ class TopicRunner : public RunnerBase } /** - * @brief the trigger function on the topic subscriber runner is used to trigger data retrival. + * @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(tinyxml2::XMLElement* parameters = nullptr) override + virtual void triggerExecution() override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_) throw runner_exception("cannot grab data without parameters"); - events[execute_id].on_started(update_on_started(events[execute_id].on_started_param)); + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } if (latest_message_) { - events[execute_id].on_success(update_on_success(events[execute_id].on_success_param)); + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } } else { RCLCPP_ERROR(node_->get_logger(), "get result call failed"); - events[execute_id].on_failure(update_on_failure(events[execute_id].on_failure_param)); - } - // 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](tinyxml2::XMLElement* result) { - if (latest_message_) - result = generate_message(latest_message_); - }; - - return result_callback; + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } + } } /** @@ -99,8 +102,11 @@ class TopicRunner : public RunnerBase if (!subscription_) throw runner_exception("cannot stop runner subscriber that was not started"); - // publish event - events[execute_id].on_stopped(update_on_stopped(events[execute_id].on_stopped_param)); + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } } protected: @@ -117,19 +123,6 @@ class TopicRunner : public RunnerBase latest_message_ = msg; } - /** - * @brief generate xml data structure of the message - * - * this method is used in a callback passed to the trigger caller to get type erased result - * of the latest message can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* generate_message(typename TopicT::SharedPtr& result) = 0; - typename rclcpp::Subscription::SharedPtr subscription_; mutable typename TopicT::SharedPtr latest_message_{ nullptr }; diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index 88924a5..c01054d 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -26,11 +26,16 @@ class DummyRunner : public RunnerBase RCLCPP_INFO(node_->get_logger(), "Dummy runner stopped"); } - std::optional> - trigger(tinyxml2::XMLElement* 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 triggerExecution() 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 index f507939..59d2e8a 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -55,25 +55,6 @@ class ListenerRunner : public ActionRunner return goal_msg; } - /** - * @brief This generate_result function overrides the generate_result() function from ActionRunner(). Since - * - * @param result message from SpeechToText action - * @return tinyxml2::XMLElement* in the format '' - */ - virtual tinyxml2::XMLElement* - generate_result(const hri_audio_msgs::action::SpeechToText::Result::SharedPtr& result) override - { - tinyxml2::XMLDocument doc; - - // Root element - tinyxml2::XMLElement* textElement = doc.NewElement("Text"); - doc.InsertEndChild(textElement); - textElement->SetText(result->stt_text.c_str()); - - return doc.FirstChildElement("Text"); - } - /** * @brief Update on_success event parameters with new data if avaible. * @@ -85,15 +66,19 @@ class ListenerRunner : public ActionRunner * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + // Create the Pose element as a child of the existing parameters element - tinyxml2::XMLElement* textElement = parameters->GetDocument()->NewElement("Text"); - parameters->InsertEndChild(textElement); + tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("Text"); + element->InsertEndChild(textElement); textElement->SetText(result_->stt_text.c_str()); - // Return the updated parameters element with Pose added - return parameters; + // Return the updated parameters element with Pose added as string + std::string result = convert_to_string(element); + + return result; }; }; diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index d8f37bc..925d90f 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -59,19 +59,6 @@ class SpeakerRunner : public ActionRunner return goal_msg; } - /** - * @brief This generate_result function overrides the generate_result() function from ActionRunner(). Since this does not - * return a value, return value is nullptr - * - * @param result message from SpeechToText action - * @return nullptr - */ - virtual tinyxml2::XMLElement* - generate_result(const hri_audio_msgs::action::TextToSpeech::Result::SharedPtr& result) override - { - return nullptr; - } - }; } // namespace capabilities2_runner \ No newline at end of file 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 index b26ebc0..ff3b463 100644 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp @@ -21,13 +21,10 @@ class BTRunnerBase : public RunnerBase, public 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) + virtual void start(rclcpp::Node::SharedPtr node, const runner_opts& run_config) { // init the runner base - init_base(node, run_config, on_started, on_terminated, on_stopped); + init_base(node, run_config); // register (bt)actions from ROS plugins try @@ -46,14 +43,14 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory tree_.reset(); } +protected: /** * @brief trigger the behaviour factory with the input data * * @param parameters * @return std::optional)>> */ - virtual std::optional)>> - trigger(std::shared_ptr parameters) + virtual void trigger(tinyxml2::XMLElement* parameters = nullptr) override { // if parameters are not provided then cannot proceed if (!parameters) @@ -63,25 +60,24 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory 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(); + // // 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"); + // // fill the result + // if (status == BT::NodeStatus::SUCCESS) + // result->SetText("OK"); + // else + // result->SetText("FAIL"); - return; - }; + // return; + // }; - return result_callback; + // return result_callback; } -protected: // the tree std::shared_ptr tree_; }; diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp index f5f0ef3..6229a76 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/occupancygrid_runner.hpp @@ -35,122 +35,6 @@ class OccupancyGridRunner : public TopicRunner } protected: - /** - * @brief This generate goal function overrides the generate_message() function from TopicRunner() - * @param result of the type geometry_msgs::msg::PoseWithCovarianceStamped - - * @return tinyxml2::XMLElement* of the robot's pose - */ - virtual tinyxml2::XMLElement* - generate_message(typename nav_msgs::msg::OccupancyGrid::SharedPtr& result) override - { - tinyxml2::XMLDocument doc; - - // Root element for OccupancyGrid - tinyxml2::XMLElement* occupancyGridElement = doc.NewElement("OccupancyGrid"); - doc.InsertFirstChild(occupancyGridElement); - - // Header element - tinyxml2::XMLElement* headerElement = doc.NewElement("header"); - occupancyGridElement->InsertEndChild(headerElement); - - // Header fields: stamp, frame_id - tinyxml2::XMLElement* stampElement = doc.NewElement("stamp"); - headerElement->InsertEndChild(stampElement); - - tinyxml2::XMLElement* secsElement = doc.NewElement("secs"); - secsElement->SetText(result->header.stamp.sec); - stampElement->InsertEndChild(secsElement); - - tinyxml2::XMLElement* nsecsElement = doc.NewElement("nsecs"); - nsecsElement->SetText(result->header.stamp.nanosec); - stampElement->InsertEndChild(nsecsElement); - - tinyxml2::XMLElement* frameIdElement = doc.NewElement("frame_id"); - frameIdElement->SetText(result->header.frame_id.c_str()); - headerElement->InsertEndChild(frameIdElement); - - // Info element - tinyxml2::XMLElement* infoElement = doc.NewElement("info"); - occupancyGridElement->InsertEndChild(infoElement); - - // Map info fields: resolution, width, height - tinyxml2::XMLElement* resolutionElement = doc.NewElement("resolution"); - resolutionElement->SetText(result->info.resolution); - infoElement->InsertEndChild(resolutionElement); - - tinyxml2::XMLElement* widthElement = doc.NewElement("width"); - widthElement->SetText(result->info.width); - infoElement->InsertEndChild(widthElement); - - tinyxml2::XMLElement* heightElement = doc.NewElement("height"); - heightElement->SetText(result->info.height); - infoElement->InsertEndChild(heightElement); - - // Origin position element - tinyxml2::XMLElement* originElement = doc.NewElement("origin"); - infoElement->InsertEndChild(originElement); - - tinyxml2::XMLElement* positionElement = doc.NewElement("position"); - originElement->InsertEndChild(positionElement); - - tinyxml2::XMLElement* posX = doc.NewElement("x"); - posX->SetText(result->info.origin.position.x); - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = doc.NewElement("y"); - posY->SetText(result->info.origin.position.y); - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = doc.NewElement("z"); - posZ->SetText(result->info.origin.position.z); - positionElement->InsertEndChild(posZ); - - // Origin orientation element - tinyxml2::XMLElement* orientationElement = doc.NewElement("orientation"); - originElement->InsertEndChild(orientationElement); - - tinyxml2::XMLElement* oriX = doc.NewElement("x"); - oriX->SetText(result->info.origin.orientation.x); - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = doc.NewElement("y"); - oriY->SetText(result->info.origin.orientation.y); - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = doc.NewElement("z"); - oriZ->SetText(result->info.origin.orientation.z); - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = doc.NewElement("w"); - oriW->SetText(result->info.origin.orientation.w); - orientationElement->InsertEndChild(oriW); - - // Map load time - tinyxml2::XMLElement* mapLoadTimeElement = doc.NewElement("map_load_time"); - infoElement->InsertEndChild(mapLoadTimeElement); - - tinyxml2::XMLElement* mapSecsElement = doc.NewElement("secs"); - mapSecsElement->SetText(result->info.map_load_time.sec); - mapLoadTimeElement->InsertEndChild(mapSecsElement); - - tinyxml2::XMLElement* mapNsecsElement = doc.NewElement("nsecs"); - mapNsecsElement->SetText(result->info.map_load_time.nanosec); - mapLoadTimeElement->InsertEndChild(mapNsecsElement); - - // Data element for occupancy grid data - tinyxml2::XMLElement* dataElement = doc.NewElement("data"); - occupancyGridElement->InsertEndChild(dataElement); - - // Convert the occupancy grid data into a space-separated string - std::string data_str; - for (const auto& value : result->data) { - data_str += std::to_string(value) + " "; - } - dataElement->SetText(data_str.c_str()); - - return doc.FirstChildElement("OccupancyGrid"); - } /** * @brief Update on_success event parameters with new data if avaible. @@ -163,99 +47,101 @@ class OccupancyGridRunner : public TopicRunner * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + 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* occupancyGridElement = parameters->GetDocument()->NewElement("OccupancyGrid"); - parameters->InsertEndChild(occupancyGridElement); + tinyxml2::XMLElement* occupancyGridElement = element->GetDocument()->NewElement("OccupancyGrid"); + element->InsertEndChild(occupancyGridElement); // Header element - tinyxml2::XMLElement* headerElement = parameters->GetDocument()->NewElement("header"); + tinyxml2::XMLElement* headerElement = element->GetDocument()->NewElement("header"); occupancyGridElement->InsertEndChild(headerElement); - tinyxml2::XMLElement* stampElement = parameters->GetDocument()->NewElement("stamp"); + tinyxml2::XMLElement* stampElement = element->GetDocument()->NewElement("stamp"); headerElement->InsertEndChild(stampElement); - tinyxml2::XMLElement* secsElement = parameters->GetDocument()->NewElement("secs"); + tinyxml2::XMLElement* secsElement = element->GetDocument()->NewElement("secs"); secsElement->SetText(latest_message_->header.stamp.sec); stampElement->InsertEndChild(secsElement); - tinyxml2::XMLElement* nsecsElement = parameters->GetDocument()->NewElement("nsecs"); + tinyxml2::XMLElement* nsecsElement = element->GetDocument()->NewElement("nsecs"); nsecsElement->SetText(latest_message_->header.stamp.nanosec); stampElement->InsertEndChild(nsecsElement); - tinyxml2::XMLElement* frameIdElement = parameters->GetDocument()->NewElement("frame_id"); + tinyxml2::XMLElement* frameIdElement = element->GetDocument()->NewElement("frame_id"); frameIdElement->SetText(latest_message_->header.frame_id.c_str()); headerElement->InsertEndChild(frameIdElement); // Info element - tinyxml2::XMLElement* infoElement = parameters->GetDocument()->NewElement("info"); + tinyxml2::XMLElement* infoElement = element->GetDocument()->NewElement("info"); occupancyGridElement->InsertEndChild(infoElement); - tinyxml2::XMLElement* resolutionElement = parameters->GetDocument()->NewElement("resolution"); + tinyxml2::XMLElement* resolutionElement = element->GetDocument()->NewElement("resolution"); resolutionElement->SetText(latest_message_->info.resolution); infoElement->InsertEndChild(resolutionElement); - tinyxml2::XMLElement* widthElement = parameters->GetDocument()->NewElement("width"); + tinyxml2::XMLElement* widthElement = element->GetDocument()->NewElement("width"); widthElement->SetText(latest_message_->info.width); infoElement->InsertEndChild(widthElement); - tinyxml2::XMLElement* heightElement = parameters->GetDocument()->NewElement("height"); + tinyxml2::XMLElement* heightElement = element->GetDocument()->NewElement("height"); heightElement->SetText(latest_message_->info.height); infoElement->InsertEndChild(heightElement); // Origin element - tinyxml2::XMLElement* originElement = parameters->GetDocument()->NewElement("origin"); + tinyxml2::XMLElement* originElement = element->GetDocument()->NewElement("origin"); infoElement->InsertEndChild(originElement); - tinyxml2::XMLElement* positionElement = parameters->GetDocument()->NewElement("position"); + tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); originElement->InsertEndChild(positionElement); - tinyxml2::XMLElement* posX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); posX->SetText(latest_message_->info.origin.position.x); positionElement->InsertEndChild(posX); - tinyxml2::XMLElement* posY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); posY->SetText(latest_message_->info.origin.position.y); positionElement->InsertEndChild(posY); - tinyxml2::XMLElement* posZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); posZ->SetText(latest_message_->info.origin.position.z); positionElement->InsertEndChild(posZ); - tinyxml2::XMLElement* orientationElement = parameters->GetDocument()->NewElement("orientation"); + tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); originElement->InsertEndChild(orientationElement); - tinyxml2::XMLElement* oriX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); oriX->SetText(latest_message_->info.origin.orientation.x); orientationElement->InsertEndChild(oriX); - tinyxml2::XMLElement* oriY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); oriY->SetText(latest_message_->info.origin.orientation.y); orientationElement->InsertEndChild(oriY); - tinyxml2::XMLElement* oriZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); oriZ->SetText(latest_message_->info.origin.orientation.z); orientationElement->InsertEndChild(oriZ); - tinyxml2::XMLElement* oriW = parameters->GetDocument()->NewElement("w"); + tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); oriW->SetText(latest_message_->info.origin.orientation.w); orientationElement->InsertEndChild(oriW); // Map load time - tinyxml2::XMLElement* mapLoadTimeElement = parameters->GetDocument()->NewElement("map_load_time"); + tinyxml2::XMLElement* mapLoadTimeElement = element->GetDocument()->NewElement("map_load_time"); infoElement->InsertEndChild(mapLoadTimeElement); - tinyxml2::XMLElement* mapSecsElement = parameters->GetDocument()->NewElement("secs"); + tinyxml2::XMLElement* mapSecsElement = element->GetDocument()->NewElement("secs"); mapSecsElement->SetText(latest_message_->info.map_load_time.sec); mapLoadTimeElement->InsertEndChild(mapSecsElement); - tinyxml2::XMLElement* mapNsecsElement = parameters->GetDocument()->NewElement("nsecs"); + tinyxml2::XMLElement* mapNsecsElement = element->GetDocument()->NewElement("nsecs"); mapNsecsElement->SetText(latest_message_->info.map_load_time.nanosec); mapLoadTimeElement->InsertEndChild(mapNsecsElement); // Data element for occupancy data (converted to a string) - tinyxml2::XMLElement* dataElement = parameters->GetDocument()->NewElement("data"); + tinyxml2::XMLElement* dataElement = element->GetDocument()->NewElement("data"); occupancyGridElement->InsertEndChild(dataElement); std::string data_str; @@ -265,7 +151,9 @@ class OccupancyGridRunner : public TopicRunner dataElement->SetText(data_str.c_str()); // Return the updated parameters element with OccupancyGrid added - return parameters; + std::string result = convert_to_string(element); + + return result; }; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp index 83904d7..763d5a5 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/robotpose_runner.hpp @@ -35,62 +35,6 @@ class RobotPoseRunner : public TopicRunnerInsertEndChild(positionElement); - - // Position x, y, z - tinyxml2::XMLElement* posX = doc.NewElement("x"); - posX->SetText(result->pose.pose.position.x); // Set x position value - positionElement->InsertEndChild(posX); - - tinyxml2::XMLElement* posY = doc.NewElement("y"); - posY->SetText(result->pose.pose.position.y); // Set y position value - positionElement->InsertEndChild(posY); - - tinyxml2::XMLElement* posZ = doc.NewElement("z"); - posZ->SetText(result->pose.pose.position.z); // Set z position value - positionElement->InsertEndChild(posZ); - - // Orientation element - tinyxml2::XMLElement* orientationElement = doc.NewElement("orientation"); - poseElement->InsertEndChild(orientationElement); - - // Orientation x, y, z, w - tinyxml2::XMLElement* oriX = doc.NewElement("x"); - oriX->SetText(result->pose.pose.orientation.x); // Set orientation x value - orientationElement->InsertEndChild(oriX); - - tinyxml2::XMLElement* oriY = doc.NewElement("y"); - oriY->SetText(result->pose.pose.orientation.y); // Set orientation y value - orientationElement->InsertEndChild(oriY); - - tinyxml2::XMLElement* oriZ = doc.NewElement("z"); - oriZ->SetText(result->pose.pose.orientation.z); // Set orientation z value - orientationElement->InsertEndChild(oriZ); - - tinyxml2::XMLElement* oriW = doc.NewElement("w"); - oriW->SetText(result->pose.pose.orientation.w); // Set orientation w value - orientationElement->InsertEndChild(oriW); - - return doc.FirstChildElement("Pose"); - } - /** * @brief Update on_success event parameters with new data if avaible. * @@ -102,52 +46,56 @@ class RobotPoseRunner : public TopicRunnerGetDocument()->NewElement("Pose"); - parameters->InsertEndChild(poseElement); + tinyxml2::XMLElement* poseElement = element->GetDocument()->NewElement("Pose"); + element->InsertEndChild(poseElement); // Position element - tinyxml2::XMLElement* positionElement = parameters->GetDocument()->NewElement("position"); + tinyxml2::XMLElement* positionElement = element->GetDocument()->NewElement("position"); poseElement->InsertEndChild(positionElement); // Position x, y, z - tinyxml2::XMLElement* posX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* posX = element->GetDocument()->NewElement("x"); posX->SetText(latest_message_->pose.pose.position.x); // Set x position value positionElement->InsertEndChild(posX); - tinyxml2::XMLElement* posY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* posY = element->GetDocument()->NewElement("y"); posY->SetText(latest_message_->pose.pose.position.y); // Set y position value positionElement->InsertEndChild(posY); - tinyxml2::XMLElement* posZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* posZ = element->GetDocument()->NewElement("z"); posZ->SetText(latest_message_->pose.pose.position.z); // Set z position value positionElement->InsertEndChild(posZ); // Orientation element - tinyxml2::XMLElement* orientationElement = parameters->GetDocument()->NewElement("orientation"); + tinyxml2::XMLElement* orientationElement = element->GetDocument()->NewElement("orientation"); poseElement->InsertEndChild(orientationElement); // Orientation x, y, z, w - tinyxml2::XMLElement* oriX = parameters->GetDocument()->NewElement("x"); + tinyxml2::XMLElement* oriX = element->GetDocument()->NewElement("x"); oriX->SetText(latest_message_->pose.pose.orientation.x); // Set orientation x value orientationElement->InsertEndChild(oriX); - tinyxml2::XMLElement* oriY = parameters->GetDocument()->NewElement("y"); + tinyxml2::XMLElement* oriY = element->GetDocument()->NewElement("y"); oriY->SetText(latest_message_->pose.pose.orientation.y); // Set orientation y value orientationElement->InsertEndChild(oriY); - tinyxml2::XMLElement* oriZ = parameters->GetDocument()->NewElement("z"); + tinyxml2::XMLElement* oriZ = element->GetDocument()->NewElement("z"); oriZ->SetText(latest_message_->pose.pose.orientation.z); // Set orientation z value orientationElement->InsertEndChild(oriZ); - tinyxml2::XMLElement* oriW = parameters->GetDocument()->NewElement("w"); + tinyxml2::XMLElement* oriW = element->GetDocument()->NewElement("w"); oriW->SetText(latest_message_->pose.pose.orientation.w); // Set orientation w value orientationElement->InsertEndChild(oriW); - // Return the updated parameters element with Pose added - return parameters; + // Return the updated parameters element with Pose added as string + std::string result = convert_to_string(element); + + return result; }; }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp index fbad70b..8d30bba 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -52,7 +52,7 @@ class WayPointRunner : public ActionRunner parameters_->QueryDoubleAttribute("x", &x); parameters_->QueryDoubleAttribute("y", &y); - RCLCPP_INFO(node_->get_logger(), "%s is triggered with x: %f and y: %f", run_config_.interface.c_str(), x, y); + RCLCPP_INFO(node_->get_logger(), "[%s] goal consist of x: %f and y: %f", run_config_.interface.c_str(), x, y); nav2_msgs::action::NavigateToPose::Goal goal_msg; geometry_msgs::msg::PoseStamped pose_msg; @@ -68,18 +68,6 @@ class WayPointRunner : public ActionRunner return goal_msg; } - /** - * @brief This generate result function overrides the generate_result() function from ActionRunner(). Since - * this is not used in this context, this returns nullptr - * @param result message from FollowWaypoints action - * @return nullptr - */ - virtual tinyxml2::XMLElement* - generate_result(const nav2_msgs::action::NavigateToPose::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*/ diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index f3086f2..709f90d 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -74,23 +74,6 @@ class PromptOccupancyRunner : public ServiceRunner return request; } - - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - return nullptr; - } }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp index c9c41c2..7e17207 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp @@ -56,13 +56,17 @@ class PromptPlanRequestRunner : public ServiceRunner if (!replan) { - request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Return only the xml plan without explanations or comments"; + request.prompt.prompt = "Build a xml plan based on the availbale capabilities to acheive mentioned task. Return " + "only the xml plan without explanations or comments"; } else { tinyxml2::XMLElement* failedElements = parameters->FirstChildElement("FailedElements"); - request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task. Just give the xml plan without explanations or comments. These XML elements had incompatibilities. " + std::string(failedElements->GetText()) + "Recorrect them as well"; + request.prompt.prompt = "Rebuild the xml plan based on the availbale capabilities to acheive mentioned task. " + "Just give the xml plan without explanations or comments. These XML elements had " + "incompatibilities. " + + std::string(failedElements->GetText()) + "Recorrect them as well"; } prompt_msgs::msg::ModelOption modelOption1; @@ -81,25 +85,6 @@ class PromptPlanRequestRunner : public ServiceRunner return request; } - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - document_string = result->response.response; - - return nullptr; - } - /** * @brief Update on_success event parameters with new data if avaible. * @@ -111,21 +96,22 @@ class PromptPlanRequestRunner : public ServiceRunner * @param parameters pointer to the XMLElement containing parameters * @return pointer to the XMLElement containing updated parameters */ - virtual tinyxml2::XMLElement* update_on_success(tinyxml2::XMLElement* parameters) + virtual std::string update_on_success(std::string& parameters) { + tinyxml2::XMLElement* element = convert_to_xml(parameters); + + std::string document_string = response_->response.response; + // Create the plan element as a child of the existing parameters element - tinyxml2::XMLElement* textElement = parameters->GetDocument()->NewElement("ReceievdPlan"); - parameters->InsertEndChild(textElement); + tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("ReceievdPlan"); + element->InsertEndChild(textElement); textElement->SetText(document_string.c_str()); // Return the updated parameters element with Pose added - return parameters; - }; + std::string result = convert_to_string(element); - /** - * Document holder for received xml plan - */ - mutable std::string document_string; + return result; + }; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp index 48e4c56..49557d3 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -60,18 +60,6 @@ class PromptPlanResponseRunner : public ActionRunnerSetAttribute("replan", true); + tinyxml2::XMLElement* element = convert_to_xml(parameters); + + element->SetAttribute("replan", true); // Create the failed elements element as a child of the existing parameters element - tinyxml2::XMLElement* failedElements = parameters->GetDocument()->NewElement("FailedElements"); - parameters->InsertEndChild(failedElements); + tinyxml2::XMLElement* failedElements = element->GetDocument()->NewElement("FailedElements"); + element->InsertEndChild(failedElements); std::string failedElementsString = ""; @@ -98,7 +88,9 @@ class PromptPlanResponseRunner : public ActionRunnerSetText(failedElementsString.c_str()); - return parameters; + std::string result = convert_to_string(element); + + return result; }; }; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 09ba97b..1205541 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -74,23 +74,6 @@ class PromptPoseRunner : public ServiceRunner return request; } - - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - return nullptr; - } }; } // namespace capabilities2_runner diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 13713d1..0058a85 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -74,23 +74,6 @@ class PromptTextRunner : public ServiceRunner return request; } - - /** - * @brief generate a typed erased response - * - * this method is used in a callback passed to the trigger caller to get type erased result - * from the service the reponse can be passed by the caller or ignored - * - * The pattern needs to be implemented in the derived class - * - * @param wrapped_result - * @return tinyxml2::XMLElement* - */ - virtual tinyxml2::XMLElement* - generate_response(const typename prompt_msgs::srv::Prompt::Response::SharedPtr& result) const override - { - return nullptr; - } }; } // namespace capabilities2_runner diff --git a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp index 45f2bc0..55acc3b 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_api.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_api.hpp @@ -280,7 +280,7 @@ class CapabilitiesAPI on_failure_parameters, on_success_capability, on_success_parameters, on_stopped_capability, on_stopped_parameters); - RCLCPP_WARN(node_logging_interface_ptr_->get_logger(), "successfully set triggers"); + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "successfully set triggers"); } catch (const capabilities2_runner::runner_exception& e) { diff --git a/capabilities2_server/include/capabilities2_server/runner_cache.hpp b/capabilities2_server/include/capabilities2_server/runner_cache.hpp index 1ebd874..b702409 100644 --- a/capabilities2_server/include/capabilities2_server/runner_cache.hpp +++ b/capabilities2_server/include/capabilities2_server/runner_cache.hpp @@ -98,17 +98,15 @@ class RunnerCache */ void trigger_runner(const std::string& capability, const std::string& parameters) { - tinyxml2::XMLDocument doc; - doc.Parse(parameters.c_str()); - tinyxml2::XMLElement* xml_parameters = doc.FirstChildElement(); - // is the runner in the cache if (running(capability)) { - runner_cache_[capability]->trigger(xml_parameters); + runner_cache_[capability]->trigger(parameters); } else { + RCLCPP_ERROR(node_logging_interface_ptr_->get_logger(), "Runner not found for capability: %s", + capability.c_str()); throw capabilities2_runner::runner_exception("capability runner not found: " + capability); } } @@ -135,98 +133,25 @@ class RunnerCache { capabilities2_runner::event_opts event_options; - if (on_started_capability != "") - { - event_options.on_started = [this, &on_started_capability, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_start", capability.c_str(), - on_started_capability.c_str()); - runner_cache_[on_started_capability]->trigger(parameters); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_started_parameters.c_str()); - event_options.on_started_param = doc.FirstChildElement(); - } - else - { - event_options.on_started = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_start event", - capability.c_str()); - }; - - event_options.on_started_param = nullptr; - } - - if (on_failure_capability != "") - { - event_options.on_failure = [this, &on_failure_capability, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_failure", capability.c_str(), - on_failure_capability.c_str()); - runner_cache_[on_failure_capability]->trigger(parameters); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_failure_parameters.c_str()); - event_options.on_failure_param = doc.FirstChildElement(); - } - else - { - event_options.on_failure = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_failure event", - capability.c_str()); - } ; - - event_options.on_failure_param = nullptr; - } - - if (on_success_capability != "") - { - event_options.on_success = [this, &on_success_capability, &capability](tinyxml2::XMLElement* parameters) { - runner_cache_[on_success_capability]->trigger(parameters); - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_success", capability.c_str(), - on_success_capability.c_str()); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_success_parameters.c_str()); - event_options.on_success_param = doc.FirstChildElement(); - } - else - { - event_options.on_success = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_success event", - capability.c_str()); - } ; - - event_options.on_success_param = nullptr; - } - - if (on_stopped_capability != "") - { - event_options.on_stopped = [this, &on_stopped_capability, &capability](tinyxml2::XMLElement* parameters) { - runner_cache_[on_stopped_capability]->trigger(parameters); - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s is triggering %s on_stopped", capability.c_str(), - on_stopped_capability.c_str()); - }; - - tinyxml2::XMLDocument doc; - doc.Parse(on_stopped_parameters.c_str()); - event_options.on_stopped_param = doc.FirstChildElement(); - } - else - { - event_options.on_stopped = [this, &capability](tinyxml2::XMLElement* parameters) { - RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), "%s does not implement on_stopped event", - capability.c_str()); - } ; - - event_options.on_stopped_param = nullptr; - } - - int event_count = runner_cache_[capability]->attach_events(event_options); - - std::string event_name = capability + "_" + std::to_string(event_count); - event_cache_[event_name] = event_options; + event_options.on_started = on_started_capability; + event_options.on_failure = on_failure_capability; + event_options.on_success = on_success_capability; + event_options.on_stopped = on_stopped_capability; + + event_options.on_started_param = on_started_parameters; + event_options.on_failure_param = on_failure_parameters; + event_options.on_success_param = on_success_parameters; + event_options.on_stopped_param = on_stopped_parameters; + + int event_count = runner_cache_[capability]->attach_events( + event_options, std::bind(&capabilities2_server::RunnerCache::trigger_runner, this, std::placeholders::_1, + std::placeholders::_2)); + + RCLCPP_INFO(node_logging_interface_ptr_->get_logger(), + "Configured triggers for capability %s: \n\tStarted: %s \n\tFailure: %s \n\tSuccess: %s \n\tStopped: " + "%s\n", + capability.c_str(), on_started_capability.c_str(), on_failure_capability.c_str(), + on_success_capability.c_str(), on_stopped_capability.c_str()); } /** @@ -336,9 +261,6 @@ class RunnerCache // capability / provider specs -> runner std::map> runner_cache_; - // map events to capabilities - std::map event_cache_; - // runner plugin loader pluginlib::ClassLoader runner_loader_; diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 4f70a3e..a7b4fa8 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): 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=[ From 7b197a0030c7a5165a46987286b189644e97026a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Tue, 7 Jan 2025 14:49:36 +1100 Subject: [PATCH 02/11] updated plans --- capabilities2_fabric/plans/navigation_1.xml | 2 +- capabilities2_fabric/plans/navigation_2.xml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/capabilities2_fabric/plans/navigation_1.xml b/capabilities2_fabric/plans/navigation_1.xml index 3b6de79..4dbae38 100644 --- a/capabilities2_fabric/plans/navigation_1.xml +++ b/capabilities2_fabric/plans/navigation_1.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml index a2569f7..4b0560a 100644 --- a/capabilities2_fabric/plans/navigation_2.xml +++ b/capabilities2_fabric/plans/navigation_2.xml @@ -1,7 +1,7 @@ - - + + \ No newline at end of file From 225db11af636a99f27e0dd13171307b7e2d666df Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 9 Jan 2025 03:28:18 +1100 Subject: [PATCH 03/11] minor fixes --- .../docs/waypoint_runner_ex1.md | 2 +- .../docs/waypoint_runner_ex2.md | 2 +- .../capabilities2_runner/action_runner.hpp | 208 +++++++++++------- .../capabilities2_runner/launch_runner.hpp | 2 +- .../notrigger_action_runner.hpp | 2 +- .../capabilities2_runner/runner_base.hpp | 6 +- .../capabilities2_runner/service_runner.hpp | 2 +- .../capabilities2_runner/topic_runner.hpp | 2 +- capabilities2_runner/src/standard_runners.cpp | 2 +- 9 files changed, 142 insertions(+), 86 deletions(-) diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 76ef73a..555c773 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -39,7 +39,7 @@ colcon build ```bash export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo empty_world.launch.py +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py ``` ### Start the Navigation2 stack diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 6cf251a..01121e9 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -39,7 +39,7 @@ colcon build ```bash export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_gazebo turtlebot3_empty_world.launch.py +ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py ``` ### Start the Navigation2 stack diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 54429ee..06e1135 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -47,15 +48,15 @@ class ActionRunner : public RunnerBase 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()); + RCLCPP_INFO(node_->get_logger(), "[%s] waiting for action: %s", run_config_.interface.c_str(), action_name.c_str()); 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(), + 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"); } - RCLCPP_INFO(node_->get_logger(), "%s connected with action: %s", run_config_.interface.c_str(), + RCLCPP_INFO(node_->get_logger(), "[%s] connected with action: %s", run_config_.interface.c_str(), action_name.c_str()); } @@ -82,28 +83,11 @@ 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"); - RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); - } - - // Trigger on_stopped event if defined - if (events[execute_id].on_stopped != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", - run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_stopped, - update_on_stopped(events[execute_id].on_stopped_param)); - } - }); + auto cancel_future = action_client_->async_cancel_goal( + goal_handle_, std::bind(&ActionRunner::cancellationCallback, this, std::placeholders::_1)); // 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) { @@ -130,7 +114,7 @@ class ActionRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void triggerExecution() override + virtual void execution() override { execute_id += 1; @@ -139,80 +123,135 @@ class ActionRunner : public RunnerBase 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_); RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); - // send goal options - // goal response callback - send_goal_options_.goal_response_callback = - [this](const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - if (goal_handle) - { - // trigger the events related to on_started state - if (events[execute_id].on_started != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", - run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); - } - } - else - { - RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); - } - - // 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) - { - // trigger the events related to on_success state - if (events[execute_id].on_success != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", - run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); - } - } - else - { - // trigger the events related to on_failure state - if (events[execute_id].on_failure != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", - run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); - } - } - - result_ = wrapped_result.result; - }; + // Synchronization tools + action_completed_ = false; // trigger the action client with goal - auto goal_handle_future = action_client_->async_send_goal(goal_msg, send_goal_options_); + goal_handle_future_ = action_client_->async_send_goal(goal_msg_); RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); // 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 - auto goal_handle = goal_handle_future.get(); + goal_handle_ = goal_handle_future_.get(); RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - if (!goal_handle) + if (!goal_handle_) { RCLCPP_INFO(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); return; } + + // Get the result asynchronously + result_future_ = action_client_->async_get_result(goal_handle_); + + while (true) + { + // Check if the cancel operation is complete + if (result_future_.wait_for(std::chrono::milliseconds(200)) == std::future_status::ready) + break; + + RCLCPP_INFO(node_->get_logger(), "[%s] waiting for result..", run_config_.interface.c_str()); + } + + RCLCPP_INFO(node_->get_logger(), "[%s] received result", run_config_.interface.c_str()); + + // Wait for result future + wrapped_result_ = result_future_.get(); + + // convert the result + if (wrapped_result_.code == rclcpp_action::ResultCode::SUCCEEDED) + { + RCLCPP_INFO(node_->get_logger(), "[%s] successfully completed", run_config_.interface.c_str()); + } + else + { + RCLCPP_INFO(node_->get_logger(), "[%s] failed to complete", run_config_.interface.c_str()); + } + + auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(20); + + // std::unique_lock lock(mutex_); + // cv_.wait(lock, [&action_completed] { return action_completed; }); } protected: + // virtual void goalResponseCallback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) + // { + // if (goal_handle) + // { + // RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); + + // // trigger the events related to on_started state + // if (events[execute_id].on_started != "") + // { + // RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", + // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_started, + // update_on_started(events[execute_id].on_started_param)); + // } + // } + // else + // { + // RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); + // } + + // // store goal handle to be used with stop funtion + // goal_handle_ = goal_handle; + // }; + + // virtual void resultCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) + // { + // std::lock_guard lock(mutex_); + + // if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) + // { + // // trigger the events related to on_success state + // if (events[execute_id].on_success != "") + // { + // RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", + // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_success, + // update_on_success(events[execute_id].on_success_param)); + // } + // } + // else + // { + // // trigger the events related to on_failure state + // if (events[execute_id].on_failure != "") + // { + // RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", + // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_failure, + // update_on_failure(events[execute_id].on_failure_param)); + // } + // } + + // result_ = wrapped_result.result; + + // action_completed_ = true; // Mark action as completed + // cv_.notify_one(); // Notify the waiting thread + // }; + + virtual void cancellationCallback(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"); + RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); + } + + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } + } + /** * @brief Generate a goal from parameters * @@ -236,8 +275,25 @@ 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_; + + // Synchronization tools + std::mutex mutex_; + std::condition_variable cv_; + bool action_completed_; }; } // namespace capabilities2_runner diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 22ff606..dcc8781 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -129,7 +129,7 @@ class LaunchRunner : public RunnerBase protected: // throw on triggerExecution function - void triggerExecution() override + void execution() override { throw runner_exception("no triggerExecution() this is a no-trigger action runner"); } diff --git a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp index de9c10b..1287105 100644 --- a/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/notrigger_action_runner.hpp @@ -26,7 +26,7 @@ class NoTriggerActionRunner : public ActionRunner protected: // throw on triggerExecution function - void triggerExecution() override + void execution() override { throw runner_exception("no triggerExecution() 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 5a0db5e..e48cea1 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -124,7 +124,7 @@ class RunnerBase { parameters_ = convert_to_xml(parameters); - executionThread = std::thread(&RunnerBase::triggerExecution, this); + executionThread = std::thread(&RunnerBase::execution, this); } /** @@ -152,7 +152,7 @@ class RunnerBase int attach_events(capabilities2_runner::event_opts& event_option, std::function triggerFunction) { - RCLCPP_INFO(node_->get_logger(), "%s accepted event options with ID : %d ", run_config_.interface.c_str(), + RCLCPP_INFO(node_->get_logger(), "[%s] accepted event options with ID : %d ", run_config_.interface.c_str(), insert_id); triggerFunction_ = triggerFunction; @@ -212,7 +212,7 @@ class RunnerBase * @param parameters pointer to tinyxml2::XMLElement that contains parameters * */ - virtual void triggerExecution() = 0; + virtual void execution() = 0; /** * @brief Update on_started event parameters with new data if avaible. diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 5058784..498d18a 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -58,7 +58,7 @@ class ServiceRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void triggerExecution() override + virtual void execution() override { execute_id += 1; diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 64c26c3..26f5840 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -50,7 +50,7 @@ class TopicRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void triggerExecution() override + virtual void execution() override { execute_id += 1; diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index c01054d..459aa42 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -33,7 +33,7 @@ class DummyRunner : public RunnerBase protected: // throw on triggerExecution function - void triggerExecution() override + void execution() override { RCLCPP_INFO(node_->get_logger(), "Dummy runner does not have triggerExecution()"); } From 5e97907c9abb2f2d166cfa051c5e3ed9afe0bb32 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 9 Jan 2025 16:04:13 +1100 Subject: [PATCH 04/11] added succeed action server component to fabric --- .../capabilities_fabric.hpp | 39 ++++++++++++------- .../capabilities_file_parser.hpp | 32 ++++++--------- 2 files changed, 37 insertions(+), 34 deletions(-) diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp index a1396a7..e6ff0de 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp @@ -164,7 +164,7 @@ class CapabilitiesFabric : public rclcpp::Node // 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"); + RCLCPP_ERROR(this->get_logger(), "Parsing the plan from goal message failed"); return rclcpp_action::GoalResponse::REJECT; } @@ -181,7 +181,7 @@ class CapabilitiesFabric : public rclcpp::Node */ rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Received the request to cancel the plan"); + RCLCPP_ERROR(this->get_logger(), "Received the request to cancel the plan"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; @@ -202,7 +202,11 @@ class CapabilitiesFabric : public rclcpp::Node */ void execution(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Execution started"); + auto feedback = std::make_shared(); + + feedback->progress = "Execution started"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); expected_providers_ = 0; completed_providers_ = 0; @@ -228,8 +232,7 @@ class CapabilitiesFabric : public rclcpp::Node feedback->progress = "Requesting Interface information"; goal_handle->publish_feedback(feedback); - - RCLCPP_INFO(this->get_logger(), "Requesting Interface information"); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); auto request_interface = std::make_shared(); @@ -240,18 +243,20 @@ class CapabilitiesFabric : public rclcpp::Node if (!future.valid()) { - RCLCPP_ERROR(this->get_logger(), "Failed to get Interface information"); - result->success = false; result->message = "Failed to get Interface information"; goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_INFO(this->get_logger(), "Server Execution Cancelled"); + RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); return; } auto response = future.get(); - RCLCPP_INFO(this->get_logger(), "Received Interface information"); + + feedback->progress = "Received Interface information"; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); expected_interfaces_ = response->interfaces.size(); @@ -274,7 +279,6 @@ class CapabilitiesFabric : public rclcpp::Node RCLCPP_INFO(this->get_logger(), ""); feedback->progress = "Requesting semantic interfaces for " + requested_interface; goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); auto request_semantic = std::make_shared(); @@ -290,7 +294,7 @@ class CapabilitiesFabric : public rclcpp::Node result->message = "Failed to get Semantic Interface information"; goal_handle->abort(result); - RCLCPP_INFO(this->get_logger(), "Failed to get Semantic Interface information. Server Execution Cancelled"); + RCLCPP_ERROR(this->get_logger(), "Failed to get Semantic Interface information. Server Execution Cancelled"); return; } @@ -320,7 +324,6 @@ class CapabilitiesFabric : public rclcpp::Node feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + requested_interface + ". So added " + requested_interface; goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); } @@ -331,8 +334,10 @@ class CapabilitiesFabric : public rclcpp::Node } else { + feedback->progress = "Received all requested Interface information"; + goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), "Received all requested Interface information"); + RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); expected_providers_ = interface_list.size(); @@ -588,7 +593,7 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); bond_id = response->bond_id; - feedback->progress = "Received the bond id : " + bond_id ; + feedback->progress = "Received the bond id : " + bond_id; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); @@ -888,6 +893,12 @@ class CapabilitiesFabric : public rclcpp::Node feedback->progress = "Successfully triggered capability " + connection_map[0].source.runner; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + auto result = std::make_shared(); + result->success = true; + result->message = "Successfully launched capabilities2 fabric"; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), result->message.c_str()); }); } diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp index c576712..8ca636a 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp @@ -38,14 +38,20 @@ class CapabilitiesFileParser : public rclcpp::Node // Create the action client for capabilities_fabric after the node is fully constructed this->client_ptr_ = rclcpp_action::create_client(shared_from_this(), "/capabilities_fabric"); - // Set up a timer to call send_goal after a short delay - this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&CapabilitiesFileParser::send_goal, this)); + if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(5))) + { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + return; + } + + RCLCPP_INFO(this->get_logger(), "Sucessfully connected to the capabilities_fabric action server"); + + send_goal(); } void send_goal() { - this->timer_->cancel(); - // try to load the file tinyxml2::XMLError xml_status = document.LoadFile(plan_file_path.c_str()); @@ -58,20 +64,11 @@ class CapabilitiesFileParser : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "Plan loaded from : %s", plan_file_path.c_str()); - if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(5))) - { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - return; - } - - RCLCPP_INFO(this->get_logger(), "Sucessfully connected to the capabilities_fabric action server"); - auto goal_msg = capabilities2_msgs::action::Plan::Goal(); capabilities2_xml_parser::convert_to_string(document, goal_msg.plan); - RCLCPP_INFO(this->get_logger(), "Following plan was read:\n\n%s ", goal_msg.plan.c_str()); + RCLCPP_INFO(this->get_logger(), "Following plan was loaded :\n\n%s ", goal_msg.plan.c_str()); RCLCPP_INFO(this->get_logger(), "Sending goal to the capabilities_fabric action server"); @@ -110,7 +107,7 @@ class CapabilitiesFileParser : public rclcpp::Node if (result.result->success) { - RCLCPP_ERROR(this->get_logger(), "Plan executed successfully"); + RCLCPP_INFO(this->get_logger(), "Plan executed successfully"); } else { @@ -124,8 +121,6 @@ class CapabilitiesFileParser : public rclcpp::Node 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); @@ -140,7 +135,4 @@ class CapabilitiesFileParser : public rclcpp::Node /** action client */ rclcpp_action::Client::SharedPtr client_ptr_; - - /** action server */ - rclcpp::TimerBase::SharedPtr timer_; }; \ No newline at end of file From 330a7e5d9450e925320aee946806a3b47ff8a216 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 10 Jan 2025 00:56:46 +1100 Subject: [PATCH 05/11] added feedback to information --- .../capabilities2_runner/action_runner.hpp | 159 ++++++++---------- .../listen_runner.hpp | 15 +- .../speak_runner.hpp | 13 ++ .../waypoint_runner.hpp | 15 ++ .../prompt_plan_response_runner.hpp | 13 ++ 5 files changed, 123 insertions(+), 92 deletions(-) diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 06e1135..1b2e82b 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -124,117 +124,82 @@ class ActionRunner : public RunnerBase // generate a goal from parameters if provided goal_msg_ = generate_goal(parameters_); - RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); - // Synchronization tools - action_completed_ = false; - // trigger the action client with goal - goal_handle_future_ = action_client_->async_send_goal(goal_msg_); - - RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); - - // 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 + send_goal_options_.goal_response_callback = + std::bind(&ActionRunner::goalResponseCallback, this, std::placeholders::_1); + send_goal_options_.feedback_callback = + std::bind(&ActionRunner::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); + send_goal_options_.result_callback = std::bind(&ActionRunner::resultCallback, this, std::placeholders::_1); - goal_handle_ = goal_handle_future_.get(); + goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); - RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + } - if (!goal_handle_) +protected: + virtual void goalResponseCallback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) + { + if (goal_handle) { - RCLCPP_INFO(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); - return; - } - - // Get the result asynchronously - result_future_ = action_client_->async_get_result(goal_handle_); + RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - while (true) + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", run_config_.interface.c_str()); + // triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } + } + else { - // Check if the cancel operation is complete - if (result_future_.wait_for(std::chrono::milliseconds(200)) == std::future_status::ready) - break; - - RCLCPP_INFO(node_->get_logger(), "[%s] waiting for result..", run_config_.interface.c_str()); + RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); } - RCLCPP_INFO(node_->get_logger(), "[%s] received result", run_config_.interface.c_str()); + // store goal handle to be used with stop funtion + goal_handle_ = goal_handle; + }; - // Wait for result future - wrapped_result_ = result_future_.get(); + virtual void resultCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) + { + RCLCPP_INFO(node_->get_logger(), "[%s] received result", run_config_.interface.c_str()); - // convert the result - if (wrapped_result_.code == rclcpp_action::ResultCode::SUCCEEDED) + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { - RCLCPP_INFO(node_->get_logger(), "[%s] successfully completed", run_config_.interface.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s] action succeeded.", run_config_.interface.c_str()); + + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", run_config_.interface.c_str()); + // triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } } else { - RCLCPP_INFO(node_->get_logger(), "[%s] failed to complete", run_config_.interface.c_str()); + RCLCPP_ERROR(node_->get_logger(), "[%s] action failed.", run_config_.interface.c_str()); + RCLCPP_ERROR(node_->get_logger(), "[%s] error code: %d", run_config_.interface.c_str(), + static_cast(wrapped_result.code)); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", run_config_.interface.c_str()); + // triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } } - auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(20); + result_ = wrapped_result.result; + }; - // std::unique_lock lock(mutex_); - // cv_.wait(lock, [&action_completed] { return action_completed; }); - } + virtual void feedbackCallback(typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + const typename ActionT::Feedback::ConstSharedPtr feedback_msg) + { + std::string feedback = generate_feedback(feedback_msg); -protected: - // virtual void goalResponseCallback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) - // { - // if (goal_handle) - // { - // RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - - // // trigger the events related to on_started state - // if (events[execute_id].on_started != "") - // { - // RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", - // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_started, - // update_on_started(events[execute_id].on_started_param)); - // } - // } - // else - // { - // RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); - // } - - // // store goal handle to be used with stop funtion - // goal_handle_ = goal_handle; - // }; - - // virtual void resultCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) - // { - // std::lock_guard lock(mutex_); - - // if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - // { - // // trigger the events related to on_success state - // if (events[execute_id].on_success != "") - // { - // RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", - // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_success, - // update_on_success(events[execute_id].on_success_param)); - // } - // } - // else - // { - // // trigger the events related to on_failure state - // if (events[execute_id].on_failure != "") - // { - // RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", - // run_config_.interface.c_str()); triggerFunction_(events[execute_id].on_failure, - // update_on_failure(events[execute_id].on_failure_param)); - // } - // } - - // result_ = wrapped_result.result; - - // action_completed_ = true; // Mark action as completed - // cv_.notify_one(); // Notify the waiting thread - // }; + RCLCPP_INFO(node_->get_logger(), "[%s] received feedback: %s", run_config_.interface.c_str(), feedback.c_str()); + }; virtual void cancellationCallback(action_msgs::srv::CancelGoal_Response::SharedPtr response) { @@ -248,7 +213,7 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_stopped != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + // triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } } @@ -265,6 +230,18 @@ class ActionRunner : public RunnerBase */ virtual typename ActionT::Goal generate_goal(tinyxml2::XMLElement* parameters) = 0; + /** + * @brief Generate a std::string from feedback message + * + * This function is used to convert feedback messages into generic strings + * + * A pattern needs to be implemented in the derived class + * + * @param parameters + * @return ActionT::Feedback the received feedback + */ + virtual std::string generate_feedback(const typename ActionT::Feedback::ConstSharedPtr msg) = 0; + /**< action client */ typename rclcpp_action::Client::SharedPtr action_client_; diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp index 59d2e8a..35ca8d2 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -55,6 +55,19 @@ class ListenerRunner : public ActionRunner return goal_msg; } + /** + * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() + * + * @param msg feedback message from the action server + * @return std::string of feedback information + */ + virtual std::string + generate_feedback(const typename hri_audio_msgs::action::SpeechToText::Feedback::ConstSharedPtr msg) override + { + std::string feedback = ""; + return feedback; + } + /** * @brief Update on_success event parameters with new data if avaible. * @@ -69,7 +82,7 @@ class ListenerRunner : public ActionRunner virtual std::string update_on_success(std::string& parameters) { tinyxml2::XMLElement* element = convert_to_xml(parameters); - + // Create the Pose element as a child of the existing parameters element tinyxml2::XMLElement* textElement = element->GetDocument()->NewElement("Text"); element->InsertEndChild(textElement); diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index 925d90f..7f55443 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -59,6 +59,19 @@ class SpeakerRunner : public ActionRunner return goal_msg; } + /** + * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() + * + * @param msg feedback message from the action server + * @return std::string of feedback information + */ + virtual std::string + generate_feedback(const typename hri_audio_msgs::action::TextToSpeech::Feedback::ConstSharedPtr msg) override + { + std::string feedback = ""; + return feedback; + } + }; } // namespace capabilities2_runner \ No newline at end of file diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp index 8d30bba..ebbf9b3 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -68,6 +69,20 @@ class WayPointRunner : public ActionRunner return goal_msg; } + /** + * @brief This generate feedback function overrides the generate_feedback() function from ActionRunner() + * + * @param msg feedback message from the action server + * @return std::string of feedback information + */ + virtual std::string + generate_feedback(const typename nav2_msgs::action::NavigateToPose::Feedback::ConstSharedPtr msg) override + { + std::string feedback = "x: " + std::to_string(msg->current_pose.pose.position.x) + + " y: " + std::to_string(msg->current_pose.pose.position.y); + return feedback; + } + protected: std::string global_frame_; /**The global frame of the robot*/ std::string robot_base_frame_; /**The frame of the robot base*/ diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp index 49557d3..67a5c31 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -60,6 +60,19 @@ class PromptPlanResponseRunner : public ActionRunnerprogress; + return feedback; + } + /** * @brief Update on_failure event parameters with new data if avaible. * From 2d4db8583f1a9d4ae83122f26dbdf7d2e3c498f8 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Fri, 10 Jan 2025 16:31:32 +1100 Subject: [PATCH 06/11] added con-composable node --- capabilities2_fabric/CMakeLists.txt | 1 + capabilities2_fabric/package.xml | 1 + .../src/capabilities_fabric.cpp | 3 +- capabilities2_server/CMakeLists.txt | 42 +++++++++++++-- .../capabilities_server.hpp | 2 +- capabilities2_server/launch/server.launch.py | 24 +++------ .../launch/server_composed.launch.py | 51 +++++++++++++++++++ capabilities2_server/package.xml | 1 + ...erver.cpp => capabilities_server_comp.cpp} | 2 +- .../src/capabilities_server_node.cpp | 22 ++++++++ 10 files changed, 124 insertions(+), 25 deletions(-) create mode 100644 capabilities2_server/launch/server_composed.launch.py rename capabilities2_server/src/{capabilities_server.cpp => capabilities_server_comp.cpp} (94%) create mode 100644 capabilities2_server/src/capabilities_server_node.cpp diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index e7f275c..36639d6 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(backward_ros REQUIRED) # Locate the static version of tinyxml2 find_library(TINYXML2_LIB NAMES tinyxml2 PATHS /usr/local/lib NO_DEFAULT_PATH) diff --git a/capabilities2_fabric/package.xml b/capabilities2_fabric/package.xml index 25f55e8..43af93a 100644 --- a/capabilities2_fabric/package.xml +++ b/capabilities2_fabric/package.xml @@ -13,6 +13,7 @@ rclcpp capabilities2_msgs rclcpp_action + backward_ros tinyxml2 diff --git a/capabilities2_fabric/src/capabilities_fabric.cpp b/capabilities2_fabric/src/capabilities_fabric.cpp index 006bcc2..1a1f902 100644 --- a/capabilities2_fabric/src/capabilities_fabric.cpp +++ b/capabilities2_fabric/src/capabilities_fabric.cpp @@ -1,6 +1,6 @@ #include -int main(int argc, char* argv[]) +int main(int argc, char** argv) { rclcpp::init(argc, argv); @@ -9,6 +9,7 @@ int main(int argc, char* argv[]) // Initialize the node components after construction node->initialize(); + rclcpp::spin(node); rclcpp::shutdown(); diff --git a/capabilities2_server/CMakeLists.txt b/capabilities2_server/CMakeLists.txt index cb79c7f..3eb130c 100644 --- a/capabilities2_server/CMakeLists.txt +++ b/capabilities2_server/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp_components REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) +find_package(backward_ros REQUIRED) find_package(PkgConfig) pkg_check_modules(UUID REQUIRED uuid) @@ -43,8 +44,10 @@ include_directories(include ${UUID_INCLUDE_DIRS} ) +# Component based implementation that compiles as a library + add_library(${PROJECT_NAME} SHARED - src/capabilities_server.cpp + src/capabilities_server_comp.cpp ) target_link_libraries(${PROJECT_NAME} @@ -69,19 +72,50 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "capabilities2_server::CapabilitiesServer" - EXECUTABLE capabilities2_server_node + EXECUTABLE capabilities2_server_component_node ) -ament_export_targets(capabilities2_server_node) +ament_export_targets(capabilities2_server_component_node) install(TARGETS ${PROJECT_NAME} - EXPORT capabilities2_server_node + EXPORT capabilities2_server_component_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +# Non Component based implementation that compiles as a executable + +add_executable(${PROJECT_NAME}_node + src/capabilities_server_node.cpp +) + +target_link_libraries(${PROJECT_NAME}_node + ${UUID_LIBRARIES} + ${TINYXML2_LIB} + ${SQLITE3_LIBRARIES} + ${YAML_CPP_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + rclcpp_action + bondcpp + pluginlib + rclcpp_components + capabilities2_msgs + capabilities2_runner + 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} diff --git a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp index b5c5edd..517c065 100644 --- a/capabilities2_server/include/capabilities2_server/capabilities_server.hpp +++ b/capabilities2_server/include/capabilities2_server/capabilities_server.hpp @@ -60,7 +60,7 @@ class CapabilitiesServer : public rclcpp::Node, public CapabilitiesAPI { public: CapabilitiesServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) - : Node("capabilities", options), CapabilitiesAPI() + : Node("capabilities2", options), CapabilitiesAPI() { // params interface // loop rate diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index a7b4fa8..6c63d94 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -5,8 +5,6 @@ import os from launch import LaunchDescription from launch_ros.actions import Node -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode from ament_index_python.packages import get_package_share_directory @@ -20,21 +18,11 @@ def generate_launch_description(): 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_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=[server_config] - ) - ] + capabilities2 = Node( + package='capabilities2_server', + executable='capabilities2_server_node', + name='capabilities', + parameters=[server_config] ) # create launch proxy node @@ -46,6 +34,6 @@ def generate_launch_description(): # return return LaunchDescription([ - capabilities, + capabilities2, launch_proxy ]) diff --git a/capabilities2_server/launch/server_composed.launch.py b/capabilities2_server/launch/server_composed.launch.py new file mode 100644 index 0000000..a7b4fa8 --- /dev/null +++ b/capabilities2_server/launch/server_composed.launch.py @@ -0,0 +1,51 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 server + """ + # load config file + 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_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=[server_config] + ) + ] + ) + + # create launch proxy node + launch_proxy = Node( + package='capabilities2_launch_py', + executable='capabilities2_launch_py', + name='capabilities2_launch_py' + ) + + # return + return LaunchDescription([ + capabilities, + launch_proxy + ]) diff --git a/capabilities2_server/package.xml b/capabilities2_server/package.xml index ffa1e48..e9fb76d 100644 --- a/capabilities2_server/package.xml +++ b/capabilities2_server/package.xml @@ -28,6 +28,7 @@ tinyxml2 libsqlite3-dev uuid + backward_ros rclpy launch_ros diff --git a/capabilities2_server/src/capabilities_server.cpp b/capabilities2_server/src/capabilities_server_comp.cpp similarity index 94% rename from capabilities2_server/src/capabilities_server.cpp rename to capabilities2_server/src/capabilities_server_comp.cpp index 8c8eb6f..efbad0e 100644 --- a/capabilities2_server/src/capabilities_server.cpp +++ b/capabilities2_server/src/capabilities_server_comp.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..48e5bb6 --- /dev/null +++ b/capabilities2_server/src/capabilities_server_node.cpp @@ -0,0 +1,22 @@ +#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(); + 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 From 07acc1f46809059ef3ddde1f51469ae30aca5644 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 13 Jan 2025 01:42:10 +1100 Subject: [PATCH 07/11] fixed statetransition error --- capabilities2_fabric/CMakeLists.txt | 2 + .../docs/waypoint_runner_ex1.md | 21 +----- .../docs/waypoint_runner_ex2.md | 21 +----- .../capabilities_fabric.hpp | 26 +++++--- .../capabilities_file_parser.hpp | 2 +- .../capabilities_xml_parser.hpp | 2 +- .../structs/bond_client.hpp | 65 +++++++++++++++++++ .../structs/connection.hpp | 0 .../src/capabilities_fabric.cpp | 2 +- .../src/capabilities_file_parser.cpp | 2 +- .../capabilities2_runner/action_runner.hpp | 3 - .../capabilities2_server/bond_cache.hpp | 4 +- docs/nav2_setup.md | 19 +++++- 13 files changed, 111 insertions(+), 58 deletions(-) rename capabilities2_fabric/include/{capabilities2_executor => capabilities2_fabric}/capabilities_fabric.hpp (98%) rename capabilities2_fabric/include/{capabilities2_executor => capabilities2_fabric}/capabilities_file_parser.hpp (98%) rename capabilities2_fabric/include/{capabilities2_executor => capabilities2_fabric}/capabilities_xml_parser.hpp (99%) create mode 100644 capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp rename capabilities2_fabric/include/{capabilities2_executor => capabilities2_fabric}/structs/connection.hpp (100%) diff --git a/capabilities2_fabric/CMakeLists.txt b/capabilities2_fabric/CMakeLists.txt index 36639d6..9faebc0 100644 --- a/capabilities2_fabric/CMakeLists.txt +++ b/capabilities2_fabric/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(capabilities2_msgs REQUIRED) +find_package(bondcpp REQUIRED) find_package(backward_ros REQUIRED) # Locate the static version of tinyxml2 @@ -39,6 +40,7 @@ target_link_libraries(${PROJECT_NAME} ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_action + bondcpp capabilities2_msgs ) diff --git a/capabilities2_fabric/docs/waypoint_runner_ex1.md b/capabilities2_fabric/docs/waypoint_runner_ex1.md index 555c773..4e978e8 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex1.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex1.md @@ -2,26 +2,7 @@ ### Dependencies -This example uses nav2 stack and turtlebot3. - -Run the following commands to install nav2 stack - -```bash -sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup -``` - -Run the following commands to install turtlebot3 - -```bash -sudo apt install ros-humble-turtlebot3* -``` - -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 -``` +This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. ### Plan selection diff --git a/capabilities2_fabric/docs/waypoint_runner_ex2.md b/capabilities2_fabric/docs/waypoint_runner_ex2.md index 01121e9..8a49adb 100644 --- a/capabilities2_fabric/docs/waypoint_runner_ex2.md +++ b/capabilities2_fabric/docs/waypoint_runner_ex2.md @@ -2,26 +2,7 @@ ### Dependencies -This example uses nav2 stack and turtlebot3. - -Run the following commands to install nav2 stack - -```bash -sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup -``` - -Run the following commands to install turtlebot3 - -```bash -sudo apt install ros-humble-turtlebot3* -``` - -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 -``` +This example uses nav2 stack and turtlebot3. Follow instructions from [Nav2 Dependency Installation](../../docs/nav2_setup.md) to setup nav stack. ### Plan selection diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp similarity index 98% rename from capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index e6ff0de..6826ba4 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -8,7 +8,8 @@ #include #include -#include +#include +#include #include @@ -591,12 +592,15 @@ class CapabilitiesFabric : public rclcpp::Node } auto response = future.get(); - bond_id = response->bond_id; + bond_id_ = response->bond_id; - feedback->progress = "Received the bond id : " + bond_id; + feedback->progress = "Received the bond id : " + bond_id_; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + bond_client_ = std::make_unique(shared_from_this(), bond_id_); + bond_client_->start(); + expected_capabilities_ = connection_map.size(); RCLCPP_INFO(this->get_logger(), "Requsting start of %d capabilities", expected_capabilities_); @@ -608,9 +612,8 @@ class CapabilitiesFabric : public rclcpp::Node /** * @brief Request use of capability from capabilities2 server * - * @param capability capability name to be started + * @param capabilities capability list to be started * @param provider provider of the capability - * @param bond_id bond_id for the capability */ void use_capability(std::map& capabilities, const std::shared_ptr goal_handle) { @@ -622,7 +625,7 @@ class CapabilitiesFabric : public rclcpp::Node auto request_use = std::make_shared(); request_use->capability = capability; request_use->preferred_provider = provider; - request_use->bond_id = bond_id; + request_use->bond_id = bond_id_; feedback->progress = "Starting capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; @@ -701,7 +704,7 @@ class CapabilitiesFabric : public rclcpp::Node auto request_free = std::make_shared(); request_free->capability = capability; - request_free->bond_id = bond_id; + request_free->bond_id = bond_id_; // send the request auto result_future = free_capability_client_->async_send_request( @@ -721,6 +724,8 @@ class CapabilitiesFabric : public rclcpp::Node feedback->progress = "Successfully freed capability " + capability; goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + bond_client_->stop(); }); } @@ -921,8 +926,11 @@ class CapabilitiesFabric : public rclcpp::Node int expected_configurations_; int completed_configurations_; - /** Bond ID between capabilities server and this client */ - std::string bond_id; + /** Bond id */ + std::string bond_id_; + + /** Bond between capabilities server and this client */ + std::shared_ptr bond_client_; /** XML Document */ tinyxml2::XMLDocument document; diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp similarity index 98% rename from capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp index 8ca636a..d7614e9 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_file_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_file_parser.hpp @@ -10,7 +10,7 @@ #include #include -#include +#include #include /** diff --git a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp similarity index 99% rename from capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp rename to capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp index fedbc53..38ff3d3 100644 --- a/capabilities2_fabric/include/capabilities2_executor/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include namespace capabilities2_xml_parser { diff --git a/capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp new file mode 100644 index 0000000..b1f286a --- /dev/null +++ b/capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp @@ -0,0 +1,65 @@ +#include +#include + +class BondClient +{ +public: + BondClient(rclcpp::Node::SharedPtr node, const std::string& bond_id, const std::string& bonds_topic = "/capabilities/bond") + { + bonds_topic_ = bonds_topic; + bond_id_ = bond_id; + node_ = node; + } + + void start() + { + RCLCPP_INFO(node_->get_logger(), "[BondClient] creating bond to capabilities server"); + + bond_ = + std::make_unique(bonds_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(); + } + + void stop() + { + RCLCPP_INFO(node_->get_logger(), "[BondClient] destroying bond to capabilities server"); + + if (bond_) + { + bond_.reset(); + } + } + + ~BondClient() + { + stop(); + } + +private: + void on_formed() + { + // log bond established event + RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server formed with id: %s", bond_id_.c_str()); + } + + void on_broken() + { + // log bond established event + RCLCPP_INFO(node_->get_logger(), "[BondClient] bond with capabilities server broken with id: %s", bond_id_.c_str()); + } + + /** Ros node pointer */ + rclcpp::Node::SharedPtr node_; + + /** Bond id string */ + std::string bond_id_; + + /** Bond topic to be published */ + std::string bonds_topic_; + + /** Heart beat bond with capabilities server */ + std::shared_ptr bond_; +}; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_executor/structs/connection.hpp b/capabilities2_fabric/include/capabilities2_fabric/structs/connection.hpp similarity index 100% rename from capabilities2_fabric/include/capabilities2_executor/structs/connection.hpp rename to capabilities2_fabric/include/capabilities2_fabric/structs/connection.hpp diff --git a/capabilities2_fabric/src/capabilities_fabric.cpp b/capabilities2_fabric/src/capabilities_fabric.cpp index 1a1f902..e89ccbb 100644 --- a/capabilities2_fabric/src/capabilities_fabric.cpp +++ b/capabilities2_fabric/src/capabilities_fabric.cpp @@ -1,4 +1,4 @@ -#include +#include int main(int argc, char** argv) { diff --git a/capabilities2_fabric/src/capabilities_file_parser.cpp b/capabilities2_fabric/src/capabilities_file_parser.cpp index aa0d9b0..bb08cf0 100644 --- a/capabilities2_fabric/src/capabilities_file_parser.cpp +++ b/capabilities2_fabric/src/capabilities_file_parser.cpp @@ -1,4 +1,4 @@ -#include +#include int main(int argc, char* argv[]) { diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index 1b2e82b..f308821 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -94,9 +94,6 @@ class ActionRunner : public RunnerBase // Check if the cancel operation is complete if (cancel_future.wait_for(std::chrono::milliseconds(100)) == std::future_status::ready) break; - - // Spin some work on the node to keep the callback within scope - rclcpp::spin_some(node_); } } catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) diff --git a/capabilities2_server/include/capabilities2_server/bond_cache.hpp b/capabilities2_server/include/capabilities2_server/bond_cache.hpp index 296ddcc..4895107 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) { } @@ -124,6 +124,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/docs/nav2_setup.md b/docs/nav2_setup.md index 1b27fe5..2a44385 100644 --- a/docs/nav2_setup.md +++ b/docs/nav2_setup.md @@ -2,8 +2,16 @@ ## Install nav2 stack +Run the following commands to install nav2 stack + +```bash +sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup +``` + +## Install slam-toolbox + ```bash -sudo apt install ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-slam-toolbox +sudo apt install ros-$ROS_DISTRO-slam-toolbox ``` ## Clone configuration @@ -16,8 +24,17 @@ git clone https://github.com/CollaborativeRoboticsLab/nav_stack.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 From 28fac047df4c9cd297b2f44f3a4c1c3dcd52149a Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Mon, 13 Jan 2025 21:20:47 +1100 Subject: [PATCH 08/11] refactored the fabric code --- .../capabilities_fabric.hpp | 544 +++++++----------- .../capabilities_xml_parser.hpp | 2 +- .../{structs => utils}/bond_client.hpp | 0 .../{structs => utils}/connection.hpp | 0 4 files changed, 203 insertions(+), 343 deletions(-) rename capabilities2_fabric/include/capabilities2_fabric/{structs => utils}/bond_client.hpp (100%) rename capabilities2_fabric/include/capabilities2_fabric/{structs => utils}/connection.hpp (100%) diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp index 6826ba4..3deec48 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_fabric.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include @@ -76,73 +76,21 @@ class CapabilitiesFabric : public rclcpp::Node establish_bond_client_ = this->create_client("/capabilities/establish_bond"); use_capability_client_ = this->create_client("/capabilities/use_capability"); free_capability_client_ = this->create_client("/capabilities/free_capability"); - trigger_capability_client_ = this->create_client("/capabilities/trigger_capability"); - configure_capability_client_ = this->create_client("/capabilities/configure_capability"); + trig_capability_client_ = this->create_client("/capabilities/trigger_capability"); + conf_capability_client_ = this->create_client("/capabilities/configure_capability"); // Wait for services to become available - while (!get_interfaces_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/get_interfaces not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/get_interfaces connected"); - - while (!get_sem_interf_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/get_semantic_interfaces not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/get_semantic_interfaces connected"); - - while (!get_providers_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/get_providers not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/get_providers connected"); - - while (!establish_bond_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/establish_bond not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/establish_bond connected"); - - while (!use_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/use_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/use_capability connected"); - - while (!free_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/free_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/free_capability connected"); - - while (!trigger_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/trigger_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/trigger_capability connected"); - - while (!configure_capability_client_->wait_for_service(std::chrono::seconds(1))) - { - RCLCPP_ERROR(this->get_logger(), "/capabilities/configure_capability not available"); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO(this->get_logger(), "/capabilities/configure_capability connected"); + check_service(!get_interfaces_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_interfaces"); + check_service(!get_sem_interf_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_semantic_interfaces"); + check_service(!get_providers_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/get_providers"); + check_service(!establish_bond_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/establish_bond"); + check_service(!use_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/use_capability"); + check_service(!free_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/free_capability"); + check_service(!trig_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/trigger_capability"); + check_service(!conf_capability_client_->wait_for_service(std::chrono::seconds(1)), "/capabilities/configure_capability"); + + feedback_msg = std::make_shared(); + result_msg = std::make_shared(); } private: @@ -203,11 +151,7 @@ class CapabilitiesFabric : public rclcpp::Node */ void execution(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - - feedback->progress = "Execution started"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Execution started", false); expected_providers_ = 0; completed_providers_ = 0; @@ -229,43 +173,29 @@ class CapabilitiesFabric : public rclcpp::Node */ void getInterfaces(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - - feedback->progress = "Requesting Interface information"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Requesting Interface information", false); auto request_interface = std::make_shared(); // request data from the server - auto result_future = - get_interfaces_client_->async_send_request(request_interface, [this, goal_handle, feedback](GetInterfacesClient::SharedFuture future) { - auto result = std::make_shared(); - - if (!future.valid()) - { - result->success = false; - result->message = "Failed to get Interface information"; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - return; - } - - auto response = future.get(); + auto result_future = get_interfaces_client_->async_send_request(request_interface, [this, goal_handle](GetInterfacesClient::SharedFuture future) { + auto result = std::make_shared(); - feedback->progress = "Received Interface information"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + if (!future.valid()) + { + print_and_result(goal_handle, "Failed to get Interface information. Server execution cancelled", false); + return; + } - expected_interfaces_ = response->interfaces.size(); + auto response = future.get(); + expected_interfaces_ = response->interfaces.size(); - RCLCPP_INFO(this->get_logger(), "Requsting Semantic Interface information for %d interfaces", expected_interfaces_); + status = "Received Interfaces. Requsting " + std::to_string(expected_interfaces_) + " semantic interface information"; + print_and_feedback(goal_handle, status, false); - // Request each interface recursively for Semantic interfaces - getSemanticInterfaces(response->interfaces, goal_handle); - }); + // Request each interface recursively for Semantic interfaces + getSemanticInterfaces(response->interfaces, goal_handle); + }); } /** @@ -273,29 +203,20 @@ class CapabilitiesFabric : public rclcpp::Node */ void getSemanticInterfaces(const std::vector& interfaces, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - std::string requested_interface = interfaces[completed_interfaces_]; - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Requesting semantic interfaces for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Requesting semantic interfaces for " + requested_interface; + print_and_feedback(goal_handle, status, true); auto request_semantic = std::make_shared(); request_semantic->interface = requested_interface; // request semantic interface from the server auto result_semantic_future = get_sem_interf_client_->async_send_request( - request_semantic, [this, goal_handle, feedback, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { + request_semantic, [this, goal_handle, interfaces, requested_interface](GetSemanticInterfacesClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to get Semantic Interface information"; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), "Failed to get Semantic Interface information. Server Execution Cancelled"); + print_and_result(goal_handle, "Failed to get Semantic Interface information. Server execution cancelled", false); return; } @@ -310,10 +231,9 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(semantic_interface); is_semantic_list.push_back(true); - feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + - semantic_interface + " for " + requested_interface + ". So added " + semantic_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = (completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received " + semantic_interface + " for " + + requested_interface + ". So added " + semantic_interface; + print_and_feedback(goal_handle, status, false); } } // if no semantic interfaces are availble for a given interface, add the interface instead @@ -322,10 +242,9 @@ class CapabilitiesFabric : public rclcpp::Node interface_list.push_back(requested_interface); is_semantic_list.push_back(false); - feedback->progress = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + - requested_interface + ". So added " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_interfaces_) + "/" + std::to_string(expected_interfaces_) + " : Received none for " + + requested_interface + ". So added " + requested_interface; + print_and_feedback(goal_handle, status, false); } if (completed_interfaces_ != expected_interfaces_) @@ -335,14 +254,12 @@ class CapabilitiesFabric : public rclcpp::Node } else { - feedback->progress = "Received all requested Interface information"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Received all requested Interface information", true); expected_providers_ = interface_list.size(); - RCLCPP_INFO(this->get_logger(), "Requsting Provider information for %d providers", expected_providers_); + status = "Requsting Provider information for " + std::to_string(expected_providers_) + " providers"; + print_and_feedback(goal_handle, status, false); // request providers from the interfaces in the interfaces_list getProvider(interface_list, is_semantic_list, goal_handle); @@ -360,12 +277,8 @@ class CapabilitiesFabric : public rclcpp::Node std::string requested_interface = interfaces[completed_providers_]; bool semantic_flag = is_semantic[completed_providers_]; - auto feedback = std::make_shared(); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Requesting provider for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Requesting provider for " + requested_interface; + print_and_feedback(goal_handle, status, true); auto request_providers = std::make_shared(); @@ -374,15 +287,11 @@ class CapabilitiesFabric : public rclcpp::Node request_providers->include_semantic = semantic_flag; auto result_providers_future = get_providers_client_->async_send_request( - request_providers, [this, is_semantic, requested_interface, interfaces, goal_handle, feedback](GetProvidersClient::SharedFuture future) { + request_providers, [this, is_semantic, requested_interface, interfaces, goal_handle](GetProvidersClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Did not retrieve providers for interface: " + requested_interface; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + status = "Did not retrieve providers for interface: " + requested_interface; + print_and_result(goal_handle, status, false); return; } @@ -394,10 +303,9 @@ class CapabilitiesFabric : public rclcpp::Node // add defualt provider to the list providers_list.push_back(response->default_provider); - feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + - response->default_provider + " for " + requested_interface + ". So added " + response->default_provider; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received " + response->default_provider + + " for " + requested_interface + ". So added " + response->default_provider; + print_and_feedback(goal_handle, status, false); } // add additional providers to the list if available @@ -406,18 +314,16 @@ class CapabilitiesFabric : public rclcpp::Node for (const auto& provider : response->providers) { providers_list.push_back(provider); - feedback->progress = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + - provider + " for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : Received and added " + provider + + " for " + requested_interface; + print_and_feedback(goal_handle, status, false); } } else { - feedback->progress = - std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_providers_) + "/" + std::to_string(expected_providers_) + " : No providers for " + requested_interface; + print_and_feedback(goal_handle, status, false); } // Check if all expected calls are completed before calling verify_plan @@ -428,12 +334,7 @@ class CapabilitiesFabric : public rclcpp::Node } else { - auto feedback = std::make_shared(); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested interface, semantic interface and provider data recieved"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "All requested interface, semantic interface and provider data recieved", true); verify_and_continue(goal_handle); } @@ -446,23 +347,17 @@ class CapabilitiesFabric : public rclcpp::Node */ void verify_and_continue(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - feedback->progress = "Verifying the plan"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Verifying the plan", false); // verify the plan if (!verify_plan(goal_handle)) { - feedback->progress = "Plan verification failed"; - goal_handle->publish_feedback(feedback); - RCLCPP_ERROR(this->get_logger(), feedback->progress.c_str()); - - auto result = std::make_shared(); + print_and_feedback(goal_handle, "Plan verification failed", false); if (rejected_list.size() > 0) { // TODO: improve with error codes + auto result = std::make_shared(); result->success = false; result->message = "Plan verification failed. There are mismatched events"; @@ -478,32 +373,23 @@ class CapabilitiesFabric : public rclcpp::Node else { // TODO: improve with error codes - result->success = false; - result->message = "Plan verification failed."; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + print_and_result(goal_handle, "Plan verification failed. Server Execution Cancelled.", false); } RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); } - feedback->progress = "Plan verification successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Plan verification successful", false); // extract the plan from the XMLDocument tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - feedback->progress = "Plan conversion successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Plan conversion successful", false); // Extract the connections from the plan capabilities2_xml_parser::extract_connections(plan, connection_map); - feedback->progress = "Connection extraction successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Connection extraction successful", false); // estasblish the bond with the server establish_bond(goal_handle); @@ -526,37 +412,25 @@ class CapabilitiesFabric : public rclcpp::Node // verify whether document got 'plan' tags if (!capabilities2_xml_parser::check_plan_tag(document)) { - result->success = false; - result->message = "Execution plan is not compatible. Please recheck and update"; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + print_and_result(goal_handle, "Execution plan is not compatible. Please recheck and update", false); return false; } - feedback->progress = "'Plan' tag checking successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "'Plan' tag checking successful", false); // extract the components within the 'plan' tags tinyxml2::XMLElement* plan = capabilities2_xml_parser::get_plan(document); - feedback->progress = "Plan extraction complete"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Plan extraction complete", false); // verify whether the plan is valid if (!capabilities2_xml_parser::check_tags(this->get_logger(), plan, interface_list, providers_list, control_tag_list, rejected_list)) { - result->success = false; - result->message = "Execution plan is faulty. Please recheck and update"; - goal_handle->abort(result); - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + print_and_result(goal_handle, "Execution plan is faulty. Please recheck and update", false); return false; } - feedback->progress = "Checking tags successful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Checking tags successful", false); return true; } @@ -566,47 +440,35 @@ class CapabilitiesFabric : public rclcpp::Node */ void establish_bond(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - feedback->progress = "Requesting bond id"; - goal_handle->publish_feedback(feedback); - - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "Requesting bond id", false); // create bond establishing server request auto request_bond = std::make_shared(); // send the request - auto result_future = - establish_bond_client_->async_send_request(request_bond, [this, goal_handle, feedback](EstablishBondClient::SharedFuture future) { - if (!future.valid()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to receive the bond id"); - - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to retrieve the bond id"; - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - - goal_handle->abort(result); - return; - } + auto result_future = establish_bond_client_->async_send_request(request_bond, [this, goal_handle](EstablishBondClient::SharedFuture future) { + if (!future.valid()) + { + print_and_result(goal_handle, "Failed to retrieve the bond id. Server execution cancelled", false); + return; + } - auto response = future.get(); - bond_id_ = response->bond_id; + auto response = future.get(); + bond_id_ = response->bond_id; - feedback->progress = "Received the bond id : " + bond_id_; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Received the bond id : " + bond_id_; + print_and_feedback(goal_handle, status, false); - bond_client_ = std::make_unique(shared_from_this(), bond_id_); - bond_client_->start(); + bond_client_ = std::make_unique(shared_from_this(), bond_id_); + bond_client_->start(); - expected_capabilities_ = connection_map.size(); + expected_capabilities_ = connection_map.size(); - RCLCPP_INFO(this->get_logger(), "Requsting start of %d capabilities", expected_capabilities_); + status = "Requsting start of " + std::to_string(expected_capabilities_) + "capabilities"; + print_and_feedback(goal_handle, status, false); - use_capability(connection_map, goal_handle); - }); + use_capability(connection_map, goal_handle); + }); } /** @@ -617,8 +479,6 @@ class CapabilitiesFabric : public rclcpp::Node */ void use_capability(std::map& capabilities, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - std::string capability = capabilities[completed_capabilities_].source.runner; std::string provider = capabilities[completed_capabilities_].source.provider; @@ -627,70 +487,53 @@ class CapabilitiesFabric : public rclcpp::Node request_use->preferred_provider = provider; request_use->bond_id = bond_id_; - feedback->progress = - "Starting capability of Node " + std::to_string(completed_capabilities_) + " named " + capabilities[completed_capabilities_].source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Starting capability of Node " + std::to_string(completed_capabilities_) + " : " + capabilities[completed_capabilities_].source.runner; + print_and_feedback(goal_handle, status, true); // send the request - auto result_future = use_capability_client_->async_send_request(request_use, [this, goal_handle, feedback, capability, - provider](UseCapabilityClient::SharedFuture future) { - if (!future.valid()) - { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to Use capability " + capability + " from " + provider; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); - - // release all capabilities that were used since not all started successfully - for (const auto& [key, value] : connection_map) - { - feedback->progress = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + auto result_future = + use_capability_client_->async_send_request(request_use, [this, goal_handle, capability, provider](UseCapabilityClient::SharedFuture future) { + if (!future.valid()) + { + status = "Failed to Use capability " + capability + " from " + provider + ". Server Execution Cancelled"; + print_and_result(goal_handle, status, false); - free_capability(value.source.runner, goal_handle); - } + // release all capabilities that were used since not all started successfully + for (const auto& [key, value] : connection_map) + { + status = "Freeing capability of Node " + std::to_string(key) + " named " + value.source.runner; + print_and_feedback(goal_handle, status, false); - return; - } + free_capability(value.source.runner, goal_handle); + } - completed_capabilities_++; + return; + } - auto response = future.get(); + completed_capabilities_++; - feedback->progress = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : Capability start succeessful"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + auto response = future.get(); - // Check if all expected calls are completed before calling verify_plan - if (completed_capabilities_ == expected_capabilities_) - { - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested capabilities have been used"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_capabilities_) + "/" + std::to_string(expected_capabilities_) + " : start succeessful"; + print_and_feedback(goal_handle, status, true); - feedback->progress = "Configuring the capabilities with events"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + // Check if all expected calls are completed before calling verify_plan + if (completed_capabilities_ == expected_capabilities_) + { + print_and_feedback(goal_handle, "All requested capabilities have been started. Configuring the capabilities with events", true); - expected_configurations_ = connection_map.size(); + expected_configurations_ = connection_map.size(); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), "Requsting capability configuration for %d capabilities", expected_configurations_); + status = "Requsting capability configuration for " + std::to_string(expected_configurations_) + "capabilities"; + print_and_feedback(goal_handle, status, true); - configure_capabilities(connection_map, goal_handle); - } - else - { - use_capability(connection_map, goal_handle); - } - }); + configure_capabilities(connection_map, goal_handle); + } + else + { + use_capability(connection_map, goal_handle); + } + }); } /** @@ -700,30 +543,24 @@ class CapabilitiesFabric : public rclcpp::Node */ void free_capability(const std::string& capability, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - auto request_free = std::make_shared(); request_free->capability = capability; request_free->bond_id = bond_id_; // send the request - auto result_future = free_capability_client_->async_send_request( - request_free, [this, goal_handle, capability, feedback](FreeCapabilityClient::SharedFuture future) { + auto result_future = + free_capability_client_->async_send_request(request_free, [this, goal_handle, capability](FreeCapabilityClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to free capability " + capability; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + status = "Failed to free capability " + capability; + print_and_result(goal_handle, status, false); return; } auto response = future.get(); - feedback->progress = "Successfully freed capability " + capability; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + + status = "Successfully freed capability " + capability; + print_and_feedback(goal_handle, status, true); bond_client_->stop(); }); @@ -734,14 +571,11 @@ class CapabilitiesFabric : public rclcpp::Node */ void configure_capabilities(std::map& capabilities, const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); auto request_configure = std::make_shared(); - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Configuring capability of Node " + std::to_string(completed_configurations_) + " named " + - capabilities[completed_configurations_].source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = "Configuring capability of Node " + std::to_string(completed_configurations_) + " named " + + capabilities[completed_configurations_].source.runner; + print_and_feedback(goal_handle, status, true); if (capabilities2_xml_parser::convert_to_string(capabilities[completed_configurations_].source.parameters, request_configure->source.parameters)) { @@ -820,17 +654,12 @@ class CapabilitiesFabric : public rclcpp::Node std::string source_capability = capabilities[completed_configurations_].source.runner; // send the request - auto result_future = configure_capability_client_->async_send_request( - request_configure, [this, goal_handle, source_capability, feedback](ConfigureCapabilityClient::SharedFuture future) { + auto result_future = conf_capability_client_->async_send_request( + request_configure, [this, goal_handle, source_capability](ConfigureCapabilityClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to configure capability :" + source_capability; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); - RCLCPP_ERROR(this->get_logger(), "Server Execution Cancelled"); + status = "Failed to configure capability :" + source_capability + ". Server execution cancelled"; + print_and_result(goal_handle, status, false); return; } @@ -838,24 +667,14 @@ class CapabilitiesFabric : public rclcpp::Node auto response = future.get(); - feedback->progress = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + - " : Successfully configured capability : " + source_capability; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), ""); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + status = std::to_string(completed_configurations_) + "/" + std::to_string(expected_configurations_) + + " : Successfully configured capability : " + source_capability; + print_and_feedback(goal_handle, status, true); // Check if all expected calls are completed before calling verify_plan if (completed_configurations_ == expected_configurations_) { - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "All requested capabilities have been configured"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - - RCLCPP_INFO(this->get_logger(), ""); - feedback->progress = "Triggering the first capability"; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); + print_and_feedback(goal_handle, "All requested capabilities have been configured. Triggering the first capability", true); trigger_first_node(goal_handle); } @@ -871,8 +690,6 @@ class CapabilitiesFabric : public rclcpp::Node */ void trigger_first_node(const std::shared_ptr goal_handle) { - auto feedback = std::make_shared(); - auto request_trigger = std::make_shared(); std::string parameter_string; @@ -882,31 +699,65 @@ class CapabilitiesFabric : public rclcpp::Node // send the request auto result_future = - trigger_capability_client_->async_send_request(request_trigger, [this, goal_handle, feedback](TriggerCapabilityClient::SharedFuture future) { + trig_capability_client_->async_send_request(request_trigger, [this, goal_handle](TriggerCapabilityClient::SharedFuture future) { if (!future.valid()) { - auto result = std::make_shared(); - result->success = false; - result->message = "Failed to trigger capability " + connection_map[0].source.runner; - goal_handle->abort(result); - - RCLCPP_ERROR(this->get_logger(), result->message.c_str()); + status = "Failed to trigger capability " + connection_map[0].source.runner; + print_and_result(goal_handle, status, false); return; } auto response = future.get(); - feedback->progress = "Successfully triggered capability " + connection_map[0].source.runner; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), feedback->progress.c_str()); - - auto result = std::make_shared(); - result->success = true; - result->message = "Successfully launched capabilities2 fabric"; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), result->message.c_str()); + + status = "Successfully triggered capability " + connection_map[0].source.runner; + print_and_feedback(goal_handle, status, false); + + print_and_result(goal_handle, "Successfully launched capabilities2 fabric", true); }); } + void check_service(bool wait_for_logic, const std::string& service_name) + { + while (wait_for_logic) + { + std::string failed = service_name + " not available"; + RCLCPP_ERROR(this->get_logger(), failed.c_str()); + rclcpp::shutdown(); + return; + } + + std::string success = service_name + " connected"; + RCLCPP_INFO(this->get_logger(), success.c_str()); + } + + void print_and_feedback(const std::shared_ptr goal_handle, const std::string& text, bool newline) + { + feedback_msg->progress = text; + goal_handle->publish_feedback(feedback_msg); + + if (newline) + RCLCPP_INFO(this->get_logger(), ""); + + RCLCPP_INFO(this->get_logger(), feedback_msg->progress.c_str()); + } + + void print_and_result(const std::shared_ptr goal_handle, const std::string& text, bool success) + { + result_msg->success = success; + result_msg->message = text; + + if (success) + { + goal_handle->succeed(result_msg); + RCLCPP_INFO(this->get_logger(), result_msg->message.c_str()); + } + else + { + goal_handle->abort(result_msg); + RCLCPP_ERROR(this->get_logger(), result_msg->message.c_str()); + } + } + private: /** File Path link */ std::string plan_file_path; @@ -926,6 +777,9 @@ class CapabilitiesFabric : public rclcpp::Node int expected_configurations_; int completed_configurations_; + /** status message string */ + std::string status; + /** Bond id */ std::string bond_id_; @@ -953,6 +807,12 @@ class CapabilitiesFabric : public rclcpp::Node /** Invalid events list */ std::vector rejected_list; + /** Feedback message for plan action server*/ + std::shared_ptr feedback_msg; + + /** Result message for plan action server*/ + std::shared_ptr result_msg; + /** action server that exposes executor*/ std::shared_ptr> planner_server_; @@ -978,8 +838,8 @@ class CapabilitiesFabric : public rclcpp::Node FreeCapabilityClient::SharedPtr free_capability_client_; /** configure an selected capability */ - ConfigureCapabilityClient::SharedPtr configure_capability_client_; + ConfigureCapabilityClient::SharedPtr conf_capability_client_; /** trigger an selected capability */ - TriggerCapabilityClient::SharedPtr trigger_capability_client_; + TriggerCapabilityClient::SharedPtr trig_capability_client_; }; \ No newline at end of file diff --git a/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp index 38ff3d3..6910d60 100644 --- a/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp +++ b/capabilities2_fabric/include/capabilities2_fabric/capabilities_xml_parser.hpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include namespace capabilities2_xml_parser { diff --git a/capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp similarity index 100% rename from capabilities2_fabric/include/capabilities2_fabric/structs/bond_client.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/bond_client.hpp diff --git a/capabilities2_fabric/include/capabilities2_fabric/structs/connection.hpp b/capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp similarity index 100% rename from capabilities2_fabric/include/capabilities2_fabric/structs/connection.hpp rename to capabilities2_fabric/include/capabilities2_fabric/utils/connection.hpp From 6297a73498dce8952ff4a635137ae5ab8d221130 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 15 Jan 2025 01:29:51 +1100 Subject: [PATCH 09/11] minor fixes --- .../launch/server.launch.py | 32 +++++++++++ capabilities2_launch_py/package.xml | 2 + capabilities2_launch_py/setup.py | 3 ++ .../capabilities2_runner/action_runner.hpp | 54 +++++++++++-------- .../capabilities2_runner/runner_base.hpp | 35 ++++++++++-- .../waypoint_runner.hpp | 7 +-- capabilities2_server/launch/server.launch.py | 14 ++--- .../launch/server_composed.launch.py | 10 +--- 8 files changed, 110 insertions(+), 47 deletions(-) create mode 100644 capabilities2_launch_py/launch/server.launch.py diff --git a/capabilities2_launch_py/launch/server.launch.py b/capabilities2_launch_py/launch/server.launch.py new file mode 100644 index 0000000..2576493 --- /dev/null +++ b/capabilities2_launch_py/launch/server.launch.py @@ -0,0 +1,32 @@ +''' +capabilities2_server launch file +''' + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for capabilities2 server + + Returns: + LaunchDescription: The launch description for capabilities2 server + """ + # load config file + server_config = os.path.join(get_package_share_directory('capabilities2_server'), 'config', 'capabilities.yaml') + + # create launch proxy node + launch_proxy = Node( + package='capabilities2_launch_py', + executable='capabilities2_launch_py', + name='capabilities2_launch_py', + output='screen', + arguments=['--ros-args', '--log-level', 'info'] + ) + + # return + return LaunchDescription([ + launch_proxy + ]) diff --git a/capabilities2_launch_py/package.xml b/capabilities2_launch_py/package.xml index 5660d5d..6c1c013 100644 --- a/capabilities2_launch_py/package.xml +++ b/capabilities2_launch_py/package.xml @@ -10,6 +10,8 @@ rclcpp capabilities2_msgs + ros2launch + ament_copyright ament_flake8 ament_pep257 diff --git a/capabilities2_launch_py/setup.py b/capabilities2_launch_py/setup.py index 717f564..605a0cf 100644 --- a/capabilities2_launch_py/setup.py +++ b/capabilities2_launch_py/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'capabilities2_launch_py' @@ -9,6 +11,7 @@ data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), ], install_requires=['setuptools'], zip_safe=True, diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index f308821..bb0b0ee 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -113,26 +113,33 @@ class ActionRunner : public RunnerBase */ virtual void execution() override { - execute_id += 1; + is_running = true; - // if parameters are not provided then cannot proceed - if (!parameters_) - throw runner_exception("cannot trigger action without parameters"); + while (new_parameter) + { + new_parameter = false; + + execute_id += 1; - // generate a goal from parameters if provided - goal_msg_ = generate_goal(parameters_); - RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); + // if parameters are not provided then cannot proceed + if (!parameters_) + throw runner_exception("cannot trigger action without parameters"); - // trigger the action client with goal - send_goal_options_.goal_response_callback = - std::bind(&ActionRunner::goalResponseCallback, this, std::placeholders::_1); - send_goal_options_.feedback_callback = - std::bind(&ActionRunner::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); - send_goal_options_.result_callback = std::bind(&ActionRunner::resultCallback, this, std::placeholders::_1); + // generate a goal from parameters if provided + goal_msg_ = generate_goal(parameters_); + RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); - goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); + // trigger the action client with goal + send_goal_options_.goal_response_callback = + std::bind(&ActionRunner::goalResponseCallback, this, std::placeholders::_1); + send_goal_options_.feedback_callback = + std::bind(&ActionRunner::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); + send_goal_options_.result_callback = std::bind(&ActionRunner::resultCallback, this, std::placeholders::_1); - RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); + + RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); + } } protected: @@ -146,7 +153,7 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_started != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", run_config_.interface.c_str()); - // triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); } } else @@ -170,7 +177,7 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_success != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", run_config_.interface.c_str()); - // triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); } } else @@ -183,11 +190,12 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_failure != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", run_config_.interface.c_str()); - // triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); } } result_ = wrapped_result.result; + is_running = false; }; virtual void feedbackCallback(typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, @@ -195,7 +203,10 @@ class ActionRunner : public RunnerBase { std::string feedback = generate_feedback(feedback_msg); - RCLCPP_INFO(node_->get_logger(), "[%s] received feedback: %s", run_config_.interface.c_str(), feedback.c_str()); + if (feedback != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] received feedback: %s", run_config_.interface.c_str(), feedback.c_str()); + } }; virtual void cancellationCallback(action_msgs::srv::CancelGoal_Response::SharedPtr response) @@ -210,7 +221,7 @@ class ActionRunner : public RunnerBase if (events[execute_id].on_stopped != "") { RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", run_config_.interface.c_str()); - // triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); } } @@ -232,7 +243,8 @@ class ActionRunner : public RunnerBase * * This function is used to convert feedback messages into generic strings * - * A 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 parameters * @return ActionT::Feedback the received feedback diff --git a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp index e48cea1..164f161 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -124,7 +125,22 @@ class RunnerBase { parameters_ = convert_to_xml(parameters); - executionThread = std::thread(&RunnerBase::execution, this); + RCLCPP_INFO(node_->get_logger(), "[%s] received new parameters", run_config_.interface.c_str()); + + new_parameter = true; + + if (!is_running) + { + RCLCPP_INFO(node_->get_logger(), "[%s] execution thread not active. Activating..", + run_config_.interface.c_str()); + + executionThread = std::thread(&RunnerBase::execution, this); + } + else + { + RCLCPP_INFO(node_->get_logger(), "[%s] execution thread running. Avoiding reactivation", + run_config_.interface.c_str()); + } } /** @@ -140,6 +156,8 @@ class RunnerBase run_config_ = run_config; insert_id = 0; execute_id = -1; + is_running = false; + new_parameter = false; } /** @@ -472,15 +490,14 @@ class RunnerBase std::map events; /** - * @brief Last tracker id to be inserted + * @brief Last event tracker id to be inserted */ int insert_id; /** - * @brief Last tracker id to be executed + * @brief Last parameter tracker id to be executed */ int execute_id; - /** * @brief pointer to XMLElement which contain parameters */ @@ -491,6 +508,16 @@ class RunnerBase */ std::thread executionThread; + /** + * @brief boolean flag for status of the thread. + */ + bool is_running; + + /** + * @brief boolean flag for new parameter availability. + */ + bool new_parameter; + /** * @brief external function that triggers capability runners */ diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp index ebbf9b3..56ebc49 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -78,9 +78,10 @@ class WayPointRunner : public ActionRunner virtual std::string generate_feedback(const typename nav2_msgs::action::NavigateToPose::Feedback::ConstSharedPtr msg) override { - std::string feedback = "x: " + std::to_string(msg->current_pose.pose.position.x) + - " y: " + std::to_string(msg->current_pose.pose.position.y); - return feedback; + // std::string feedback = "x: " + std::to_string(msg->current_pose.pose.position.x) + + // " y: " + std::to_string(msg->current_pose.pose.position.y); + // return feedback; + return ""; } protected: diff --git a/capabilities2_server/launch/server.launch.py b/capabilities2_server/launch/server.launch.py index 6c63d94..6879e11 100644 --- a/capabilities2_server/launch/server.launch.py +++ b/capabilities2_server/launch/server.launch.py @@ -22,18 +22,12 @@ def generate_launch_description(): package='capabilities2_server', executable='capabilities2_server_node', name='capabilities', - parameters=[server_config] - ) - - # create launch proxy node - launch_proxy = Node( - package='capabilities2_launch_py', - executable='capabilities2_launch_py', - name='capabilities2_launch_py' + parameters=[server_config], + output='screen', + arguments=['--ros-args', '--log-level', 'info'] ) # return return LaunchDescription([ - capabilities2, - launch_proxy + capabilities2 ]) diff --git a/capabilities2_server/launch/server_composed.launch.py b/capabilities2_server/launch/server_composed.launch.py index a7b4fa8..498f824 100644 --- a/capabilities2_server/launch/server_composed.launch.py +++ b/capabilities2_server/launch/server_composed.launch.py @@ -37,15 +37,7 @@ def generate_launch_description(): ] ) - # create launch proxy node - launch_proxy = Node( - package='capabilities2_launch_py', - executable='capabilities2_launch_py', - name='capabilities2_launch_py' - ) - # return return LaunchDescription([ - capabilities, - launch_proxy + capabilities ]) From 3bd2e9d6bb49d20de0fb84394ffdf46a27f3087b Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Wed, 15 Jan 2025 14:51:19 +1100 Subject: [PATCH 10/11] completed threading implementation and tested sequential triggering --- capabilities2_fabric/plans/navigation_2.xml | 2 +- .../capabilities2_runner/action_runner.hpp | 222 +++++++++--------- .../capabilities2_runner/launch_runner.hpp | 2 +- .../capabilities2_runner/runner_base.hpp | 47 ++-- .../capabilities2_runner/service_runner.hpp | 6 +- .../capabilities2_runner/topic_runner.hpp | 4 +- capabilities2_runner/src/standard_runners.cpp | 2 +- .../listen_runner.hpp | 2 - .../speak_runner.hpp | 4 +- .../bt_factory_runner.hpp | 8 +- .../waypoint_runner.hpp | 6 +- .../prompt_occupancy_runner.hpp | 2 - .../prompt_plan_request_runner.hpp | 2 - .../prompt_plan_response_runner.hpp | 2 - .../prompt_pose_runner.hpp | 2 - .../prompt_text_runner.hpp | 2 - 16 files changed, 149 insertions(+), 166 deletions(-) diff --git a/capabilities2_fabric/plans/navigation_2.xml b/capabilities2_fabric/plans/navigation_2.xml index 4b0560a..5c283d1 100644 --- a/capabilities2_fabric/plans/navigation_2.xml +++ b/capabilities2_fabric/plans/navigation_2.xml @@ -2,6 +2,6 @@ - + \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp index bb0b0ee..bcef201 100644 --- a/capabilities2_runner/include/capabilities2_runner/action_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/action_runner.hpp @@ -84,7 +84,21 @@ class ActionRunner : public RunnerBase try { auto cancel_future = action_client_->async_cancel_goal( - goal_handle_, std::bind(&ActionRunner::cancellationCallback, this, std::placeholders::_1)); + 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"); + RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); + } + + // Trigger on_stopped event if defined + if (events[execute_id].on_stopped != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", + run_config_.interface.c_str()); + triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); + } + }); // 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 @@ -111,120 +125,106 @@ class ActionRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void execution() override + virtual void execution(int id) override { - is_running = true; - - while (new_parameter) - { - new_parameter = false; - - execute_id += 1; - - // if parameters are not provided then cannot proceed - if (!parameters_) - throw runner_exception("cannot trigger action without parameters"); - - // generate a goal from parameters if provided - goal_msg_ = generate_goal(parameters_); - RCLCPP_INFO(node_->get_logger(), "[%s] goal generated.", run_config_.interface.c_str()); - - // trigger the action client with goal - send_goal_options_.goal_response_callback = - std::bind(&ActionRunner::goalResponseCallback, this, std::placeholders::_1); - send_goal_options_.feedback_callback = - std::bind(&ActionRunner::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); - send_goal_options_.result_callback = std::bind(&ActionRunner::resultCallback, this, std::placeholders::_1); - - goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); - - RCLCPP_INFO(node_->get_logger(), "[%s] goal sent. Waiting for acceptance", run_config_.interface.c_str()); - } + execute_id += 1; + + // if parameters are not provided then cannot proceed + if (!parameters_[id]) + throw runner_exception("cannot trigger action without parameters"); + + // generate a goal from parameters if provided + goal_msg_ = generate_goal(parameters_[id]); + + RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal generated.", run_config_.interface.c_str(), id); + + std::unique_lock lock(send_goal_mutex); + action_complete = 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) + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal accepted. Waiting for result", run_config_.interface.c_str(), + id); + + // trigger the events related to on_started state + if (events[execute_id].on_started != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_started event available. Triggering", + run_config_.interface.c_str(), id); + + triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); + } + } + else + { + RCLCPP_ERROR(node_->get_logger(), "[%s/%d] goal rejected", run_config_.interface.c_str(), id); + } + + // 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); + + if (feedback != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] received feedback: %s", run_config_.interface.c_str(), id, + feedback.c_str()); + } + }; + + send_goal_options_.result_callback = + [this, id](const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] received result", run_config_.interface.c_str(), id); + + if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] action succeeded.", run_config_.interface.c_str(), id); + + // trigger the events related to on_success state + if (events[execute_id].on_success != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_success event available. Triggering", + run_config_.interface.c_str(), id); + triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); + } + } + else + { + RCLCPP_ERROR(node_->get_logger(), "[%s/%d] action failed.", run_config_.interface.c_str(), id); + RCLCPP_ERROR(node_->get_logger(), "[%s/%d] error code: %d", run_config_.interface.c_str(), id, + static_cast(wrapped_result.code)); + + // trigger the events related to on_failure state + if (events[execute_id].on_failure != "") + { + RCLCPP_INFO(node_->get_logger(), "[%s/%d] on_failure event available. Triggering", + run_config_.interface.c_str(), id); + triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); + } + } + + result_ = wrapped_result.result; + action_complete = true; + send_goal_cv.notify_all(); + }; + + goal_handle_future_ = action_client_->async_send_goal(goal_msg_, send_goal_options_); + + RCLCPP_INFO(node_->get_logger(), "[%s/%d] goal sent. Waiting for acceptance", run_config_.interface.c_str(), id); + + // Conditional wait + send_goal_cv.wait(lock, [this] { return action_complete; }); + RCLCPP_INFO(node_->get_logger(), "[%s/%d] action complete. Thread closing.", run_config_.interface.c_str(), id); } protected: - virtual void goalResponseCallback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) - { - if (goal_handle) - { - RCLCPP_INFO(node_->get_logger(), "[%s] goal accepted. Waiting for result", run_config_.interface.c_str()); - - // trigger the events related to on_started state - if (events[execute_id].on_started != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_started event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_started, update_on_started(events[execute_id].on_started_param)); - } - } - else - { - RCLCPP_ERROR(node_->get_logger(), "[%s] goal rejected", run_config_.interface.c_str()); - } - - // store goal handle to be used with stop funtion - goal_handle_ = goal_handle; - }; - - virtual void resultCallback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& wrapped_result) - { - RCLCPP_INFO(node_->get_logger(), "[%s] received result", run_config_.interface.c_str()); - - if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) - { - RCLCPP_INFO(node_->get_logger(), "[%s] action succeeded.", run_config_.interface.c_str()); - - // trigger the events related to on_success state - if (events[execute_id].on_success != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_success event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_success, update_on_success(events[execute_id].on_success_param)); - } - } - else - { - RCLCPP_ERROR(node_->get_logger(), "[%s] action failed.", run_config_.interface.c_str()); - RCLCPP_ERROR(node_->get_logger(), "[%s] error code: %d", run_config_.interface.c_str(), - static_cast(wrapped_result.code)); - - // trigger the events related to on_failure state - if (events[execute_id].on_failure != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_failure event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_failure, update_on_failure(events[execute_id].on_failure_param)); - } - } - - result_ = wrapped_result.result; - is_running = false; - }; - - virtual void feedbackCallback(typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, - const typename ActionT::Feedback::ConstSharedPtr feedback_msg) - { - std::string feedback = generate_feedback(feedback_msg); - - if (feedback != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] received feedback: %s", run_config_.interface.c_str(), feedback.c_str()); - } - }; - - virtual void cancellationCallback(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"); - RCLCPP_WARN(node_->get_logger(), "Runner cancellation failed."); - } - - // Trigger on_stopped event if defined - if (events[execute_id].on_stopped != "") - { - RCLCPP_INFO(node_->get_logger(), "[%s] on_stopped event available. Triggering", run_config_.interface.c_str()); - triggerFunction_(events[execute_id].on_stopped, update_on_stopped(events[execute_id].on_stopped_param)); - } - } - /** * @brief Generate a goal from parameters * diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index dcc8781..1b74036 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -129,7 +129,7 @@ class LaunchRunner : public RunnerBase protected: // throw on triggerExecution function - void execution() override + void execution(int id) override { throw runner_exception("no triggerExecution() 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 164f161..2733b44 100644 --- a/capabilities2_runner/include/capabilities2_runner/runner_base.hpp +++ b/capabilities2_runner/include/capabilities2_runner/runner_base.hpp @@ -123,24 +123,15 @@ class RunnerBase */ virtual void trigger(const std::string& parameters) { - parameters_ = convert_to_xml(parameters); + RCLCPP_INFO(node_->get_logger(), "[%s/%d] received new parameters", run_config_.interface.c_str(), thread_id); - RCLCPP_INFO(node_->get_logger(), "[%s] received new parameters", run_config_.interface.c_str()); + parameters_[thread_id] = convert_to_xml(parameters); - new_parameter = true; + executionThreadPool[thread_id] = std::thread(&RunnerBase::execution, this, thread_id); - if (!is_running) - { - RCLCPP_INFO(node_->get_logger(), "[%s] execution thread not active. Activating..", - run_config_.interface.c_str()); - - executionThread = std::thread(&RunnerBase::execution, this); - } - else - { - RCLCPP_INFO(node_->get_logger(), "[%s] execution thread running. Avoiding reactivation", - run_config_.interface.c_str()); - } + RCLCPP_INFO(node_->get_logger(), "[%s/%d] started execution", run_config_.interface.c_str(), thread_id); + + thread_id += 1; } /** @@ -156,8 +147,7 @@ class RunnerBase run_config_ = run_config; insert_id = 0; execute_id = -1; - is_running = false; - new_parameter = false; + thread_id = 0; } /** @@ -230,7 +220,7 @@ class RunnerBase * @param parameters pointer to tinyxml2::XMLElement that contains parameters * */ - virtual void execution() = 0; + virtual void execution(int id) = 0; /** * @brief Update on_started event parameters with new data if avaible. @@ -498,25 +488,32 @@ class RunnerBase * @brief Last parameter tracker id to be executed */ int execute_id; + + /** + * @brief Last parameter tracker id to be executed + */ + int thread_id; + /** * @brief pointer to XMLElement which contain parameters */ - tinyxml2::XMLElement* parameters_; + std::map parameters_; /** - * @brief thread that executes the triggerExecution functionality + * @brief dictionary of threads that executes the triggerExecution functionality */ - std::thread executionThread; + std::map executionThreadPool; /** - * @brief boolean flag for status of the thread. + * @brief mutex and conditional variable for threadpool synchronisation. */ - bool is_running; + std::mutex send_goal_mutex; + std::condition_variable send_goal_cv; /** - * @brief boolean flag for new parameter availability. + * @brief boolean flag for thread completion. */ - bool new_parameter; + bool action_complete; /** * @brief external function that triggers capability runners diff --git a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp index 498d18a..6fbf2d2 100644 --- a/capabilities2_runner/include/capabilities2_runner/service_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/service_runner.hpp @@ -58,16 +58,16 @@ class ServiceRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void execution() override + virtual void execution(int id) override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters_) + 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_)); + auto request_msg = std::make_shared(generate_request(parameters_[id])); auto result_future = service_client_->async_send_request( request_msg, [this](typename rclcpp::Client::SharedFuture future) { diff --git a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp index 26f5840..2450851 100644 --- a/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/topic_runner.hpp @@ -50,12 +50,12 @@ class TopicRunner : public RunnerBase * * @param parameters pointer to tinyxml2::XMLElement that contains parameters */ - virtual void execution() override + virtual void execution(int id) override { execute_id += 1; // if parameters are not provided then cannot proceed - if (!parameters_) + if (!parameters_[id]) throw runner_exception("cannot grab data without parameters"); // trigger the events related to on_started state diff --git a/capabilities2_runner/src/standard_runners.cpp b/capabilities2_runner/src/standard_runners.cpp index 459aa42..58ca354 100644 --- a/capabilities2_runner/src/standard_runners.cpp +++ b/capabilities2_runner/src/standard_runners.cpp @@ -33,7 +33,7 @@ class DummyRunner : public RunnerBase protected: // throw on triggerExecution function - void execution() override + 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 index 35ca8d2..84fd7a7 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/listen_runner.hpp @@ -46,8 +46,6 @@ class ListenerRunner : public ActionRunner */ virtual hri_audio_msgs::action::SpeechToText::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - hri_audio_msgs::action::SpeechToText::Goal goal_msg; goal_msg.header.stamp = node_->get_clock()->now(); diff --git a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp index 7f55443..014e953 100644 --- a/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp +++ b/capabilities2_runner_audio/include/capabilities2_runner_audio/speak_runner.hpp @@ -45,10 +45,8 @@ class SpeakerRunner : public ActionRunner */ virtual hri_audio_msgs::action::TextToSpeech::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - const char **text; - parameters_->QueryStringAttribute("text", text); + parameters->QueryStringAttribute("text", text); std::string tts_text(*text); hri_audio_msgs::action::TextToSpeech::Goal goal_msg; 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 index ff3b463..846f1a8 100644 --- a/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp +++ b/capabilities2_runner_bt/include/capabilities2_runner_bt/bt_factory_runner.hpp @@ -50,14 +50,16 @@ class BTRunnerBase : public RunnerBase, public BT::BehaviorTreeFactory * @param parameters * @return std::optional)>> */ - virtual void trigger(tinyxml2::XMLElement* parameters = nullptr) override + virtual void trigger(const std::string& parameters) override { + tinyxml2::XMLElement* parameters_xml = convert_to_xml(parameters); + // if parameters are not provided then cannot proceed - if (!parameters) + if (!parameters_xml) throw runner_exception("cannot trigger action without parameters"); // create the tree (ptr) - tree_ = std::make_shared(this->createTreeFromText(parameters->GetText())); + tree_ = std::make_shared(this->createTreeFromText(parameters_xml->GetText())); // return the tick function // // the caller can call this function to tick the tree diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp index 56ebc49..0a5cd6d 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -48,10 +48,8 @@ class WayPointRunner : public ActionRunner */ virtual nav2_msgs::action::NavigateToPose::Goal generate_goal(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - - parameters_->QueryDoubleAttribute("x", &x); - parameters_->QueryDoubleAttribute("y", &y); + parameters->QueryDoubleAttribute("x", &x); + parameters->QueryDoubleAttribute("y", &y); RCLCPP_INFO(node_->get_logger(), "[%s] goal consist of x: %f and y: %f", run_config_.interface.c_str(), x, y); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp index 709f90d..cea6c6d 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_occupancy_runner.hpp @@ -46,8 +46,6 @@ class PromptOccupancyRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - tinyxml2::XMLElement* occupancyElement = parameters->FirstChildElement("OccupancyGrid"); tinyxml2::XMLPrinter printer; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp index 7e17207..b75c5e5 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_request_runner.hpp @@ -47,8 +47,6 @@ class PromptPlanRequestRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - bool replan; parameters->QueryBoolAttribute("replan", &replan); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp index 67a5c31..d6c3a36 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_plan_response_runner.hpp @@ -45,8 +45,6 @@ class PromptPlanResponseRunner : public ActionRunnerFirstChildElement("ReceievdPlan"); auto goal_msg = capabilities2_msgs::action::Plan::Goal(); diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp index 1205541..3dda94a 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_pose_runner.hpp @@ -46,8 +46,6 @@ class PromptPoseRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - tinyxml2::XMLElement* poseElement = parameters->FirstChildElement("Pose"); tinyxml2::XMLPrinter printer; diff --git a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp index 0058a85..a418f76 100644 --- a/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp +++ b/capabilities2_runner_prompt/include/capabilities2_runner_prompt/prompt_text_runner.hpp @@ -46,8 +46,6 @@ class PromptTextRunner : public ServiceRunner */ virtual typename prompt_msgs::srv::Prompt::Request generate_request(tinyxml2::XMLElement* parameters) override { - parameters_ = parameters; - tinyxml2::XMLElement* textElement = parameters->FirstChildElement("Text"); tinyxml2::XMLPrinter printer; From 541895b747abc3d5c9e8c22bd6d7df97606e3247 Mon Sep 17 00:00:00 2001 From: Kalana Ratnayake Date: Thu, 16 Jan 2025 14:18:26 +1100 Subject: [PATCH 11/11] completed testing of thread based fabric triggering using nav2 stack --- .../capabilities2_launch}/__init__.py | 0 .../capabilities2_launch.py | 146 ++++++++++++++++++ .../launch/server.launch.py | 19 ++- .../launch_server}/__init__.py | 0 .../launch_server}/launch_process.py | 47 +++++- capabilities2_launch/launch_server/server.py | 65 ++++++++ .../package.xml | 4 +- .../resource/capabilities2_launch | 0 capabilities2_launch/setup.cfg | 4 + .../setup.py | 5 +- .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 .../capabilities2_launch_py/launch_server.py | 79 ---------- capabilities2_launch_py/setup.cfg | 4 - capabilities2_msgs/srv/Launch.srv | 3 +- .../capabilities2_runner/launch_runner.hpp | 20 +-- capabilities2_runner_nav2/CMakeLists.txt | 2 + .../waypoint_runner.hpp | 8 + 19 files changed, 300 insertions(+), 106 deletions(-) rename {capabilities2_launch_py/capabilities2_launch_py => capabilities2_launch/capabilities2_launch}/__init__.py (100%) create mode 100644 capabilities2_launch/capabilities2_launch/capabilities2_launch.py rename {capabilities2_launch_py => capabilities2_launch}/launch/server.launch.py (51%) rename {capabilities2_launch_py/launch_process => capabilities2_launch/launch_server}/__init__.py (100%) rename {capabilities2_launch_py/launch_process => capabilities2_launch/launch_server}/launch_process.py (69%) create mode 100644 capabilities2_launch/launch_server/server.py rename {capabilities2_launch_py => capabilities2_launch}/package.xml (94%) rename capabilities2_launch_py/resource/capabilities2_launch_py => capabilities2_launch/resource/capabilities2_launch (100%) create mode 100644 capabilities2_launch/setup.cfg rename {capabilities2_launch_py => capabilities2_launch}/setup.py (77%) rename {capabilities2_launch_py => capabilities2_launch}/test/test_copyright.py (100%) rename {capabilities2_launch_py => capabilities2_launch}/test/test_flake8.py (100%) rename {capabilities2_launch_py => capabilities2_launch}/test/test_pep257.py (100%) delete mode 100644 capabilities2_launch_py/capabilities2_launch_py/launch_server.py delete mode 100644 capabilities2_launch_py/setup.cfg diff --git a/capabilities2_launch_py/capabilities2_launch_py/__init__.py b/capabilities2_launch/capabilities2_launch/__init__.py similarity index 100% rename from capabilities2_launch_py/capabilities2_launch_py/__init__.py rename to capabilities2_launch/capabilities2_launch/__init__.py diff --git a/capabilities2_launch/capabilities2_launch/capabilities2_launch.py b/capabilities2_launch/capabilities2_launch/capabilities2_launch.py new file mode 100644 index 0000000..854c232 --- /dev/null +++ b/capabilities2_launch/capabilities2_launch/capabilities2_launch.py @@ -0,0 +1,146 @@ +import os +import signal +import rclpy +from rclpy.node import Node +from capabilities2_msgs.srv import Launch +from multiprocessing.connection import Client + +class LaunchServer(Node): + def __init__(self): + super().__init__('capabilities2_launch_server') + self.address = ('localhost', 6000) + self.client = Client(self.address, authkey=b'capabilities2') + + # Service for starting a launch file + self.start_server = self.create_service( + Launch, + '/capabilities/launch/start', + self.start_request + ) + + # Service for stopping a launch file + self.stop_server = self.create_service( + Launch, + '/capabilities/launch/stop', + self.stop_request + ) + + # Service for stopping a launch file + self.stop_server = self.create_service( + Launch, + '/capabilities/launch/status', + self.status_request + ) + + self.get_logger().info('Capabilities2 LaunchServer is ready.') + + def start_request(self, request, response): + package_name = request.package_name + launch_file_name = request.launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received start request') + + message_outgoing = {} + message_outgoing["command"] = "start" + message_outgoing["package"] = package_name + message_outgoing["launch_file"] = launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] sending start request to Launch Server') + + self.client.send(message_outgoing) + + self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') + + message_incoming = self.client.recv() + response_content = message_incoming["status"] + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') + + response.status = response_content + + return response + + def stop_request(self, request, response): + package_name = request.package_name + launch_file_name = request.launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received stop request') + + message_outgoing = {} + message_outgoing["command"] = "stop" + message_outgoing["package"] = package_name + message_outgoing["launch_file"] = launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] sending stop request to Launch Server') + + self.client.send(message_outgoing) + + self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') + + message_incoming = self.client.recv() + response_content = message_incoming["status"] + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') + + response.status = response_content + + return response + + + def status_request(self, request, response): + package_name = request.package_name + launch_file_name = request.launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received status request') + + message_outgoing = {} + message_outgoing["command"] = "status" + message_outgoing["package"] = package_name + message_outgoing["launch_file"] = launch_file_name + + self.get_logger().info(f'[{package_name}/{launch_file_name}] sending status request to Launch Server') + + self.client.send(message_outgoing) + + self.get_logger().info(f'[{package_name}/{launch_file_name}] waiting for response from Launch Server') + + message_incoming = self.client.recv() + response_content = message_incoming["status"] + + self.get_logger().info(f'[{package_name}/{launch_file_name}] received response : {response_content}') + + response.status = response_content + + return response + + def shutdown(self): + + self.get_logger().info(f'Shutting down LaunchFileServer...') + + message_outgoing = {} + message_outgoing["command"] = "exit" + message_outgoing["package"] = "" + message_outgoing["launch_file"] = "" + + self.get_logger().info(f'[sending shutdown request to Launch Server') + + self.client.send(message_outgoing) + + +def main(args=None): + rclpy.init(args=args) + + # Create the LaunchServer node + launch_server = LaunchServer() + + # Keep the node running + try: + rclpy.spin(launch_server) + except KeyboardInterrupt: + launch_server.shutdown() + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/capabilities2_launch_py/launch/server.launch.py b/capabilities2_launch/launch/server.launch.py similarity index 51% rename from capabilities2_launch_py/launch/server.launch.py rename to capabilities2_launch/launch/server.launch.py index 2576493..8ce3c51 100644 --- a/capabilities2_launch_py/launch/server.launch.py +++ b/capabilities2_launch/launch/server.launch.py @@ -4,7 +4,9 @@ import os from launch import LaunchDescription +from launch.actions import ExecuteProcess from launch_ros.actions import Node +from launch.substitutions import FindExecutable from ament_index_python.packages import get_package_share_directory @@ -14,19 +16,22 @@ def generate_launch_description(): 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') + + filePath = os.path.join(get_package_share_directory('capabilities2_launch'), 'launch_server', 'server.py') # create launch proxy node - launch_proxy = Node( - package='capabilities2_launch_py', - executable='capabilities2_launch_py', - name='capabilities2_launch_py', + launch_interface = Node( + package='capabilities2_launch', + executable='capabilities2_launch', + name='capabilities2_launch', output='screen', arguments=['--ros-args', '--log-level', 'info'] ) + server_process = ExecuteProcess(cmd=[[FindExecutable(name='python3'), ' ', filePath]], shell=True, output='screen') + # return return LaunchDescription([ - launch_proxy + launch_interface, + server_process ]) diff --git a/capabilities2_launch_py/launch_process/__init__.py b/capabilities2_launch/launch_server/__init__.py similarity index 100% rename from capabilities2_launch_py/launch_process/__init__.py rename to capabilities2_launch/launch_server/__init__.py diff --git a/capabilities2_launch_py/launch_process/launch_process.py b/capabilities2_launch/launch_server/launch_process.py similarity index 69% rename from capabilities2_launch_py/launch_process/launch_process.py rename to capabilities2_launch/launch_server/launch_process.py index 14ba22d..04af439 100644 --- a/capabilities2_launch_py/launch_process/launch_process.py +++ b/capabilities2_launch/launch_server/launch_process.py @@ -49,7 +49,7 @@ def __init__( raise RuntimeError(str(exc)) def run(self): - print(f"Launch file {self.launch_file_name} from package {self.package_name} started.") + print(f"Launch file {self.launch_file_name} from package {self.package_name} started with pid [{self.pid}") launch_service = launch.LaunchService( argv=self.launch_file_arguments, @@ -79,6 +79,51 @@ def stop(self): print(f"Stopping Launch file {self.launch_file_name} from package {self.package_name}") self.kill() +class LaunchManager: + def __init__(self): + self.processes = {} + + def start(self, package_name, launch_file_name): + name = package_name + "/" + launch_file_name + + if name not in self.processes: + try: + self.processes[name] = LaunchProcess(package_name=package_name, launch_file_name=launch_file_name) + self.processes[name].start() + except: + return {"status": "error occured"} + else: + return {"status": "successfuly started"} + else: + return {"status": "already started. ignoring"} + + def status(self, package_name, launch_file_name): + name = package_name + "/" + launch_file_name + + if name in self.processes: + if self.processes[name].is_alive(): + return {"status": "running"} + else: + return {"status": "failed"} + else: + return {"status": "stopped"} + + + def stop(self, package_name, launch_file_name): + name = package_name + "/" + launch_file_name + + if name in self.processes: + try: + self.processes[name].stop() + self.processes[name].join() + except: + return {"status": "error occured"} + else: + return {"status": "successfuly stopped"} + else: + return {"status": "already stopped. ignoring"} + + if __name__ == "__main__": launch_process = LaunchProcess( package_name="nav_stack", diff --git a/capabilities2_launch/launch_server/server.py b/capabilities2_launch/launch_server/server.py new file mode 100644 index 0000000..beb0251 --- /dev/null +++ b/capabilities2_launch/launch_server/server.py @@ -0,0 +1,65 @@ +from launch_server.launch_process import LaunchManager +from multiprocessing.connection import Listener + +class LaunchServer: + def __init__(self): + self.address = ('localhost', 6000) + self.listener = Listener(self.address, authkey=b'capabilities2') + self.manager = LaunchManager() + + def initialize(self): + self.connection = self.listener.accept() + print("[Launch Server] connection Accepted from {self.listener.last_accepted}") + + def run(self): + while True: + # message would be a dictionary with the format of {"command": , "package": , "launch_file": } + message_incoming = self.connection.recv() + + command = message_incoming["command"] + + if ((command=="start") or (command=="stop") or (command=="status")): + package_name = message_incoming["package"] + launch_file_name = message_incoming["launch_file"] + else: + package_name = "" + launch_file_name = "" + + if (command=="start"): + print("[Launch Server] start request for {package_name}/{launch_file_name}") + status = self.manager.start(package_name=package_name, launch_file_name=launch_file_name) + result = status["status"] + + print("[Launch Server] start response of {package_name}/{launch_file_name}: {result}") + + elif (command=="stop"): + print("[Launch Server] stop request for {package_name}/{launch_file_name}") + status = self.manager.stop(package_name=package_name, launch_file_name=launch_file_name) + result = status["status"] + + print("[Launch Server] stop response of {package_name}/{launch_file_name}: {result}") + + elif (command=="status"): + print("[Launch Server] status request for {package_name}/{launch_file_name}") + status = self.manager.status(package_name=package_name, launch_file_name=launch_file_name) + result = status["status"] + + print("[Launch Server] status response of {package_name}/{launch_file_name}: {result}") + + elif (command=="exit"): + break + + else: + print("[Launch Server] does not support command of type : {command}") + break + + self.connection.send(status) + + self.listener.close + + +if __name__ == "__main__": + + server = LaunchServer() + server.initialize() + server.run() \ No newline at end of file diff --git a/capabilities2_launch_py/package.xml b/capabilities2_launch/package.xml similarity index 94% rename from capabilities2_launch_py/package.xml rename to capabilities2_launch/package.xml index 6c1c013..8b0c6fa 100644 --- a/capabilities2_launch_py/package.xml +++ b/capabilities2_launch/package.xml @@ -1,7 +1,7 @@ - capabilities2_launch_py + capabilities2_launch 0.0.0 TODO: Package description kalana @@ -11,7 +11,7 @@ capabilities2_msgs ros2launch - + ament_copyright ament_flake8 ament_pep257 diff --git a/capabilities2_launch_py/resource/capabilities2_launch_py b/capabilities2_launch/resource/capabilities2_launch similarity index 100% rename from capabilities2_launch_py/resource/capabilities2_launch_py rename to capabilities2_launch/resource/capabilities2_launch diff --git a/capabilities2_launch/setup.cfg b/capabilities2_launch/setup.cfg new file mode 100644 index 0000000..9844c9c --- /dev/null +++ b/capabilities2_launch/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/capabilities2_launch +[install] +install_scripts=$base/lib/capabilities2_launch diff --git a/capabilities2_launch_py/setup.py b/capabilities2_launch/setup.py similarity index 77% rename from capabilities2_launch_py/setup.py rename to capabilities2_launch/setup.py index 605a0cf..856147b 100644 --- a/capabilities2_launch_py/setup.py +++ b/capabilities2_launch/setup.py @@ -2,7 +2,7 @@ import os from glob import glob -package_name = 'capabilities2_launch_py' +package_name = 'capabilities2_launch' setup( name=package_name, @@ -12,6 +12,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', package_name, 'launch_server'), glob(os.path.join('launch_server', '*.py'))), ], install_requires=['setuptools'], zip_safe=True, @@ -22,7 +23,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'capabilities2_launch_py = capabilities2_launch_py.launch_server:main', + 'capabilities2_launch = capabilities2_launch.capabilities2_launch:main', ], }, ) diff --git a/capabilities2_launch_py/test/test_copyright.py b/capabilities2_launch/test/test_copyright.py similarity index 100% rename from capabilities2_launch_py/test/test_copyright.py rename to capabilities2_launch/test/test_copyright.py diff --git a/capabilities2_launch_py/test/test_flake8.py b/capabilities2_launch/test/test_flake8.py similarity index 100% rename from capabilities2_launch_py/test/test_flake8.py rename to capabilities2_launch/test/test_flake8.py diff --git a/capabilities2_launch_py/test/test_pep257.py b/capabilities2_launch/test/test_pep257.py similarity index 100% rename from capabilities2_launch_py/test/test_pep257.py rename to capabilities2_launch/test/test_pep257.py diff --git a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py b/capabilities2_launch_py/capabilities2_launch_py/launch_server.py deleted file mode 100644 index 0f1a462..0000000 --- a/capabilities2_launch_py/capabilities2_launch_py/launch_server.py +++ /dev/null @@ -1,79 +0,0 @@ -import os -import signal -import rclpy -from launch_process import launch_process -from rclpy.node import Node -from capabilities2_msgs.srv import Launch - -class LaunchServer(Node): - def __init__(self): - super().__init__('launch_file_server') - - # Service for starting a launch file - self.start_server = self.create_service( - Launch, - '/capabilities/launch_start', - self.start_request - ) - - # Service for stopping a launch file - self.stop_server = self.create_service( - Launch, - '/capabilities/launch_stop', - self.stop_request - ) - - # Dictionary to track running processes - self.processes = {} - - self.get_logger().info('LaunchFileServer is ready.') - - def start_request(self, request, response): - package_name = request.package_name - launch_name = request.launch_file_name - - self.get_logger().info(f'Received launch start request: package = {package_name}, launch file = {launch_name}') - - name = package_name + "/" + launch_name - - self.processes[name] = launch_process.LaunchProcess(package_name=package_name, launch_file_name=launch_name) - self.processes[name].start() - - return response - - def stop_request(self, request, response): - package_name = request.package_name - launch_name = request.launch_file_name - - self.get_logger().info(f'Received launch stop request: package = {package_name}, launch file = {launch_name}') - - name = package_name + "/" + launch_name - - self.processes[name].stop() - self.processes[name].join() - - return response - - -def main(args=None): - rclpy.init(args=args) - - # Create the LaunchServer node - launch_server = LaunchServer() - - # Keep the node running - try: - rclpy.spin(launch_server) - except KeyboardInterrupt: - launch_server.get_logger().info('Shutting down LaunchFileServer...') - finally: - # Clean up all running processes - for pid, process in launch_server.processes.items(): - os.killpg(os.getpgid(pid), signal.SIGTERM) - process.wait() - - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/capabilities2_launch_py/setup.cfg b/capabilities2_launch_py/setup.cfg deleted file mode 100644 index 5cc0c4a..0000000 --- a/capabilities2_launch_py/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/capabilities2_launch_py -[install] -install_scripts=$base/lib/capabilities2_launch_py diff --git a/capabilities2_msgs/srv/Launch.srv b/capabilities2_msgs/srv/Launch.srv index b3eedd3..a1938ed 100644 --- a/capabilities2_msgs/srv/Launch.srv +++ b/capabilities2_msgs/srv/Launch.srv @@ -2,4 +2,5 @@ std_msgs/Header header string package_name string launch_file_name --- -std_msgs/Header header \ No newline at end of file +std_msgs/Header header +string status \ No newline at end of file diff --git a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp index 1b74036..2697bf3 100644 --- a/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp +++ b/capabilities2_runner/include/capabilities2_runner/launch_runner.hpp @@ -37,36 +37,36 @@ class LaunchRunner : public RunnerBase launch_name = run_config_.runner.substr(run_config_.runner.find("/") + 1); // create an service client - start_service_client_ = node_->create_client("/capabilities/launch_start"); + start_service_client_ = node_->create_client("/capabilities/launch/start"); - RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch_start", + RCLCPP_INFO(node_->get_logger(), "%s waiting for service: /capabilities/launch/start", run_config_.interface.c_str()); if (!start_service_client_->wait_for_service(std::chrono::seconds(3))) { - RCLCPP_ERROR(node_->get_logger(), "%s failed to connect to service: /capabilities/launch_start", + 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"); + throw runner_exception("Failed to connect to server: /capabilities/launch/start"); } - RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch_start", + 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"); + 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", + 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(), "%s failed to connect to service: /capabilities/launch_stop", + 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"); + throw runner_exception("Failed to connect to server: /capabilities/launch/stop"); } - RCLCPP_INFO(node_->get_logger(), "%s connected to service: /capabilities/launch_stop", + 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 diff --git a/capabilities2_runner_nav2/CMakeLists.txt b/capabilities2_runner_nav2/CMakeLists.txt index 3edafbc..1ce3c58 100644 --- a/capabilities2_runner_nav2/CMakeLists.txt +++ b/capabilities2_runner_nav2/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(nav2_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(capabilities2_msgs REQUIRED) find_package(capabilities2_runner REQUIRED) @@ -44,6 +45,7 @@ ament_target_dependencies(${PROJECT_NAME} pluginlib nav2_msgs geometry_msgs + lifecycle_msgs capabilities2_msgs capabilities2_runner ) diff --git a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp index 0a5cd6d..a28a2da 100644 --- a/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp +++ b/capabilities2_runner_nav2/include/capabilities2_runner_nav2/waypoint_runner.hpp @@ -6,12 +6,15 @@ #include #include #include +#include #include #include #include +using namespace std::chrono_literals; + namespace capabilities2_runner { @@ -57,6 +60,9 @@ class WayPointRunner : public ActionRunner geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = "map"; + pose_msg.header.stamp.sec = 0; + pose_msg.header.stamp.nanosec = 0; + pose_msg.pose.position.x = x; pose_msg.pose.position.y = y; pose_msg.pose.position.z = 0.0; @@ -87,6 +93,8 @@ class WayPointRunner : public ActionRunner std::string robot_base_frame_; /**The frame of the robot base*/ double x, y; /**Coordinate frame parameters*/ + + rclcpp::Client::SharedPtr get_state_client_; }; } // namespace capabilities2_runner