diff --git a/eplansys_demo/pddl/survey-sites.pddl b/eplansys_demo/pddl/survey-sites.pddl index 50d305ce..878a320d 100644 --- a/eplansys_demo/pddl/survey-sites.pddl +++ b/eplansys_demo/pddl/survey-sites.pddl @@ -32,7 +32,6 @@ :condition (and (at start (at_depot ?r))) :effect (and - (at start (not (at_depot ?r))) (at end (on_north ?r))) ) @@ -42,7 +41,6 @@ :condition (and (at start (at_depot ?r))) :effect (and - (at start (not (at_depot ?r))) (at end (on_south ?r))) ) diff --git a/eplansys_demo/pddl/survey.pddl b/eplansys_demo/pddl/survey.pddl index 9be4fc27..01710438 100644 --- a/eplansys_demo/pddl/survey.pddl +++ b/eplansys_demo/pddl/survey.pddl @@ -3,6 +3,10 @@ ;; What the robots know lives in the epistemic task the planner solved; this ;; file is what the executor needs to drive the actions that task names. ;; +;; A drive leaves `at_depot` alone. Consuming it would make the drive +;; undispatchable a second time, and a mission that replans after a failure +;; dispatches it a second time. +;; ;; Deliberately thin, and deliberately blind. Nothing here distinguishes a ;; broadcast from an encrypted relay: both are a robot talking, they take the ;; same time and change the same facts, and a PDDL domain has no vocabulary @@ -29,7 +33,6 @@ :condition (and (at start (at_depot ?r))) :effect (and - (at start (not (at_depot ?r))) (at end (on_site ?r))) ) diff --git a/eplansys_demo/src/survey_mission.cpp b/eplansys_demo/src/survey_mission.cpp index e7fcfe71..6a67b074 100644 --- a/eplansys_demo/src/survey_mission.cpp +++ b/eplansys_demo/src/survey_mission.cpp @@ -12,17 +12,28 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Starts the corridor mission, so that the demo is one command. +// Starts the site survey, so that the demo is one command. // // The same three steps anyone would type into `ros2 plansys2 terminal`: say // what exists, say what is wanted, run. Doing it from a node instead means the // demo has nothing to type and nothing to get wrong, and it is the shortest // honest description of how a mission is started. - +// +// It does one thing more, and it is the thing a mission on hardware needs: +// when an action fails it asks for a policy again rather than stopping. The +// new policy is planned from the epistemic state the mission reached, so what +// was learned before the failure is kept and only what is left is planned for. +// An action can fail for a reason that outlives one attempt --- a fleet that +// stopped answering, a door that will not open --- so the attempts are counted +// and the mission gives up saying which action it was that kept failing. + +#include +#include #include #include #include "plansys2_msgs/action/execute_plan.hpp" +#include "plansys2_msgs/msg/action_execution_info.hpp" #include "plansys2_msgs/msg/plan.hpp" #include "plansys2_executor/ExecutorClient.hpp" #include "plansys2_planner/PlannerClient.hpp" @@ -67,43 +78,94 @@ int main(int argc, char ** argv) // travels with the EPDDL the planner grounds and solves. problem->setGoal(plansys2::Goal("(and(told scout))")); - RCLCPP_INFO(node->get_logger(), "planning"); - const auto plan = planner->getPlan(domain->getDomain(), problem->getProblem()); - if (!plan.has_value()) { - RCLCPP_ERROR(node->get_logger(), "no plan; is the epistemic solver configured?"); - rclcpp::shutdown(); - return 1; + // How many policies the mission is willing to ask for. One is the classical + // behaviour: plan, run, report. More than one is what makes a failure + // recoverable, and the bound is what keeps a permanent failure from being + // retried for ever. + int attempts = 3; + for (int i = 1; i + 1 < argc; ++i) { + if (std::string(argv[i]) == "--attempts") { + attempts = std::atoi(argv[i + 1]); + } } - bool branches = false; - for (const auto & item : plan->items) { - branches = branches || item.children.size() > 1; - } - RCLCPP_INFO( - node->get_logger(), "policy with %zu nodes, %s", - plan->items.size(), branches ? "branching" : "linear"); - - if (!executor->start_plan_execution(plan.value())) { - RCLCPP_ERROR(node->get_logger(), "the executor refused the policy"); - rclcpp::shutdown(); - return 1; - } + bool succeeded = false; - RCLCPP_INFO(node->get_logger(), "executing"); - rclcpp::Rate rate(4); - while (rclcpp::ok() && executor->execute_and_check_plan()) { - rclcpp::spin_some(node); - rate.sleep(); - } + for (int attempt = 1; attempt <= attempts && rclcpp::ok() && !succeeded; ++attempt) { + RCLCPP_INFO(node->get_logger(), "planning (attempt %d of %d)", attempt, attempts); + + // On any attempt after the first this is a replan, and the planner reads + // the epistemic state as the mission left it: a scan that already happened + // is not planned for again. + const auto plan = planner->getPlan(domain->getDomain(), problem->getProblem()); + if (!plan.has_value()) { + RCLCPP_ERROR(node->get_logger(), "no plan; is the epistemic solver configured?"); + rclcpp::shutdown(); + return 1; + } + + bool branches = false; + for (const auto & item : plan->items) { + branches = branches || item.children.size() > 1; + } + RCLCPP_INFO( + node->get_logger(), "policy with %zu nodes, %s", + plan->items.size(), branches ? "branching" : "linear"); + + if (plan->items.empty()) { + // A replan returns nothing when the goal already holds, which is a + // mission that finished between the failure and the replan. + RCLCPP_INFO(node->get_logger(), "the goal already holds; nothing left to run"); + succeeded = true; + break; + } - const auto result = executor->getResult(); - const bool succeeded = - result && result->result == plansys2_msgs::action::ExecutePlan::Result::SUCCESS; + if (!executor->start_plan_execution(plan.value())) { + RCLCPP_ERROR(node->get_logger(), "the executor refused the policy"); + rclcpp::shutdown(); + return 1; + } + + RCLCPP_INFO(node->get_logger(), "executing"); + rclcpp::Rate rate(4); + while (rclcpp::ok() && executor->execute_and_check_plan()) { + rclcpp::spin_some(node); + rate.sleep(); + } + + const auto result = executor->getResult(); + succeeded = result && + result->result == plansys2_msgs::action::ExecutePlan::Result::SUCCESS; + + if (succeeded) { + break; + } + + // Which action died is the whole of what a reader needs afterwards, and + // the executor knows it: the result carries a status per action. + if (result) { + for (const auto & status : result->action_execution_status) { + if (status.status == plansys2_msgs::msg::ActionExecutionInfo::FAILED) { + RCLCPP_WARN( + node->get_logger(), "%s failed: %s", + status.action_full_name.c_str(), status.message_status.c_str()); + } + } + } + + if (attempt < attempts) { + RCLCPP_WARN( + node->get_logger(), + "the policy failed; replanning from the state the mission reached"); + std::this_thread::sleep_for(2s); + rclcpp::spin_some(node); + } + } if (succeeded) { RCLCPP_INFO(node->get_logger(), "mission complete"); } else { - RCLCPP_ERROR(node->get_logger(), "mission failed"); + RCLCPP_ERROR(node->get_logger(), "mission failed after %d attempt(s)", attempts); } rclcpp::shutdown();