mower logic update with all paths progress

ClemensElflein · Aug 10, 2023 · 5ee2df5 · 5ee2df5
1 parent 8bff336
commit 5ee2df5
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 2 deletions.
diff --git a/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp
@@ -136,6 +136,8 @@ bool MowingBehavior::create_mowing_plan(int area_index) {
     ROS_INFO_STREAM("MowingBehavior: Creating mowing plan for area: " << area_index);
     // Delete old plan and progress.
     currentMowingPaths.clear();
+    currentMowingPathsProgress = 0;
+    currentMowingPathsLength = 0;
 
     // get the mowing area
     mower_map::GetMowingAreaSrv mapSrv;
@@ -188,6 +190,12 @@ bool MowingBehavior::create_mowing_plan(int area_index) {
 
     currentMowingPaths = pathSrv.response.paths;
 
+    // calculate currentMowingPaths length
+    currentMowingPathsLength = 0;
+    for(auto& path : currentMowingPaths) {
+        currentMowingPathsLength += path.path.poses.size();
+    }
+
     return true;
 }
 
@@ -391,7 +399,7 @@ bool MowingBehavior::execute_mowing_plan() {
                     // show progress
                     ROS_INFO_STREAM_THROTTLE(5, "MowingBehavior: (MOW) Progress: " << getCurrentMowPathIndex() << "/" << path.path.poses.size());
 
-                    int progress = (int)round((getCurrentMowPathIndex() / (float)path.path.poses.size()) * 100);
+                    int progress = (int)round((currentMowingPathsProgress + getCurrentMowPathIndex() / (float)path.path.poses.size()) * 100);
                     subStateName = std::to_string(progress) + "%";
                 } else {
                     ROS_INFO_STREAM("MowingBehavior: (MOW)  Got status " << current_status.state_ << " from MBF/FTCPlanner -> Stopping path execution.");
@@ -408,15 +416,20 @@ bool MowingBehavior::execute_mowing_plan() {
                 int currentIndex = getCurrentMowPathIndex();
                 ROS_INFO_STREAM(">> MowingBehavior: (MOW) PlannerGetProgress currentIndex = " << currentIndex << " of " << path.path.poses.size());
                 printNavState(current_status.state_);
+
+                auto posesSize = path.path.poses.size();
+
                 // if we have fully processed the segment or we have encountered an error, drop the path segment
                 /* TODO: we can not trust the SUCCEEDED state because the planner sometimes says suceeded with
                     the currentIndex far from the size of the poses ! (BUG in planner ?)
                     instead we trust only the currentIndex vs. poses.size() */
-                if (currentIndex >= path.path.poses.size() || (path.path.poses.size() - currentIndex) < 5) // fully mowed the path ?
+                if (currentIndex >= posesSize || (posesSize - currentIndex) < 5) // fully mowed the path ?
                 {
                     ROS_INFO_STREAM("MowingBehavior: (MOW) Mow path finished, skipping to next mow path.");
                     currentMowingPaths.erase(currentMowingPaths.begin());
                     // continue with next segment
+
+                    currentMowingPathsProgress += posesSize;
                 }
                 else
                 {

diff --git a/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.h b/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.h
@@ -37,6 +37,8 @@ class MowingBehavior : public Behavior {
     // Progress
     bool mowerEnabled = false;
     std::vector<slic3r_coverage_planner::Path> currentMowingPaths;
+    unsigned int currentMowingPathsLength = 0;
+    unsigned int currentMowingPathsProgress = 0;
 
     std::string subStateName;