Implement rain logic (#148)

Adds 3 modes for rain:
0 - Ignore it
1 - Dock when rain detected. If in automatic mode it will resume again
when battery is full etc...
2 - Dock until dry. If in automatic mode will resume again when rain is
no longer detected for OM_RAIN_DELAY_MINUTES
3 - Pause Automatic Mode. Pauses automatic mode when rain detected.
Needs to be manually resumed

.github/workflows/build-image.yaml

@@ -48,9 +48,7 @@ jobs:
       # Get the repository's code
       - name: Checkout
         uses: actions/checkout@v4
-
       - uses: actions/setup-python@v5
-
       - uses: pre-commit/[email protected]
 
       - name: Docker meta

config/mower_config.schema.json

@@ -401,6 +401,32 @@
           "description": "Set to true to record your session for debugging. Recordings will be stored in $HOME. Only enable for testing, otherwise your SD card will get full soon.",
           "x-environment-variable": "OM_ENABLE_RECORDING_ALL"
         },
+        "OM_RAIN_MODE": {
+          "type": "string",
+          "enum": [
+            "Ignore",
+            "Dock",
+            "Dock Until Dry",
+            "Pause Automatic Mode"
+          ],
+          "default": "Ignore",
+          "title": "Rain Mode",
+          "description": "Behaviour when rain detected",
+          "x-environment-variable": "OM_RAIN_MODE",
+          "x-remap-values": {
+            "Ignore": 0,
+            "Dock": 1,
+            "Dock Until Dry": 2,
+            "Pause Automatic Mowing": 3
+          }
+        },
+        "OM_RAIN_DELAY_MINUTES": {
+          "type": "number",
+          "default": 30,
+          "title": "Rain delay minutes",
+          "description": "How long to wait after rain to resume mowing when mode is \"Dock Until Dry\"",
+          "x-environment-variable": "OM_RAIN_DELAY_MINUTES"
+        },
         "advanced": {
           "type": "boolean",
           "description": "Show advanced Mower Logic settings"

config/mower_config.sh.example

@@ -191,6 +191,17 @@ export OM_AUTOMATIC_MODE=0
 
 export OM_OUTLINE_OFFSET=0.05
 
+# Rain mode
+#  0 - Ignore
+#  1 - Dock
+#  2 - Dock Until Dry
+#  3 - Pause Automatic Mode
+export OM_RAIN_MODE=0
+
+# Rain delay
+# How long to wait after rain to resume mowing when mode is "Dock Until Dry"
+export OM_RAIN_DELAY_MINUTES=30
+
 ################################
 ##    External MQTT Broker    ##
 ################################

src/mower_logic/cfg/MowerLogic.cfg

@@ -33,5 +33,8 @@ 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("rain_mode", int_t, 0, "0 - Ignore, 1 - Dock, 2 - Dock until dry, 3 - Pause Automatic Mode", 0, 0, 3)
+gen.add("rain_delay_minutes", int_t, 0, "Time to wait after rain to resume mowing", 1, 30, 1440)
+gen.add("rain_check_seconds", int_t, 0, "Rain must be detected continuosly for this time to trigger rain_mode", 0, 20, 300)
 
 exit(gen.generate("mower_logic", "mower_logic", "MowerLogic"))

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

@@ -25,6 +25,7 @@ extern void setEmergencyMode(bool emergency);
 extern void setGPS(bool enabled);
 extern void setRobotPose(geometry_msgs::Pose &pose);
 extern void registerActions(std::string prefix, const std::vector<xbot_msgs::ActionInfo> &actions);
+extern ros::Time rain_resume;
 
 extern ros::ServiceClient dockingPointClient;
 extern mower_msgs::Status getStatus();
@@ -74,9 +75,13 @@ Behavior *IdleBehavior::execute() {
     const bool active_semiautomatic_task = last_config.automatic_mode == eAutoMode::SEMIAUTO &&
                                            shared_state->active_semiautomatic_task &&
                                            !shared_state->semiautomatic_task_paused;
+    const bool rain_delay = last_config.rain_mode == 2 && ros::Time::now() < rain_resume;
+    if (rain_delay) {
+      ROS_INFO_STREAM_THROTTLE(300, "Rain delay: " << int((rain_resume - ros::Time::now()).toSec() / 60) << " minutes");
+    }
     const bool mower_ready = last_status.v_battery > last_config.battery_full_voltage &&
                              last_status.mow_esc_status.temperature_motor < last_config.motor_cold_temperature &&
-                             !last_config.manual_pause_mowing;
+                             !last_config.manual_pause_mowing && !rain_delay;
 
     if (manual_start_mowing || ((automatic_mode || active_semiautomatic_task) && mower_ready)) {
       // set the robot's position to the dock if we're actually docked

src/mower_logic/src/mower_logic/mower_logic.cpp

@@ -90,6 +90,10 @@ std::vector<xbot_msgs::ActionInfo> rootActions;
 ros::Time last_v_battery_check;
 double max_v_battery_seen = 0.0;
 
+ros::Time last_rain_check;
+bool rain_detected = true;
+ros::Time rain_resume;
+
 /**
  * Some thread safe methods to get a copy of the logic state
  */
@@ -534,6 +538,29 @@ void checkSafety(const ros::TimerEvent &timer_event) {
     dockingNeeded = true;
   }
 
+  // Rain detected is initialized to true and flips to false if rain is not detected
+  // continuously for rain_check_seconds. This is to avoid false positives due to noise
+  rain_detected = rain_detected && last_status.rain_detected;
+  if (last_config.rain_check_seconds == 0 ||
+      ros::Time::now() - last_rain_check > ros::Duration(last_config.rain_check_seconds)) {
+    if (rain_detected) {
+      // Reset rain resume time
+      rain_resume =
+          ros::Time::now() + ros::Duration(last_config.rain_check_seconds + last_config.rain_delay_minutes * 60);
+    }
+    if (!dockingNeeded && rain_detected && last_config.rain_mode) {
+      dockingReason << "Rain detected";
+      dockingNeeded = true;
+      if (last_config.rain_mode == 3) {
+        auto new_config = getConfig();
+        new_config.manual_pause_mowing = true;
+        setConfig(new_config);
+      }
+    }
+    last_rain_check = ros::Time::now();
+    rain_detected = true;
+  }
+
   if (dockingNeeded && currentBehavior != &DockingBehavior::INSTANCE &&
       currentBehavior != &UndockingBehavior::RETRY_INSTANCE && currentBehavior != &IdleBehavior::INSTANCE &&
       currentBehavior != &IdleBehavior::DOCKED_INSTANCE) {
@@ -815,7 +842,7 @@ int main(int argc, char **argv) {
 
   ROS_INFO("om_mower_logic: Got all servers, we can mow");
 
-  last_v_battery_check = ros::Time::now();
+  rain_resume = last_rain_check = last_v_battery_check = ros::Time::now();
   ros::Timer safety_timer = n->createTimer(ros::Duration(0.5), checkSafety);
   ros::Timer ui_timer = n->createTimer(ros::Duration(1.0), updateUI);
 

src/mower_simulation/cfg/MowerSimulation.cfg

@@ -11,6 +11,7 @@ gen.add("mower_error", bool_t, 0, "mower error", False)
 gen.add("mower_running", bool_t, 0, "mower running", False)
 gen.add("wheels_stalled", bool_t, 0, "wheels_stalled", False)
 gen.add("emergency_stop", bool_t, 0, "emergency_stop", False)
+gen.add("rain", bool_t, 0, "rain", False)
 
 
 exit(gen.generate("mower_simulation", "mower_simulation", "MowerSimulation"))

src/mower_simulation/src/mower_simulation.cpp

@@ -103,6 +103,7 @@ void publishStatus(const ros::TimerEvent &timer_event) {
 
   fake_mow_status.stamp = ros::Time::now();
   fake_mow_status.mow_enabled = config.mower_running;
+  fake_mow_status.rain_detected = config.rain;
   fake_mow_status.mow_esc_status.temperature_motor = config.temperature_mower;
   fake_mow_status.mow_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_OK;
   if (config.mower_error) {

src/open_mower/launch/open_mower.launch

@@ -37,6 +37,9 @@
         <param name="motor_cold_temperature" value="$(env OM_MOWING_MOTOR_TEMP_LOW)"/>
         <param name="gps_wait_time" value="$(optenv OM_GPS_WAIT_TIME_SEC 10.0)"/>
         <param name="gps_timeout" value="$(optenv OM_GPS_TIMEOUT_SEC 10.0)"/>
+        <param name="rain_mode" value="$(optenv OM_RAIN_MODE 0)"/>
+        <param name="rain_delay_minutes" value="$(optenv OM_RAIN_DELAY_MINUTES 30)"/>
+        <param name="rain_check_seconds" value="$(optenv OM_RAIN_CHECK_SECONDS 20)"/>
     </node>
     <node pkg="slic3r_coverage_planner" type="slic3r_coverage_planner" name="slic3r_coverage_planner" output="screen"/>
 

src/open_mower/launch/sim_mower_logic.launch

@@ -13,7 +13,7 @@
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(find open_mower)/rviz/sim_mower_logic.rviz" required="true" />
     <node pkg="mower_map" type="mower_map_service" name="mower_map" required="true" />
     <node pkg="slic3r_coverage_planner" type="slic3r_coverage_planner" name="slic3r_coverage_planner" output="screen" required="true" />
-    <node pkg="mower_logic" type="mower_logic" name="mower_logic" output="log" required="false">
+    <node pkg="mower_logic" type="mower_logic" name="mower_logic" output="screen" required="true">
         <param name="ignore_gps_errors" value="true"/>
         <param name="gps_wait_time" value="0"/>
         <param name="automatic_mode" value="$(optenv OM_AUTOMATIC_MODE 0)"/>
@@ -35,6 +35,9 @@
         <param name="mow_angle_increment" value="$(optenv OM_MOWING_ANGLE_INCREMENT 0)"/>
         <param name="motor_hot_temperature" value="$(env OM_MOWING_MOTOR_TEMP_HIGH)"/>
         <param name="motor_cold_temperature" value="$(env OM_MOWING_MOTOR_TEMP_LOW)"/>
+        <param name="rain_mode" value="$(optenv OM_RAIN_MODE 0)"/>
+        <param name="rain_delay_minutes" value="$(optenv OM_RAIN_DELAY_MINUTES 30)"/>
+        <param name="rain_check_seconds" value="$(optenv OM_RAIN_CHECK_SECONDS 20)"/>
     </node>
 
     <node pkg="twist_mux" type="twist_mux" name="twist_mux" output="screen">