From 9288096f46fa5ee0319c4eaf98636f846bf76d3d Mon Sep 17 00:00:00 2001 From: Ollie Walsh Date: Wed, 30 Aug 2023 13:44:27 +0100 Subject: [PATCH] Dock when rain detected and only dock when mowing Log the docking reason. 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 | 1 + .../mower_logic/behaviors/IdleBehavior.cpp | 3 +- .../src/mower_logic/mower_logic.cpp | 38 ++++++++++++++++++- src/open_mower/config/mower_config.sh.example | 3 ++ src/open_mower/launch/open_mower.launch | 1 + 5 files changed, 43 insertions(+), 3 deletions(-) diff --git a/src/mower_logic/cfg/MowerLogic.cfg b/src/mower_logic/cfg/MowerLogic.cfg index 58699a10..fd069571 100755 --- a/src/mower_logic/cfg/MowerLogic.cfg +++ b/src/mower_logic/cfg/MowerLogic.cfg @@ -30,5 +30,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 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")) diff --git a/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp index 94045c5d..bede3f66 100644 --- a/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp @@ -31,6 +31,7 @@ extern dynamic_reconfigure::Server *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 diff --git a/src/mower_logic/src/mower_logic/mower_logic.cpp b/src/mower_logic/src/mower_logic/mower_logic.cpp index 88f81746..561683ee 100644 --- a/src/mower_logic/src/mower_logic/mower_logic.cpp +++ b/src/mower_logic/src/mower_logic/mower_logic.cpp @@ -51,6 +51,8 @@ #include "xbot_msgs/RegisterActionsSrv.h" #include #include +#include +#include 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 mowerEnabled; +ros::Time last_rain_check; +bool rain = false; +bool rain_detected_status = true; + Behavior *currentBehavior = &IdleBehavior::INSTANCE; @@ -464,8 +470,34 @@ 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; } @@ -474,6 +506,7 @@ void checkSafety(const ros::TimerEvent &timer_event) { currentBehavior != &DockingBehavior::INSTANCE && currentBehavior != &UndockingBehavior::RETRY_INSTANCE ) { + ROS_INFO_STREAM(dockingReason.rdbuf()); abortExecution(); } } @@ -757,6 +790,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); diff --git a/src/open_mower/config/mower_config.sh.example b/src/open_mower/config/mower_config.sh.example index bb737a80..ca24668c 100644 --- a/src/open_mower/config/mower_config.sh.example +++ b/src/open_mower/config/mower_config.sh.example @@ -127,6 +127,9 @@ export OM_AUTOMATIC_MODE=0 export OM_OUTLINE_OFFSET=0.05 +# Return to dock when rain detected and pause automatic start until rain sensor is dry +export OM_DOCK_WHEN_RAINING=False + ################################ ## External MQTT Broker ## ################################ diff --git a/src/open_mower/launch/open_mower.launch b/src/open_mower/launch/open_mower.launch index 6d775190..1d0afc18 100644 --- a/src/open_mower/launch/open_mower.launch +++ b/src/open_mower/launch/open_mower.launch @@ -26,6 +26,7 @@ +