Allow aborting Docking/Undocking

Pressing "Home" when undocking aborts it, and pressing "play" when docking aborts it. Uninterruptable docking can be a real problem if your mower is stuck trying to return to the dock and you want to tele-operate it. This adds button controls, as well as actions which can be used from the GUI.
ClemensElflein · Aug 11, 2024 · 759fdda · 759fdda
1 parent 671c109
commit 759fdda
Show file tree

Hide file tree

Showing 4 changed files with 157 additions and 8 deletions.
diff --git a/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp
@@ -27,8 +27,20 @@ extern mower_msgs::Status getStatus();
 extern void stopMoving();
 extern bool setGPS(bool enabled);
 
+extern void registerActions(std::string prefix, const std::vector<xbot_msgs::ActionInfo> &actions);
+
 DockingBehavior DockingBehavior::INSTANCE;
 
+DockingBehavior::DockingBehavior() {
+  xbot_msgs::ActionInfo abort_docking_action;
+  abort_docking_action.action_id = "abort_docking";
+  abort_docking_action.enabled = true;
+  abort_docking_action.action_name = "Stop Docking";
+
+  actions.clear();
+  actions.push_back(abort_docking_action);
+}
+
 bool DockingBehavior::approach_docking_point() {
   ROS_INFO_STREAM("Calculating approach path");
 
@@ -47,9 +59,31 @@ bool DockingBehavior::approach_docking_point() {
     mbf_msgs::MoveBaseGoal moveBaseGoal;
     moveBaseGoal.target_pose = docking_approach_point;
     moveBaseGoal.controller = "FTCPlanner";
-    auto result = mbfClient->sendGoalAndWait(moveBaseGoal);
-    if (result.state_ != result.SUCCEEDED) {
-      return false;
+
+    mbfClient->sendGoal(moveBaseGoal);
+
+    ros::Rate r(10);
+    bool waitingForResult = true;
+
+    while (waitingForResult) {
+      r.sleep();
+      if (aborted) {
+        ROS_INFO_STREAM("Docking aborted.");
+        mbfClientExePath->cancelGoal();
+        stopMoving();
+        return false;
+      }
+
+      switch (mbfClientExePath->getState().state_) {
+        case actionlib::SimpleClientGoalState::ACTIVE:
+        case actionlib::SimpleClientGoalState::PENDING:
+          break;
+        case actionlib::SimpleClientGoalState::SUCCEEDED:
+          waitingForResult = false;
+          break;
+        default:
+          return false;
+      }
     }
   }
 
@@ -73,9 +107,30 @@ bool DockingBehavior::approach_docking_point() {
     exePathGoal.controller = "FTCPlanner";
     ROS_INFO_STREAM("Executing Docking Approach");
 
-    auto approachResult = mbfClientExePath->sendGoalAndWait(exePathGoal);
-    if (approachResult.state_ != approachResult.SUCCEEDED) {
-      return false;
+    mbfClientExePath->sendGoal(exePathGoal);
+
+    ros::Rate r(10);
+    bool waitingForResult = true;
+
+    while (waitingForResult) {
+      r.sleep();
+      if (aborted) {
+        ROS_INFO_STREAM("Docking aborted.");
+        mbfClientExePath->cancelGoal();
+        stopMoving();
+        return false;
+      }
+
+      switch (mbfClientExePath->getState().state_) {
+        case actionlib::SimpleClientGoalState::ACTIVE:
+        case actionlib::SimpleClientGoalState::PENDING:
+          break;
+        case actionlib::SimpleClientGoalState::SUCCEEDED:
+          waitingForResult = false;
+          break;
+        default:
+          return false;
+      }
     }
   }
 
@@ -179,12 +234,24 @@ Behavior *DockingBehavior::execute() {
   }
 
   while (!isGPSGood) {
+    if (aborted) {
+      ROS_INFO_STREAM("Docking aborted.");
+      stopMoving();
+      return &IdleBehavior::INSTANCE;
+    }
+
     ROS_WARN_STREAM("Waiting for good GPS");
     ros::Duration(1.0).sleep();
   }
 
   bool approachSuccess = approach_docking_point();
 
+  if (aborted) {
+    ROS_INFO_STREAM("Docking aborted.");
+    stopMoving();
+    return &IdleBehavior::INSTANCE;
+  }
+
   if (!approachSuccess) {
     ROS_ERROR("Error during docking approach.");
 
@@ -208,6 +275,12 @@ Behavior *DockingBehavior::execute() {
   if (PerimeterSearchBehavior::configured(config)) return &PerimeterSearchBehavior::INSTANCE;
 
   bool docked = dock_straight();
+
+  if (aborted) {
+    ROS_INFO_STREAM("Docking aborted.");
+    stopMoving();
+    return &IdleBehavior::INSTANCE;
+  }
 
   if (!docked) {
     ROS_ERROR("Error during docking.");
@@ -241,9 +314,18 @@ void DockingBehavior::enter() {
   docking_pose_stamped.pose = get_docking_point_srv.response.docking_pose;
   docking_pose_stamped.header.frame_id = "map";
   docking_pose_stamped.header.stamp = ros::Time::now();
+
+  for (auto &a : actions) {
+    a.enabled = true;
+  }
+  registerActions("mower_logic:docking", actions);
 }
 
 void DockingBehavior::exit() {
+  for (auto &a : actions) {
+    a.enabled = false;
+  }
+  registerActions("mower_logic:docking", actions);
 }
 
 void DockingBehavior::reset() {
@@ -264,6 +346,7 @@ void DockingBehavior::command_home() {
 }
 
 void DockingBehavior::command_start() {
+  this->abort();
 }
 
 void DockingBehavior::command_s1() {
@@ -284,4 +367,8 @@ uint8_t DockingBehavior::get_state() {
 }
 
 void DockingBehavior::handle_action(std::string action) {
+  if (action == "mower_logic:docking/abort_docking") {
+    ROS_INFO_STREAM("Got abort docking command");
+    command_start();
+  } 
 }
diff --git a/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.h b/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.h
@@ -31,12 +31,15 @@
 #include "mower_msgs/Status.h"
 #include "ros/ros.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "xbot_msgs/ActionInfo.h"
 
 class DockingBehavior : public Behavior {
  public:
   static DockingBehavior INSTANCE;
 
  private:
+  std::vector<xbot_msgs::ActionInfo> actions;
+
   uint retryCount;
   bool inApproachMode;
   geometry_msgs::PoseStamped docking_pose_stamped;
@@ -46,6 +49,8 @@ class DockingBehavior : public Behavior {
   bool dock_straight();
 
  public:
+  DockingBehavior();
+
   std::string state_name() override;
 
   Behavior *execute() override;

diff --git a/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp
@@ -27,9 +27,22 @@ extern void stopMoving();
 extern bool isGpsGood();
 extern bool setGPS(bool enabled);
 
+extern void registerActions(std::string prefix, const std::vector<xbot_msgs::ActionInfo> &actions);
+
 UndockingBehavior UndockingBehavior::INSTANCE(&MowingBehavior::INSTANCE);
 UndockingBehavior UndockingBehavior::RETRY_INSTANCE(&DockingBehavior::INSTANCE);
 
+UndockingBehavior::UndockingBehavior() {
+  xbot_msgs::ActionInfo abort_docking_action;
+  abort_docking_action.action_id = "abort_undocking";
+  abort_docking_action.enabled = true;
+  abort_docking_action.action_name = "Stop Undocking";
+
+  actions.clear();
+  actions.push_back(abort_docking_action);
+}
+
+
 std::string UndockingBehavior::state_name() {
   return "UNDOCKING";
 }
@@ -63,9 +76,34 @@ Behavior *UndockingBehavior::execute() {
   exePathGoal.tolerance_from_action = true;
   exePathGoal.controller = "DockingFTCPlanner";
 
-  auto result = mbfClientExePath->sendGoalAndWait(exePathGoal);
+  mbfClientExePath->sendGoal(exePathGoal);
+
+  ros::Rate r(10);
+  bool waitingForResult = true;
+  bool success = false;
+
+  while (waitingForResult) {
+    r.sleep();
+    if (aborted) {
+      ROS_INFO_STREAM("Undocking aborted.");
+      mbfClientExePath->cancelGoal();
+      stopMoving();
+      return &IdleBehavior::INSTANCE;
+    }
 
-  bool success = result.state_ == actionlib::SimpleClientGoalState::SUCCEEDED;
+    switch (mbfClientExePath->getState().state_) {
+      case actionlib::SimpleClientGoalState::ACTIVE:
+      case actionlib::SimpleClientGoalState::PENDING:
+        break;
+      case actionlib::SimpleClientGoalState::SUCCEEDED:
+        waitingForResult = false;
+        success = true;
+        break;
+      default:
+        waitingForResult = false;
+        break;
+    }
+  }
 
   // stop the bot for now
   stopMoving();
@@ -103,9 +141,18 @@ void UndockingBehavior::enter() {
     ROS_INFO_STREAM("Currently inside the docking station, we set the robot's pose to the docks pose.");
     setRobotPose(docking_pose_stamped.pose);
   }
+
+  for (auto &a : actions) {
+    a.enabled = true;
+  }
+  registerActions("mower_logic:undocking", actions);
 }
 
 void UndockingBehavior::exit() {
+  for (auto &a : actions) {
+    a.enabled = false;
+  }
+  registerActions("mower_logic:undocking", actions);
 }
 
 void UndockingBehavior::reset() {
@@ -152,6 +199,7 @@ UndockingBehavior::UndockingBehavior(Behavior *next) {
 }
 
 void UndockingBehavior::command_home() {
+  this->abort();
 }
 
 void UndockingBehavior::command_start() {
@@ -175,4 +223,8 @@ uint8_t UndockingBehavior::get_state() {
 }
 
 void UndockingBehavior::handle_action(std::string action) {
+  if (action == "mower_logic:undocking/abort_undocking") {
+    ROS_INFO_STREAM("Got abort undocking command");
+    command_home();
+  } 
 }
diff --git a/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.h b/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.h
@@ -29,6 +29,7 @@
 #include "ros/ros.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 #include "xbot_msgs/AbsolutePose.h"
+#include "xbot_msgs/ActionInfo.h"
 
 class UndockingBehavior : public Behavior {
  public:
@@ -38,13 +39,17 @@ class UndockingBehavior : public Behavior {
   UndockingBehavior(Behavior* nextBehavior);
 
  private:
+  std::vector<xbot_msgs::ActionInfo> actions;
+
   Behavior* nextBehavior;
   geometry_msgs::PoseStamped docking_pose_stamped;
   bool gpsRequired;
 
   bool waitForGPS();
 
  public:
+  UndockingBehavior();
+
   std::string state_name() override;
 
   Behavior* execute() override;