Dock when rain detected and only dock when mowing

Log the docking reason.
Only dock when mowing, otherwise can abort DockingBehavior.

Add option to dock when rain is detected.
Initial logic is very basic, just dock when rain detected and if automatic
mode is set will stay docked until rain is no longer detected.

src/mower_logic/cfg/MowerLogic.cfg

@@ -29,5 +29,6 @@ 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("dock_when_raining", bool_t, 0, "True to dock when rain is detected", False)
 
 exit(gen.generate("mower_logic", "mower_logic", "MowerLogic"))

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

@@ -31,6 +31,7 @@ extern dynamic_reconfigure::Server<mower_logic::MowerLogicConfig> *reconfigServe
 extern ros::ServiceClient mapClient;
 extern ros::ServiceClient dockingPointClient;
 
+extern bool rain;
 
 IdleBehavior IdleBehavior::INSTANCE;
 
@@ -72,7 +73,7 @@ Behavior *IdleBehavior::execute() {
         const bool automatic_mode = last_config.automatic_mode == eAutoMode::AUTO;
         const bool active_semiautomatic_task = last_config.automatic_mode == eAutoMode::SEMIAUTO && shared_state->active_semiautomatic_task == true;
         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 && !(last_config.dock_when_raining && rain);
 
         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

@@ -51,6 +51,8 @@
 #include "xbot_msgs/RegisterActionsSrv.h"
 #include <mutex>
 #include <atomic>
+#include <sstream>
+#include <ios>
 
 ros::ServiceClient pathClient, mapClient, dockingPointClient, gpsClient, mowClient, emergencyClient, pathProgressClient, setNavPointClient, clearNavPointClient, clearMapClient, positioningClient, actionRegistrationClient;
 
@@ -79,6 +81,10 @@ mower_msgs::HighLevelStatus high_level_status;
 
 std::atomic<bool> mowerEnabled;
 
+ros::Time last_rain_check;
+bool rain = false;
+bool rain_detected_status = true;
+
 Behavior *currentBehavior = &IdleBehavior::INSTANCE;
 
 
@@ -464,16 +470,39 @@ void checkSafety(const ros::TimerEvent &timer_event) {
 
     // we are in non emergency, check if we should pause. This could be empty battery, rain or hot mower motor etc.
     bool dockingNeeded = false;
-    if (last_status.v_battery < last_config.battery_empty_voltage || last_status.mow_esc_status.temperature_motor >= last_config.motor_hot_temperature ||
-        last_config.manual_pause_mowing) {
+    std::stringstream dockingReason("Docking: ", std::ios_base::ate | std::ios_base::in | std::ios_base::out);
+
+    if (last_config.manual_pause_mowing) {
+        dockingReason << "Manual pause";
+        dockingNeeded = true;
+    }
+
+    // rain_detected_status is initialized to true and flips to false if rain is not detected
+    // continuously for 20s. This is to avoid false positives due to noise
+    rain_detected_status = rain_detected_status && last_status.rain_detected;
+    if (ros::Time::now() - last_rain_check > ros::Duration(20.0)) {
+        rain = rain_detected_status;
+        last_rain_check = ros::Time::now();
+        rain_detected_status = true;
+    }
+
+    if(!dockingNeeded && rain && last_config.dock_when_raining) {
+        dockingReason << "Rain detected";
+        dockingNeeded = true;
+    }
+
+    if(!dockingNeeded && last_status.v_battery < last_config.battery_empty_voltage) {
+        dockingReason << "Battery low: " << last_status.v_battery;
+        dockingNeeded = true;
+    }
+
+    if (!dockingNeeded && last_status.mow_esc_status.temperature_motor >= last_config.motor_hot_temperature) {
+        dockingReason << "Over temp: " << last_status.mow_esc_status.temperature_motor;
         dockingNeeded = true;
     }
 
-    if (
-            dockingNeeded &&
-            currentBehavior != &DockingBehavior::INSTANCE &&
-            currentBehavior != &UndockingBehavior::RETRY_INSTANCE
-        ) {
+    if (dockingNeeded && currentBehavior == &MowingBehavior::INSTANCE) {
+        ROS_INFO_STREAM(dockingReason.rdbuf());
         abortExecution();
     }
 }
@@ -757,6 +786,7 @@ int main(int argc, char **argv) {
 
 
 
+    last_rain_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/open_mower/launch/open_mower.launch

@@ -23,6 +23,7 @@
         <param name="battery_full_voltage" value="$(env OM_BATTERY_FULL_VOLTAGE)"/>
         <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="dock_when_raining" value="$(env OM_DOCK_WHEN_RAINING False)"/>
     </node>
     <node pkg="slic3r_coverage_planner" type="slic3r_coverage_planner" name="slic3r_coverage_planner" output="screen"/>