From 378db909677b0e79609211082dd449bf581b2c9a Mon Sep 17 00:00:00 2001 From: Oliver Walsh Date: Sun, 5 May 2024 21:59:39 +0100 Subject: [PATCH] Workaround actionlib race (#83) Looks like there is a race condition in actionlib when calling getState soon after sendGoal: [ INFO] [1714746133.764753198]: MowingBehavior: (FIRST POINT) Moving to path segment starting point [ERROR] [1714746133.766643334]: getElem() should not see invalid handles Try adding a sleep to workaround it. --- src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp index eeb98bd5..b2cb21d2 100644 --- a/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp @@ -328,6 +328,7 @@ bool MowingBehavior::execute_mowing_plan() { moveBaseGoal.target_pose = path.path.poses[currentMowingPathIndex]; moveBaseGoal.controller = "FTCPlanner"; mbfClient->sendGoal(moveBaseGoal); + sleep(1); actionlib::SimpleClientGoalState current_status(actionlib::SimpleClientGoalState::PENDING); ros::Rate r(10); @@ -438,6 +439,7 @@ bool MowingBehavior::execute_mowing_plan() { ROS_INFO_STREAM("MowingBehavior: (MOW) First point reached - Executing mow path with " << path.path.poses.size() << " poses, from index " << exePathStartIndex); mbfClientExePath->sendGoal(exePathGoal); + sleep(1); actionlib::SimpleClientGoalState current_status(actionlib::SimpleClientGoalState::PENDING); ros::Rate r(10);