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 feedback) { ui->logTextBrowser->append(QString::fromStdString(feedback->message)); }; + goal_option.goal_response_callback = + [](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) { + // if (goal_handle->get_status() == rclcpp_action::GoalStatus::) {} + }; + goal_option.result_callback = + [&](const rclcpp_action::ClientGoalHandle::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(skills::Status::FAILURE)) { + ui->logTextBrowser->append(QString::fromStdString("Task " + task.name + " failed")); + } else if (task_result == static_cast(skills::Status::SUCCESS)) { + ui->logTextBrowser->append(QString::fromStdString("Task " + task.name + " succeeded")); + } + }; } CraneCommander::~CraneCommander()