add cancel task

src/brain/include/brain/cerebrum_node.hpp

@@ -270,6 +270,9 @@ private:
    * @thread_safety Calls into ActionClientRegistry; assumed thread-safe for cancellation.
    */
   void CancelActiveExecuteBtGoal();
+
+  void StopBehaviorTree(bool cancel_goals);
+
   /**
    * @brief (Legacy) Update per-epoch allowed skills. Currently a no-op (handled by EpochSkillFilter).
    * @param skills Ignored.

src/brain/src/cerebrum_node.cpp

@@ -912,6 +912,58 @@ void CerebrumNode::CancelActiveExecuteBtGoal()
   }
 }
 
+void CerebrumNode::StopBehaviorTree(bool cancel_goals)
+{
+  // 1) Cancel any active action goals
+  if (cancel_goals) {
+    try {
+      CancelActiveExecuteBtGoal();
+    } catch (...) {
+      RCLCPP_ERROR(this->get_logger(), "Exception while cancelling ExecuteBtAction goals");
+    }
+  }
+
+  // 2) Invalidate in-flight feedback/results by bumping epoch
+  const uint64_t new_epoch = epoch_filter_.epoch() + 1;
+  epoch_filter_.reset_epoch(new_epoch, {});
+
+  // 3) Halt the BT (forces nodes to IDLE)
+  try {
+    tree_.haltTree();
+  } catch (...) {
+    RCLCPP_ERROR(this->get_logger(), "Exception halting Behavior Tree");
+  }
+
+  // 4) Stop periodic ticking and clear cooldown gate
+  bt_pending_run_ = false;
+  g_bt_resume_time_steady = std::chrono::steady_clock::time_point{};
+
+  // 5) Mark any per-instance execution as cancelled and finished
+  for (auto & kv : per_instance_exec_) {
+    auto & inst = kv.second;
+    if (inst.in_flight.load(std::memory_order_acquire) && !inst.finished.load(std::memory_order_acquire)) {
+      inst.in_flight.store(false, std::memory_order_release);
+      inst.finished.store(true, std::memory_order_release);
+      inst.success = false;
+      inst.result_code = ExecResultCode::CANCELLED;
+    }
+  }
+  per_node_exec_.clear();
+  {
+    std::lock_guard<std::mutex> lk(exec_mutex_);
+    done_instances_epoch_.clear();
+  }
+
+  // 6) Update work info
+  if (robot_work_info_.task.size() > 2) {
+    robot_work_info_.task[2] = "None"; // current
+  }
+  robot_work_info_.bt_node_status = "CANCELLED";
+  robot_work_time_.total_seconds += (rclcpp::Clock().now() - robot_work_time_.start).seconds();
+
+  RCLCPP_INFO(this->get_logger(), "Behavior Tree stopped (epoch=%llu)", static_cast<unsigned long long>(new_epoch));
+}
+
 /**
  * @brief Legacy epoch skill setter (now a no-op; filtering handled elsewhere).
  * @param skills Ignored.
@@ -1020,6 +1072,22 @@ void CerebrumNode::CreateServices()
     [this](
       const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
       std::shared_ptr<interfaces::srv::BtRebuild::Response> resp) {
+
+      if (req->config == "CancelBTTask") {
+        if (!bt_pending_run_) {
+          // Even if not pending, still call StopBehaviorTree to ensure cleanup states.
+          StopBehaviorTree(true);
+          resp->success = true;
+          resp->message = "No active BT (cleanup applied)";
+          RCLCPP_WARN(this->get_logger(), "No active BT to cancel (cleanup applied)");
+          return;
+        }
+        StopBehaviorTree(true);
+        resp->success = true;
+        resp->message = "Cancel BTTask success";
+        return;
+      }
+
       // Check if rebuild is allowed
       if (bt_pending_run_ && !allow_bt_rebuild_during_execution_) {
         resp->success = false;
@@ -1059,6 +1127,20 @@ void CerebrumNode::HandleTriggerRebuild(
   const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
   const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp)
 {
+
+  if (req->config == "CancelBTTask") {
+    try {
+      tree_.haltTree();
+      RCLCPP_INFO(this->get_logger(), "halting previous BT ok, CancelBTTask");
+    } catch (...) {
+      // Swallow halt exceptions.
+      RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT CancelBTTask");
+    }
+    resp->success = true;
+    resp->message = "Rebuild triggered";
+    return;
+  }
+
   for (const auto & path_param : bt_config_params_paths_) {
     // path_param is a pair {config, param}; compare the config path string against the current path
     // if (path_param.config == current_bt_config_params_path_.config) {