From 54e1bbf9d1d46d0891ebb9aa8592a3a2446c6cd7 Mon Sep 17 00:00:00 2001
From: Ollie Walsh <ollie.walsh@gmail.com>
Date: Mon, 14 Oct 2024 01:40:32 +0100
Subject: [PATCH] Add manual mower control actions in AreaRecording mode

Previously users would call mower_comms setMowEnabled directly to enable the
mower while in AreaRecording mode.
However this should not have been possible since
AreaRecording::mower_enabled() always returned false.

mower_logic now confirms that the mower state is as expected,
so bypassing it in this way no longer works.

Add actions to toggle AreaRecording::mower_enabled() instead.
---
 .../behaviors/AreaRecordingBehavior.cpp       | 25 +++++++++++++++++--
 .../behaviors/AreaRecordingBehavior.h         |  2 ++
 2 files changed, 25 insertions(+), 2 deletions(-)

diff --git a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp
index 61cdac8c..5f7e471e 100644
--- a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp
+++ b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp
@@ -217,8 +217,7 @@ bool AreaRecordingBehavior::needs_gps() {
 }
 
 bool AreaRecordingBehavior::mower_enabled() {
-  // No mower during docking
-  return false;
+  return manual_mowing;
 }
 
 void AreaRecordingBehavior::pose_received(const xbot_msgs::AbsolutePose::ConstPtr &msg) {
@@ -578,6 +577,12 @@ void AreaRecordingBehavior::handle_action(std::string action) {
   } else if (action == "mower_logic:area_recording/collect_point") {
     ROS_INFO_STREAM("Got collect point");
     collect_point = true;
+  } else if (action == "mower_logic:area_recording/start_manual_mowing") {
+    ROS_INFO_STREAM("Starting manual mowing");
+    manual_mowing = true;
+  } else if (action == "mower_logic:area_recording/stop_manual_mowing") {
+    ROS_INFO_STREAM("Stopping manual mowing");
+    manual_mowing = false;
   }
   update_actions();
 }
@@ -633,6 +638,16 @@ AreaRecordingBehavior::AreaRecordingBehavior() {
   collect_point_action.enabled = false;
   collect_point_action.action_name = "Collect point";
 
+  xbot_msgs::ActionInfo start_manual_mowing_action;
+  start_manual_mowing_action.action_id = "start_manual_mowing";
+  start_manual_mowing_action.enabled = false;
+  start_manual_mowing_action.action_name = "Start manual mowing";
+
+  xbot_msgs::ActionInfo stop_manual_mowing_action;
+  stop_manual_mowing_action.action_id = "stop_manual_mowing";
+  stop_manual_mowing_action.enabled = false;
+  stop_manual_mowing_action.action_name = "Stop manual mowing";
+
   actions.clear();
   actions.push_back(start_recording_action);
   actions.push_back(stop_recording_action);
@@ -644,6 +659,8 @@ AreaRecordingBehavior::AreaRecordingBehavior() {
   actions.push_back(auto_point_collecting_enable_action);
   actions.push_back(auto_point_collecting_disable_action);
   actions.push_back(collect_point_action);
+  actions.push_back(start_manual_mowing_action);
+  actions.push_back(stop_manual_mowing_action);
 }
 
 void AreaRecordingBehavior::update_actions() {
@@ -681,6 +698,10 @@ void AreaRecordingBehavior::update_actions() {
         actions[6].enabled = true;
       }
     }
+    // start_manual_mowing
+    actions[10].enabled = !manual_mowing;
+    // stop manual mowing
+    actions[11].enabled = manual_mowing;
 
     registerActions("mower_logic:area_recording", actions);
   }
diff --git a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.h b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.h
index 823e7bef..1f3b26b4 100644
--- a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.h
+++ b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.h
@@ -89,6 +89,8 @@ class AreaRecordingBehavior : public Behavior {
   bool auto_point_collecting = true;
   bool collect_point = false;
 
+  bool manual_mowing = false;
+
   visualization_msgs::MarkerArray markers;
   visualization_msgs::Marker marker;