Use non-blocking action logic for first point

The current sync/blocking action can quite often trigger battery undervoltage as low voltage condition will not abort until the first path is reached. Also skip_area/skip_path does not apply until the first point is reached.
ClemensElflein · Oct 27, 2023 · 56b0a7a · 56b0a7a
1 parent 2e07c31
commit 56b0a7a
Showing 1 changed file with 48 additions and 6 deletions.
diff --git a/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp
@@ -284,11 +284,55 @@ bool MowingBehavior::execute_mowing_plan() {
             mbf_msgs::MoveBaseGoal moveBaseGoal;
             moveBaseGoal.target_pose = path.path.poses.front();
             moveBaseGoal.controller = "FTCPlanner";
-            auto result = mbfClient->sendGoalAndWait(moveBaseGoal);
+            mbfClient->sendGoal(moveBaseGoal);
+            actionlib::SimpleClientGoalState current_status(actionlib::SimpleClientGoalState::PENDING);
+            ros::Rate r(10);
+
+            // wait for path execution to finish
+            while (ros::ok()) {
+                current_status = mbfClient->getState();
+                if (current_status.state_ == actionlib::SimpleClientGoalState::ACTIVE ||
+                    current_status.state_ == actionlib::SimpleClientGoalState::PENDING) {
+                    // path is being executed, everything seems fine.
+                    // check if we should pause or abort mowing
+                    if(skip_area) {
+                        ROS_INFO_STREAM("MowingBehavior: (FIRST POINT) SKIP AREA was requested.");
+                        // remove all paths in current area and return true
+                        mowerEnabled = false;
+                        mbfClientExePath->cancelAllGoals();
+                        currentMowingPaths.clear();
+                        skip_area = false;
+                        return true;
+                    }
+                    if(skip_path) {
+                        currentMowingPaths.erase(currentMowingPaths.begin());
+                        skip_path=false;
+                        return false;
+                    }
+                    if (aborted) {
+                        ROS_INFO_STREAM("MowingBehavior: (FIRST POINT) ABORT was requested - stopping path execution.");
+                        mbfClientExePath->cancelAllGoals();
+                        mowerEnabled = false;
+                        return false;
+                    }
+                    if (requested_pause_flag) {
+                        ROS_INFO_STREAM("MowingBehavior: (FIRST POINT) PAUSE was requested - stopping path execution.");
+                        mbfClientExePath->cancelAllGoals();
+                        mowerEnabled = false;
+                        return false;
+                    }
+                } else {
+                    ROS_INFO_STREAM("MowingBehavior: (FIRST POINT)  Got status " << current_status.state_ << " from MBF/FTCPlanner -> Stopping path execution.");
+                    // we're done, break out of the loop
+                    break;
+                }
+                r.sleep();
+            }
+
             first_point_attempt_counter++;
-            if (result.state_ != result.SUCCEEDED) {
+            if (current_status.state_ != actionlib::SimpleClientGoalState::SUCCEEDED) {
                 // we cannot reach the start point
-                ROS_ERROR_STREAM("MowingBehavior: (FIRST POINT) - Could not reach goal (first point). Planner Status was: " << result.state_);
+                ROS_ERROR_STREAM("MowingBehavior: (FIRST POINT) - Could not reach goal (first point). Planner Status was: " << current_status.state_);
                 // we have 3 attempts to get to the start pose of the mowing area
                 if (first_point_attempt_counter < config.max_first_point_attempts)
                 {
@@ -364,13 +408,11 @@ bool MowingBehavior::execute_mowing_plan() {
                         skip_area = false;
                         return true;
                     }
-                    if(skip_path) 
-                    {
+                    if(skip_path) {
                         currentMowingPaths.erase(currentMowingPaths.begin());
                         skip_path=false;
                         return false;
                     }
-
                     if (aborted) {
                         ROS_INFO_STREAM("MowingBehavior: (MOW) ABORT was requested - stopping path execution.");
                         mbfClientExePath->cancelAllGoals();