From cffaea0dde647a1d4c3a6dfc371de48501181890 Mon Sep 17 00:00:00 2001 From: rayvburn Date: Sat, 12 Feb 2022 04:27:20 +0100 Subject: [PATCH] hubero_ros - TaskRequestRos: acceptance of a new goal request during execution of the same task (workaround) [#23] --- hubero_ros/include/hubero_ros/task_request_ros.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/hubero_ros/include/hubero_ros/task_request_ros.h b/hubero_ros/include/hubero_ros/task_request_ros.h index f6c290cf..69856472 100644 --- a/hubero_ros/include/hubero_ros/task_request_ros.h +++ b/hubero_ros/include/hubero_ros/task_request_ros.h @@ -204,6 +204,15 @@ class TaskRequestRos: public TaskRequestBase { // check if new goal was selected and execute procedure after new goal received (once) if (as_ptr->isNewGoalAvailable()) { + // make sure that current goal is aborted + abort(task_type); + /* + * NOTE: this sleep must be longer than Actor class update period (FSM must be updated to catch + * the trigger of abort->request->activation) + * // FIXME: work out a cleaner/nicer solution + */ + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + auto goal = as_ptr->acceptNewGoal(); bool new_request_ok = requestActionGoal(goal); HUBERO_LOG(