From baf750ee1e587519fb0a02e21eb5df458681a1ee Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Wed, 8 Jan 2025 01:07:29 +0900
Subject: [PATCH] =?UTF-8?q?client=E5=81=B4=E3=81=AE=E8=A8=AD=E5=AE=9A?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 crane_simple_ai/src/crane_commander.cpp | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/crane_simple_ai/src/crane_commander.cpp b/crane_simple_ai/src/crane_commander.cpp
index 7ebe67a0f..085fc6959 100644
--- a/crane_simple_ai/src/crane_commander.cpp
+++ b/crane_simple_ai/src/crane_commander.cpp
@@ -242,6 +242,30 @@ void CraneCommander::postSkill(
       const std::shared_ptr<const SkillExecution::Feedback> feedback) {
       ui->logTextBrowser->append(QString::fromStdString(feedback->message));
     };
+  goal_option.goal_response_callback =
+    [](rclcpp_action::ClientGoalHandle<SkillExecution>::SharedPtr goal_handle) {
+      // if (goal_handle->get_status() == rclcpp_action::GoalStatus::) {}
+    };
+  goal_option.result_callback =
+    [&](const rclcpp_action::ClientGoalHandle<SkillExecution>::WrappedResult result) {
+      auto & task = task_queue_execution.front();
+      auto task_result = result.result->result;
+      if (not task.retry()) {
+        task_queue_execution.pop_front();
+        if (task_queue_execution.empty()) {
+          onQueueToBeEmpty();
+        }
+      } else {
+        ui->logTextBrowser->append(QString::fromStdString(std::format(
+          "{}を再実行します。残り時間[s]：{}", task.name, std::to_string(task.getRestTime()))));
+        postSkill(name, parameters);
+      }
+      if (task_result == static_cast<uint16_t>(skills::Status::FAILURE)) {
+        ui->logTextBrowser->append(QString::fromStdString("Task " + task.name + " failed"));
+      } else if (task_result == static_cast<uint16_t>(skills::Status::SUCCESS)) {
+        ui->logTextBrowser->append(QString::fromStdString("Task " + task.name + " succeeded"));
+      }
+    };
 }
 
 CraneCommander::~CraneCommander()