Update mower behavior skip path option added

.github/workflows/build-image.yaml

@@ -7,11 +7,13 @@ on:
     branches:
       - 'main'
       - 'releases/**'
+      - 'startinzone'
     tags:
       - 'v*.*.*'
   pull_request:
     branches:
       - 'main'
+      - 'startinzone'
 
 # Allow to stop obsolete workflows
 concurrency:

src/mower_logic/cfg/MowerLogic.cfg

@@ -29,5 +29,5 @@ gen.add("add_fake_obstacle", bool_t, 0, "True to add a fake obstacle to hopefull
 gen.add("ignore_gps_errors", bool_t, 0, "True to ignore gps errors. USE ONLY FOR SIMULATION!", False)
 gen.add("max_first_point_attempts", int_t, 0, "Maximum attempts to reach the first point of the mow path before trimming", 3, 1, 10)
 gen.add("max_first_point_trim_attempts", int_t, 0, "After <max_first_point_attempts> we start to trim the path beginning this often", 3, 1, 10)
-
+gen.add("clear_path_on_start", bool_t, 0, "True to reset current mowing path on start, useful to start in given area", False)
 exit(gen.generate("mower_logic", "mower_logic", "MowerLogic"))

src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp

@@ -73,6 +73,7 @@ Behavior *MowingBehavior::execute() {
 
 void MowingBehavior::enter() {
     skip_area = false;
+    skip_path = false;
     paused = aborted = false;
 
     for(auto& a : actions) {
@@ -252,7 +253,7 @@ bool MowingBehavior::execute_mowing_plan() {
             update_actions();
             mowerEnabled = true;
         }
-
+    
 
         auto &path = currentMowingPaths.front();
         ROS_INFO_STREAM("MowingBehavior: Path segment length: " << path.path.poses.size() << " poses.");
@@ -363,6 +364,13 @@ bool MowingBehavior::execute_mowing_plan() {
                         skip_area = false;
                         return true;
                     }
+                    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();
@@ -489,11 +497,17 @@ MowingBehavior::MowingBehavior() {
     skip_area_action.enabled = false;
     skip_area_action.action_name = "Skip Area";
 
+    xbot_msgs::ActionInfo skip_path_action;
+    skip_path_action.action_id = "skip_path";
+    skip_path_action.enabled = false;
+    skip_path_action.action_name = "Skip Path";
+
     actions.clear();
     actions.push_back(pause_action);
     actions.push_back(continue_action);
     actions.push_back(abort_mowing_action);
     actions.push_back(skip_area_action);
+    actions.push_back(skip_path_action);
 }
 
 void MowingBehavior::handle_action(std::string action) {
@@ -515,6 +529,9 @@ void MowingBehavior::handle_action(std::string action) {
     } else if(action == "mower_logic:mowing/skip_area") {
         ROS_INFO_STREAM("got skip_area command");
         skip_area = true;
+    } else if(action == "mower_logic:mowing/skip_path") {
+        ROS_INFO_STREAM("got skip_path command");
+        skip_path = true;
     }
     update_actions();
 }

src/mower_logic/src/mower_logic/behaviors/MowingBehavior.h

@@ -31,6 +31,7 @@ class MowingBehavior : public Behavior {
     std::vector<xbot_msgs::ActionInfo> actions;
 
     bool skip_area;
+    bool skip_path;
     bool create_mowing_plan(int area_index);
 
     bool execute_mowing_plan();