diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index da2a1d62..a30d6b3d 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -318,7 +318,12 @@ ExecutorNode::get_plan_service_callback( const std::shared_ptr request, const std::shared_ptr response) { - response->plan = *complete_plan_; + if (complete_plan_ != nullptr) { + response->success = true; + response->plan = *complete_plan_; + } else { + response->success = false; + } } void @@ -327,7 +332,12 @@ ExecutorNode::get_remaining_plan_service_callback( const std::shared_ptr request, const std::shared_ptr response) { - response->plan = *remaining_plan_; + if (remaining_plan_ != nullptr) { + response->success = true; + response->plan = *remaining_plan_; + } else { + response->success = false; + } } rclcpp_action::GoalResponse @@ -678,6 +688,9 @@ ExecutorNode::execute_plan() executor_state_ = IDLE_STATE; runtime_info = PlanRuntineInfo(); + complete_plan_ = nullptr; + remaining_plan_ = nullptr; + ordered_sub_goals_ = nullptr; } void