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..53296052 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_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); 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 @@ +