From c8b07ac898bb638391de11d67c85133c46206d0e 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. 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 | 1 + .../mower_logic/behaviors/IdleBehavior.cpp | 3 +- .../src/mower_logic/mower_logic.cpp | 44 ++++++++++++++++--- src/open_mower/launch/open_mower.launch | 1 + 4 files changed, 41 insertions(+), 8 deletions(-) diff --git a/src/mower_logic/cfg/MowerLogic.cfg b/src/mower_logic/cfg/MowerLogic.cfg index e20c7b85..e87a623f 100755 --- a/src/mower_logic/cfg/MowerLogic.cfg +++ b/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 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..f27e83e2 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,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_pcb; 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); diff --git a/src/open_mower/launch/open_mower.launch b/src/open_mower/launch/open_mower.launch index 9392fb5f..554cf28b 100644 --- a/src/open_mower/launch/open_mower.launch +++ b/src/open_mower/launch/open_mower.launch @@ -23,6 +23,7 @@ +