From 5f30c7f1c32eccb9292953240645e7b7d476111d Mon Sep 17 00:00:00 2001 From: Dan Riley Date: Sun, 9 Feb 2020 17:04:08 -0700 Subject: [PATCH] Fixed stopping to use estop again --- src/multi_agent.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/multi_agent.py b/src/multi_agent.py index 319b1db..f35bf43 100755 --- a/src/multi_agent.py +++ b/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()