diff --git a/src/mower_logic/cfg/MowerLogic.cfg b/src/mower_logic/cfg/MowerLogic.cfg index 82bdfdb0..e20c7b85 100755 --- a/src/mower_logic/cfg/MowerLogic.cfg +++ b/src/mower_logic/cfg/MowerLogic.cfg @@ -13,6 +13,7 @@ gen.add("outline_count", int_t, 0, "Outline count to mow before filling", 3, 0, gen.add("outline_offset", double_t, 0, "Additional outer outline offset, use positive values for safety, negative values to enlarge the area", 0.0, -1.0, 1.0) gen.add("mow_angle_offset", double_t, 0, "Mowing angle offset", 0, -180, 180) gen.add("mow_angle_offset_is_absolute", bool_t, 0, "If true then the offset is relative to East, if false then the offset is relative to the auto-detected angle.", False) +gen.add("mow_angle_increment", double_t, 0, "Mowing angle automatic increment. Will be added to the offset every time the entire map is finished", 0, 0, 180) gen.add("tool_width", double_t, 0, "Width of the mower", 0.14, 0.1, 2) gen.add("enable_mower", bool_t, 0, "True to enable mow motor", False) gen.add("manual_pause_mowing", bool_t, 0, "True to disable mowing automatically", False) diff --git a/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp index 9aae438d..949a4b72 100644 --- a/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp @@ -92,10 +92,17 @@ void MowingBehavior::reset() { currentMowingPaths.clear(); auto config = getConfig(); config.current_area = 0; + if (config.automatic_mode == eAutoMode::SEMIAUTO) { ROS_INFO_STREAM("MowingBehavior: Finished semiautomatic task"); shared_state->active_semiautomatic_task = false; } + + // increment mowing angle offset and return into the <-180, 180> range + config.mow_angle_offset = std::fmod(config.mow_angle_offset + config.mow_angle_increment + 180, 360); + if (config.mow_angle_offset < 0) config.mow_angle_offset += 360; + config.mow_angle_offset -= 180; + setConfig(config); } diff --git a/src/open_mower/config/mower_config.sh.example b/src/open_mower/config/mower_config.sh.example index 947c317b..dad0c46a 100644 --- a/src/open_mower/config/mower_config.sh.example +++ b/src/open_mower/config/mower_config.sh.example @@ -90,6 +90,8 @@ export OM_OUTLINE_COUNT=4 # Mowing angle offset -180 deg - +180 deg, 0 = east, -90 = north. If mowing angle offset is not absolute it gets added to the auto detected angle which is set by the first 2 m of recorded outline. export OM_MOWING_ANGLE_OFFSET=0 export OM_MOWING_ANGLE_OFFSET_IS_ABSOLUTE=False +# The increment value will automatically add specified number of degrees to the mowing angle everytime the whole map is finished +export OM_MOWING_ANGLE_INCREMENT=0 # The width of mowing paths. # Choose it smaller than your actual mowing tool in order to have some overlap. diff --git a/src/open_mower/launch/open_mower.launch b/src/open_mower/launch/open_mower.launch index 2edfb66e..a5abd9c7 100644 --- a/src/open_mower/launch/open_mower.launch +++ b/src/open_mower/launch/open_mower.launch @@ -19,6 +19,7 @@ +