Merge pull request #338 from fmrico/replanning

[WIP] Replanning

plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp

@@ -23,6 +23,8 @@
 #include "plansys2_core/Types.hpp"
 #include "plansys2_domain_expert/DomainExpertInterface.hpp"
 
+#include "std_msgs/msg/string.hpp"
+
 #include "plansys2_msgs/msg/action.hpp"
 #include "plansys2_msgs/msg/derived.hpp"
 #include "plansys2_msgs/msg/durative_action.hpp"
@@ -157,6 +159,14 @@ class DomainExpertClient : public DomainExpertInterface
    */
   std::string getDomain();
 
+  /// Get the current cached domain, ready to be saved to file, or to initialize another domain.
+  /**
+   * \param[in] use_cache Use cache, if available.
+   * \return A string containing the domain.
+   */
+  std::string getDomain(bool use_cache);
+  std::string cached_domain_;
+
 private:
   rclcpp::Node::SharedPtr node_;
 
@@ -176,6 +186,7 @@ class DomainExpertClient : public DomainExpertInterface
   rclcpp::Client<plansys2_msgs::srv::GetDomainActionDetails>::SharedPtr get_action_details_client_;
   rclcpp::Client<plansys2_msgs::srv::GetDomainDurativeActionDetails>::SharedPtr
     get_durative_action_details_client_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr domain_sub_;
 };
 
 }  // namespace plansys2

plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertNode.hpp

@@ -267,6 +267,8 @@ class DomainExpertNode : public rclcpp_lifecycle::LifecycleNode
     validate_domain_client_;
   rclcpp::CallbackGroup::SharedPtr validate_domain_callback_group_;
 
+  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr domain_pub_;
+
   std::unique_ptr<plansys2::POPFPlanSolver> popf_plan_solver_;
 };
 

plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertClient.cpp

@@ -59,6 +59,12 @@ DomainExpertClient::DomainExpertClient()
   get_durative_action_details_client_ =
     node_->create_client<plansys2_msgs::srv::GetDomainDurativeActionDetails>(
     "domain_expert/get_domain_durative_action_details");
+
+  domain_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    "domain_expert/domain",
+    rclcpp::QoS(100).transient_local(), [this](std_msgs::msg::String::SharedPtr msg) {
+      cached_domain_ = msg->data;
+    });
 }
 
 std::string
@@ -526,6 +532,16 @@ DomainExpertClient::getDurativeAction(
   }
 }
 
+std::string
+DomainExpertClient::getDomain(bool use_cache)
+{
+  if (use_cache && cached_domain_ != "") {
+    return cached_domain_;
+  } else {
+    return getDomain();
+  }
+}
+
 std::string
 DomainExpertClient::getDomain()
 {

plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp

@@ -107,6 +107,9 @@ DomainExpertNode::DomainExpertNode()
       &DomainExpertNode::get_domain_service_callback,
       this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
+  domain_pub_ = create_publisher<std_msgs::msg::String>(
+    "domain_expert/domain",
+    rclcpp::QoS(100).transient_local());
 }
 
 
@@ -181,6 +184,12 @@ DomainExpertNode::on_activate(const rclcpp_lifecycle::State & state)
   RCLCPP_INFO(get_logger(), "[%s] Activating...", get_name());
   RCLCPP_INFO(get_logger(), "[%s] Activated", get_name());
 
+  domain_pub_->on_activate();
+
+  std_msgs::msg::String domain_msg;
+  domain_msg.data = domain_expert_->getDomain();
+  domain_pub_->publish(domain_msg);
+
   return CallbackReturnT::SUCCESS;
 }
 
@@ -190,6 +199,8 @@ DomainExpertNode::on_deactivate(const rclcpp_lifecycle::State & state)
   RCLCPP_INFO(get_logger(), "[%s] Deactivating...", get_name());
   RCLCPP_INFO(get_logger(), "[%s] Deactivated", get_name());
 
+  domain_pub_->on_deactivate();
+
   return CallbackReturnT::SUCCESS;
 }
 
@@ -199,6 +210,8 @@ DomainExpertNode::on_cleanup(const rclcpp_lifecycle::State & state)
   RCLCPP_INFO(get_logger(), "[%s] Cleaning up...", get_name());
   RCLCPP_INFO(get_logger(), "[%s] Cleaned up", get_name());
 
+  domain_pub_->on_deactivate();
+
   return CallbackReturnT::SUCCESS;
 }
 

plansys2_domain_expert/test/unit/domain_expert_node_test.cpp

@@ -75,6 +75,7 @@ TEST(domain_expert, lifecycle)
     domain_node->get_current_state().id(),
     lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
 
+  ASSERT_EQ(domain_client->getDomain(), domain_client->getDomain(true));
   auto domain_str = domain_client->getDomain();
 
   {

plansys2_executor/include/plansys2_executor/ActionExecutor.hpp

@@ -77,6 +77,8 @@ class ActionExecutor
   std::string get_feedback() const {return feedback_;}
   float get_completion() const {return completion_;}
 
+  void clean_up();
+
 protected:
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
 

plansys2_executor/include/plansys2_executor/ExecutorNode.hpp

@@ -19,6 +19,7 @@
 #include <vector>
 #include <string>
 #include <map>
+#include <tuple>
 
 #include "plansys2_domain_expert/DomainExpertClient.hpp"
 #include "plansys2_problem_expert/ProblemExpertClient.hpp"
@@ -29,6 +30,9 @@
 #include "lifecycle_msgs/msg/state.hpp"
 #include "lifecycle_msgs/msg/transition.hpp"
 
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/blackboard.h"
+
 #include "plansys2_msgs/action/execute_plan.hpp"
 #include "plansys2_msgs/msg/action_execution_info.hpp"
 #include "plansys2_msgs/srv/get_ordered_sub_goals.hpp"
@@ -46,6 +50,23 @@
 namespace plansys2
 {
 
+struct TreeInfo
+{
+  using Ptr = std::shared_ptr<TreeInfo>;
+  BT::Tree tree;
+  BT::Blackboard::Ptr blackboard;
+  std::shared_ptr<plansys2::BTBuilder> bt_builder;
+};
+
+struct PlanRuntineInfo
+{
+  plansys2_msgs::msg::Plan remaining_plan;
+  plansys2_msgs::msg::Plan complete_plan;
+  std::vector<plansys2_msgs::msg::Tree> ordered_sub_goals;
+  std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map;
+  TreeInfo::Ptr current_tree;
+};
+
 class ExecutorNode : public rclcpp_lifecycle::LifecycleNode
 {
 public:
@@ -80,9 +101,11 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode
 
 protected:
   bool cancel_plan_requested_;
-  std::optional<plansys2_msgs::msg::Plan> remaining_plan_;
-  std::optional<plansys2_msgs::msg::Plan> complete_plan_;
-  std::optional<std::vector<plansys2_msgs::msg::Tree>> ordered_sub_goals_;
+  bool replan_requested_;
+
+  plansys2_msgs::msg::Plan * remaining_plan_;
+  plansys2_msgs::msg::Plan * complete_plan_;
+  std::vector<plansys2_msgs::msg::Tree> * ordered_sub_goals_;
 
   std::string action_bt_xml_;
   std::string start_action_bt_xml_;
@@ -103,7 +126,7 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode
     get_ordered_sub_goals_service_;
   rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr dotgraph_pub_;
 
-  std::optional<std::vector<plansys2_msgs::msg::Tree>> getOrderedSubGoals();
+  void get_ordered_subgoals(PlanRuntineInfo & runtime_info);
 
   rclcpp::Service<plansys2_msgs::srv::GetPlan>::SharedPtr get_plan_service_;
   rclcpp::Service<plansys2_msgs::srv::GetPlan>::SharedPtr get_remaining_plan_service_;
@@ -114,20 +137,27 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode
 
   rclcpp_action::CancelResponse handle_cancel(
     const std::shared_ptr<GoalHandleExecutePlan> goal_handle);
-
-  void execute(const std::shared_ptr<GoalHandleExecutePlan> goal_handle);
-
   void handle_accepted(const std::shared_ptr<GoalHandleExecutePlan> goal_handle);
+  std::shared_ptr<GoalHandleExecutePlan> current_goal_handle_;
 
   std::vector<plansys2_msgs::msg::ActionExecutionInfo> get_feedback_info(
     std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map);
 
   void print_execution_info(
     std::shared_ptr<std::map<std::string, ActionExecutionInfo>> exec_info);
 
-  void update_plan(
-    std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map,
-    std::optional<plansys2_msgs::msg::Plan> & remaining_plan);
+  void update_plan(PlanRuntineInfo & runtime_info);
+  bool init_plan_for_execution(PlanRuntineInfo & runtime_info);
+  bool replan_for_execution(PlanRuntineInfo & runtime_info);
+
+  void execute_plan();
+  bool get_tree_from_plan(PlanRuntineInfo & runtime_info);
+  void create_plan_runtime_info(PlanRuntineInfo & runtime_info);
+  void cancel_all_running_actions(PlanRuntineInfo & runtime_info);
+
+  static const int IDLE_STATE = 0;
+  static const int EXECUTING_STATE = 1;
+  int executor_state_;
 };
 
 }  // namespace plansys2

plansys2_executor/src/plansys2_executor/ActionExecutor.cpp

@@ -46,6 +46,14 @@ ActionExecutor::ActionExecutor(
   state_time_ = start_execution_;
 }
 
+void
+ActionExecutor::clean_up()
+{
+  if (action_hub_sub_ != nullptr) {
+    action_hub_sub_->clear_on_new_message_callback();
+  }
+}
+
 void
 ActionExecutor::action_hub_callback(const plansys2_msgs::msg::ActionExecution::SharedPtr msg)
 {

plansys2_executor/src/plansys2_executor/ComputeBT.cpp

@@ -245,8 +245,8 @@ ComputeBT::computeBTCallback(
 
   problem_client_->addProblem(problem_string);
 
-  auto domain = domain_client_->getDomain();
-  auto problem = problem_client_->getProblem();
+  auto domain = domain_client_->getDomain(true);
+  auto problem = problem_client_->getProblem(true);
   auto plan = planner_client_->getPlan(domain, problem);
 
   savePlan(plan.value(), problem_path.stem().u8string());

plansys2_executor/src/plansys2_executor/ExecutorClient.cpp

@@ -68,16 +68,11 @@ ExecutorClient::createActionClient()
 bool
 ExecutorClient::start_plan_execution(const plansys2_msgs::msg::Plan & plan)
 {
-  if (!executing_plan_) {
-    createActionClient();
-    auto success = on_new_goal_received(plan);
+  auto success = on_new_goal_received(plan);
 
-    if (success) {
-      executing_plan_ = true;
-      return true;
-    }
-  } else {
-    RCLCPP_INFO(node_->get_logger(), "Already executing a plan");
+  if (success) {
+    executing_plan_ = true;
+    return true;
   }
 
   return false;
@@ -99,8 +94,10 @@ ExecutorClient::execute_and_check_plan()
       if (result_.result == nullptr) {
         RCLCPP_WARN(
           node_->get_logger(), "Plan failed due to a nullptr in the result");
-      } else if (result_.result->success) {
+      } else if (result_.result->result == plansys2_msgs::action::ExecutePlan::Result::SUCCESS) {
         RCLCPP_INFO(node_->get_logger(), "Plan Succeeded");
+      } else if (result_.result->result == plansys2_msgs::action::ExecutePlan::Result::PREEMPT) {
+        RCLCPP_INFO(node_->get_logger(), "Plan Preempted");
       } else {
         RCLCPP_ERROR(node_->get_logger(), "Plan Failed");
         for (auto msg : result_.result->action_execution_status) {
@@ -158,7 +155,9 @@ ExecutorClient::execute_and_check_plan()
       throw std::logic_error("ExecutorClient::executePlan: invalid status value");
   }
 
-  executing_plan_ = false;
+  if (result_.result->result != plansys2_msgs::action::ExecutePlan::Result::PREEMPT) {
+    executing_plan_ = false;
+  }
   goal_result_available_ = false;
 
   return false;  // Plan finished
@@ -350,9 +349,11 @@ ExecutorClient::feedback_callback(
 void
 ExecutorClient::result_callback(const GoalHandleExecutePlan::WrappedResult & result)
 {
-  goal_result_available_ = true;
-  result_ = result;
-  feedback_ = ExecutePlan::Feedback();
+  if (goal_handle_.get()->get_goal_id() == result.goal_id) {
+    goal_result_available_ = true;
+    result_ = result;
+    feedback_ = ExecutePlan::Feedback();
+  }
 }
 
 std::optional<ExecutePlan::Result>