feat: Docking behavior fixes (#26)

Wait for GPS in docking approach.

If docking approach fails, it's not safe to undock, so just retry docking.
Reset retry count after docking approach.

When starting mowing, only undock if docked, otherwise just start mowing.

If docking needed, e.g low battery, do not abort the current behaviour if
it's already docking/undocking.

src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp

@@ -173,6 +173,10 @@ std::string DockingBehavior::state_name() {
 }
 
 Behavior *DockingBehavior::execute() {
+    while(!isGPSGood){
+        ROS_WARN_STREAM("Waiting for good GPS");
+        ros::Duration(10.0).sleep();
+    }
 
     bool approachSuccess = approach_docking_point();
 
@@ -181,14 +185,17 @@ Behavior *DockingBehavior::execute() {
 
         retryCount++;
         if(retryCount <= config.docking_retry_count) {
-            ROS_ERROR("Retrying docking");
-            return &UndockingBehavior::RETRY_INSTANCE;
+            ROS_ERROR("Retrying docking approach");
+            return &DockingBehavior::INSTANCE;
         }
 
         ROS_ERROR("Giving up on docking");
         return &IdleBehavior::INSTANCE;
     }
 
+    // Reset retryCount
+    reset();
+
     // Disable GPS
     inApproachMode = false;
     setGPS(false);

src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp

@@ -73,8 +73,11 @@ Behavior *IdleBehavior::execute() {
             if(last_status.v_charge > 5.0) {
                 ROS_INFO_STREAM("Currently inside the docking station, we set the robot's pose to the docks pose.");
                 setRobotPose(docking_pose_stamped.pose);
+                return &UndockingBehavior::INSTANCE;
             }
-            return &UndockingBehavior::INSTANCE;
+            // Not docked, so just mow
+            setGPS(true);
+            return &MowingBehavior::INSTANCE;
         }
 
         if(start_area_recorder) {

src/mower_logic/src/mower_logic/mower_logic.cpp

@@ -401,7 +401,11 @@ void checkSafety(const ros::TimerEvent &timer_event) {
         dockingNeeded = true;
     }
 
-    if (dockingNeeded) {
+    if (
+            dockingNeeded &&
+            currentBehavior != &DockingBehavior::INSTANCE &&
+            currentBehavior != &UndockingBehavior::RETRY_INSTANCE
+        ) {
         abortExecution();
     }
 }