Handle IdleBehavior::DOCKED_INSTANCE in more places (#120)

These were missed in #114. Especially the failure to reset emergency mode got my mower stuck in the docking station. I hope I got the other cases right as well.
ClemensElflein · Jul 8, 2024 · 96be2d3 · 96be2d3
1 parent 7e73539
commit 96be2d3
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 14 deletions.
diff --git a/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp
@@ -77,7 +77,7 @@ Behavior *IdleBehavior::execute() {
                 !last_config.manual_pause_mowing;
 
         if (manual_start_mowing || ((automatic_mode || active_semiautomatic_task) && mower_ready)) {
-                        // set the robot's position to the dock if we're actually docked
+            // set the robot's position to the dock if we're actually docked
             if(last_status.v_charge > 5.0) {
               if (PerimeterUndockingBehavior::configured(config))
                 return &PerimeterUndockingBehavior::INSTANCE;
@@ -158,7 +158,7 @@ void IdleBehavior::command_s1() {
 }
 
 void IdleBehavior::command_s2() {
-    
+
 }
 
 bool IdleBehavior::redirect_joystick() {

diff --git a/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.cpp b/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.cpp
@@ -100,15 +100,15 @@ Behavior* PerimeterSearchBehavior::execute() {
   /* We expect to be currently outside the perimeter wire => negative signal */
   calibrationLeft=calibrationRight=signCenter=
     lastPerimeter.left<0 ? 1 : -1; // Determine polarity of the cable.
-  
+
   if (innerSignal()>-MIN_SIGNAL || outerSignal()>-MIN_SIGNAL) {
     ROS_ERROR("Signal too weak.");
     return shutdownConnections();
   }
   calibrationLeft/=fabs(lastPerimeter.left);
   maxCenter=fabs(lastPerimeter.center);
   calibrationRight/=fabs(lastPerimeter.right);
-  
+
   geometry_msgs::Twist vel;
   /* Move straight until one of the signals changes to positive (inside) */
   vel.angular.z=0;
@@ -177,7 +177,7 @@ Behavior* PerimeterUndockingBehavior::execute() {
   // Determine polarity of the cable.
   calibrationLeft=calibrationRight=signCenter=1;
   if (innerSignal()<0) calibrationLeft=calibrationRight=signCenter=-1;
-  
+
   float maxLeft=lastPerimeter.left*calibrationLeft;
   maxCenter=fabs(lastPerimeter.center);
   float maxRight=lastPerimeter.right*calibrationRight;;
@@ -202,7 +202,7 @@ Behavior* PerimeterUndockingBehavior::execute() {
     ROS_ERROR("Could not turn inwards");
     return shutdownConnections();
   }
-  
+
   if (maxLeft<MIN_SIGNAL || maxRight<MIN_SIGNAL) {
     ROS_ERROR("Signal too weak.");
     return shutdownConnections();
@@ -228,12 +228,12 @@ Behavior* PerimeterDockingBehavior::arrived() {
     chargeSeen++;
     if (chargeSeen>=2) {
       chargeSeen=0;
-      return &IdleBehavior::INSTANCE;
+      return &IdleBehavior::DOCKED_INSTANCE;
     }
   } else {
     chargeSeen=0;
   }
-  return NULL; 
+  return NULL;
 }
 
 Behavior* PerimeterFollowBehavior::execute() {
@@ -381,4 +381,3 @@ std::string PerimeterMoveToGpsBehavior::state_name() {
 Behavior* PerimeterMoveToGpsBehavior::arrived() {
   return travelled>=config.undock_distance-0.9 ? &MowingBehavior::INSTANCE : NULL;
 }
-
diff --git a/src/mower_logic/src/mower_logic/mower_logic.cpp b/src/mower_logic/src/mower_logic/mower_logic.cpp
@@ -396,7 +396,7 @@ void checkSafety(const ros::TimerEvent &timer_event) {
     if (currentBehavior != nullptr) {
         if (last_status.emergency) {
             currentBehavior->requestPause(pauseType::PAUSE_EMERGENCY);
-            if (currentBehavior == &AreaRecordingBehavior::INSTANCE || currentBehavior == &IdleBehavior::INSTANCE) {
+            if (currentBehavior == &AreaRecordingBehavior::INSTANCE || currentBehavior == &IdleBehavior::INSTANCE || currentBehavior == &IdleBehavior::DOCKED_INSTANCE) {
                 if (last_status.v_charge > 10.0) {
                     // emergency and docked and idle or area recording, so it's safe to reset the emergency mode, reset it. It's safe since we won't start moving in this mode.
                     setEmergencyMode(false);
@@ -517,7 +517,8 @@ void checkSafety(const ros::TimerEvent &timer_event) {
             dockingNeeded &&
             currentBehavior != &DockingBehavior::INSTANCE &&
             currentBehavior != &UndockingBehavior::RETRY_INSTANCE &&
-            currentBehavior != &IdleBehavior::INSTANCE
+            currentBehavior != &IdleBehavior::INSTANCE &&
+            currentBehavior != &IdleBehavior::DOCKED_INSTANCE
     ) {
         ROS_INFO_STREAM(dockingReason.rdbuf());
         abortExecution();
@@ -558,8 +559,7 @@ bool highLevelCommand(mower_msgs::HighLevelControlSrvRequest &req, mower_msgs::H
         case mower_msgs::HighLevelControlSrvRequest::COMMAND_DELETE_MAPS: {
             ROS_WARN_STREAM("COMMAND_DELETE_MAPS");
             if (currentBehavior != &AreaRecordingBehavior::INSTANCE && currentBehavior != &IdleBehavior::INSTANCE &&
-                currentBehavior !=
-                nullptr) {
+                currentBehavior != &IdleBehavior::DOCKED_INSTANCE && currentBehavior != nullptr) {
                 ROS_ERROR_STREAM("Deleting maps is only allowed during IDLE or AreaRecording!");
                 return true;
             }
@@ -861,4 +861,3 @@ int main(int argc, char **argv) {
     delete (mbfClientExePath);
     return 0;
 }
-