Skip to content

Commit

Permalink
Dock when rain detected and only dock when mowing
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
olliewalsh committed Oct 14, 2023
1 parent 9955648 commit 79da90c
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 8 deletions.
1 change: 1 addition & 0 deletions src/mower_logic/cfg/MowerLogic.cfg
Original file line number Diff line number Diff line change
Expand Up @@ -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"))
3 changes: 2 additions & 1 deletion src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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
Expand Down
44 changes: 37 additions & 7 deletions src/mower_logic/src/mower_logic/mower_logic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;


Expand Down Expand Up @@ -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();
}
}
Expand Down Expand Up @@ -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);

Expand Down
1 change: 1 addition & 0 deletions src/open_mower/launch/open_mower.launch
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>

Expand Down

0 comments on commit 79da90c

Please sign in to comment.