Fixed stopping to use estop again

src/multi_agent.py

@@ -838,10 +838,13 @@ def deployBeacon(self, inplace, dropReason):
 
         if deploy:
             # Stop the robot and publish message to deployment mechanism
-            # self.stop_pub.publish(True)
             self.stop()
             pose = self.agent.odometry.pose.pose
 
+            # TODO doesn't currently work since the node is paused!
+            self.agent.status = 'Deploy'
+            self.task_pub.publish(self.agent.status)
+
             if self.useSimComms:
                 # Either need to identify location before comm loss, or make this a guidance
                 # command to return to a point in range
@@ -878,7 +881,7 @@ def deployBeacon(self, inplace, dropReason):
                     rospy.sleep(10)
 
                 # Resume the mission
-                # self.stop_pub.publish(False)
+                self.stop_pub.publish(False)
                 self.deconflictExplore()
                 self.deploy_pub.publish(False)
 
@@ -1068,7 +1071,9 @@ def stop(self):
         # Stop the robot by publishing no path, but don't change the displayed goal
         self.agent.status = 'Stop'
         self.task_pub.publish(self.agent.status)
-        if self.stopStart:
+        self.stop_pub.publish(True)
+
+        if self.stopStart and self.useSimComms:
             print(self.id, "stopping")
             path = Path()
             path.header.frame_id = 'world'
@@ -1161,6 +1166,9 @@ def start(self):
                             self.deployBeacon(True, 'GUI Command')
                             checkBeacon = False
                             self.mode = 'Explore'
+
+                        # Disable the estop.  'Stop' will re-enable it
+                        self.stop_pub.publish(False)
                     else:
                         self.publishGUITask()