Fix: Prevent mowing motor from starting and stopping repeatedly if G…

…PS fix is lost. #125

src/mower_comms/src/mower_comms.cpp

@@ -48,7 +48,6 @@ ros::Publisher sensor_mag_pub;
 
 COBS cobs;
 
-
 // True, if ROS thinks there sould be an emergency
 bool emergency_high_level = false;
 // True, if the LL board thinks there should be an emergency
@@ -61,7 +60,7 @@ bool ll_clear_emergency = false;
 bool allow_send = false;
 
 // Current speeds (duty cycle) for the three ESCs
-float speed_l = 0, speed_r = 0, speed_mow = 0;
+float speed_l = 0, speed_r = 0, speed_mow = 0, target_speed_mow = 0;
 
 // Ticks / m and wheel distance for this robot
 double wheel_ticks_per_m = 0.0;
@@ -98,7 +97,10 @@ bool is_emergency() {
 }
 
 void publishActuators() {
-// emergency or timeout -> send 0 speeds
+
+    speed_mow = target_speed_mow;
+
+    // emergency or timeout -> send 0 speeds
     if (is_emergency()) {
         speed_l = 0;
         speed_r = 0;
@@ -186,6 +188,7 @@ void publishStatus() {
     status_msg.sound_module_available = (last_ll_status.status_bitmask & 0b00100000) != 0;
     status_msg.sound_module_busy = (last_ll_status.status_bitmask & 0b01000000) != 0;
     status_msg.ui_board_available = (last_ll_status.status_bitmask & 0b10000000) != 0;
+    status_msg.mow_enabled = !(target_speed_mow == 0);
 
     for (uint8_t i = 0; i < 5; i++) {
         status_msg.ultrasonic_ranges[i] = last_ll_status.uss_ranges_m[i];
@@ -272,11 +275,11 @@ void publishActuatorsTimerTask(const ros::TimerEvent &timer_event) {
 
 bool setMowEnabled(mower_msgs::MowerControlSrvRequest &req, mower_msgs::MowerControlSrvResponse &res) {
     if (req.mow_enabled && !is_emergency()) {
-        speed_mow = req.mow_direction ? 1 : -1;
+        target_speed_mow = req.mow_direction ? 1 : -1;
     } else {
-        speed_mow = 0;
+        target_speed_mow = 0;
     }
-    ROS_INFO_STREAM("Setting mow enabled to " << speed_mow);
+    ROS_INFO_STREAM("Setting mow enabled to " << target_speed_mow);
     return true;
 }
 
@@ -469,7 +472,7 @@ int main(int argc, char **argv) {
     ROS_INFO_STREAM("Wheel ticks [1/m]: " << wheel_ticks_per_m);
     ROS_INFO_STREAM("Wheel distance [m]: " << wheel_distance_m);
 
-    speed_l = speed_r = speed_mow = 0;
+    speed_l = speed_r = speed_mow = target_speed_mow = 0;
 
     paramNh.getParam("dfp_is_5v", dfp_is_5v);
     paramNh.getParam("language", language);

src/mower_logic/src/mower_logic/mower_logic.cpp

@@ -79,7 +79,7 @@ std::recursive_mutex mower_logic_mutex;
 
 mower_msgs::HighLevelStatus high_level_status;
 
-std::atomic<bool> mowerEnabled;
+std::atomic<bool> mowerAllowed;
 
 Behavior *currentBehavior = &IdleBehavior::INSTANCE;
 
@@ -246,7 +246,8 @@ bool setMowerEnabled(bool enabled) {
     }
 
     // status change ?
-    if (mowerEnabled != enabled) {
+    if (last_status.mow_enabled != enabled)
+    {
         ros::Time started = ros::Time::now();
         mower_msgs::MowerControlSrv mow_srv;
         mow_srv.request.mow_enabled = enabled;
@@ -277,7 +278,6 @@ bool setMowerEnabled(bool enabled) {
                                                                 << static_cast<unsigned>(mow_srv.request.mow_direction)
                                                                 << ") call completed within "
                                                                 << (ros::Time::now() - started).toSec() << "s");
-        mowerEnabled = enabled;
     }
 
 // TODO: Spinup feedback & delay
@@ -318,11 +318,11 @@ void stopMoving() {
 }
 
 /// @brief If the BLADE motor is currently enabled, we stop it
-void stopBlade() {
+void stopBlade()
+{
     // ROS_INFO_STREAM("om_mower_logic: stopBlade() - stopping blade motor if running");
-    if (mowerEnabled) {
-        setMowerEnabled(false);
-    }
+    setMowerEnabled(false);
+    mowerAllowed = false;
     // ROS_INFO_STREAM("om_mower_logic: stopBlade() - finished");
 }
 
@@ -417,12 +417,12 @@ void checkSafety(const ros::TimerEvent &timer_event) {
     const auto status_time = getStatusTime();
     const auto last_good_gps = getLastGoodGPS();
 
-    // call the mower
-    setMowerEnabled(currentBehavior != nullptr && currentBehavior->mower_enabled());
-
     high_level_status.emergency = last_status.emergency;
     high_level_status.is_charging = last_status.v_charge > 10.0;
 
+    // Initialize to true, if after all checks it is still true then mower should be enabled.
+    mowerAllowed = true;
+
     // send to idle if emergency and we're not recording
     if (currentBehavior != nullptr) {
         if (last_status.emergency) {
@@ -495,14 +495,18 @@ void checkSafety(const ros::TimerEvent &timer_event) {
     }
 
     if (currentBehavior != nullptr && currentBehavior->needs_gps()) {
+        currentBehavior->setGoodGPS(!gpsTimeout);
         // Stop the mower
         if (gpsTimeout) {
             stopBlade();
             stopMoving();
+            return;
         }
-        currentBehavior->setGoodGPS(!gpsTimeout);
     }
 
+    // enable the mower (if not aleady) if mowerAllowed is still true after checks and bahavior agrees
+    setMowerEnabled(currentBehavior != nullptr && mowerAllowed && currentBehavior->mower_enabled());
+
     double battery_percent = (last_status.v_battery - last_config.battery_empty_voltage) /
                              (last_config.battery_full_voltage - last_config.battery_empty_voltage);
     if (battery_percent > 1.0) {
@@ -643,7 +647,7 @@ int main(int argc, char **argv) {
 
     n = new ros::NodeHandle();
     paramNh = new ros::NodeHandle("~");
-    mowerEnabled = false;
+    mowerAllowed = false;
 
     boost::recursive_mutex mutex;
 

src/mower_msgs/msg/Status.msg

@@ -19,6 +19,7 @@ bool emergency
 float32 v_charge
 float32 v_battery
 float32 charge_current
+bool mow_enabled
 
 # ESC stuff
 ESCStatus left_esc_status

src/mower_simulation/src/mower_simulation.cpp

@@ -104,6 +104,7 @@ void publishStatus(const ros::TimerEvent &timer_event) {
     }
 
     fake_mow_status.stamp = ros::Time::now();
+    fake_mow_status.mow_enabled = config.mower_running;
     fake_mow_status.mow_esc_status.temperature_motor = config.temperature_mower;
     fake_mow_status.mow_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_OK;
     if (config.mower_error) {