Refactor to create waitForResultOrAborted

ClemensElflein · Aug 13, 2024 · e4a0492 · e4a0492
1 parent 09218b1
commit e4a0492
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 68 deletions.
diff --git a/src/mower_logic/src/mower_logic/behaviors/Behavior.h b/src/mower_logic/src/mower_logic/behaviors/Behavior.h
@@ -18,6 +18,8 @@
 #ifndef SRC_BEHAVIOR_H
 #define SRC_BEHAVIOR_H
 
+#include <actionlib/client/simple_action_client.h>
+
 #include <atomic>
 #include <memory>
 
@@ -86,7 +88,7 @@ class Behavior {
     requested_pause_flag |= reason;
   }
 
-  void start(mower_logic::MowerLogicConfig &c, std::shared_ptr<sSharedState> s) {
+  void start(mower_logic::MowerLogicConfig& c, std::shared_ptr<sSharedState> s) {
     ROS_INFO_STREAM("");
     ROS_INFO_STREAM("");
     ROS_INFO_STREAM("--------------------------------------");
@@ -103,11 +105,34 @@ class Behavior {
     enter();
   }
 
+  template <typename ActionSpec>
+  actionlib::SimpleClientGoalState waitForResultOrAborted(
+      actionlib::SimpleActionClient<ActionSpec>* client, const typename ActionSpec::_action_goal_type::_goal_type& goal,
+      double poll_rate = 10) {
+    ros::Rate rate(poll_rate);
+    client->sendGoal(goal);
+
+    while (true) {
+      rate.sleep();
+      if (aborted) {
+        client->cancelGoal();
+        return client->getState();
+      }
+
+      switch (client->getState().state_) {
+        case actionlib::SimpleClientGoalState::ACTIVE:
+        case actionlib::SimpleClientGoalState::PENDING: break;
+        default: return client->getState();
+      }
+    }
+    return client->getState();
+  }
+
   /**
    * Execute the behavior. This call should block until the behavior is executed fully.
    * @returns the pointer to the next behavior (can return itself).
    */
-  virtual Behavior *execute() = 0;
+  virtual Behavior* execute() = 0;
 
   /**
    * Called ONCE before state exits

diff --git a/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp
@@ -60,26 +60,9 @@ bool DockingBehavior::approach_docking_point() {
     moveBaseGoal.target_pose = docking_approach_point;
     moveBaseGoal.controller = "FTCPlanner";
 
-    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;
-      }
+    auto result = waitForResultOrAborted(mbfClient, moveBaseGoal);
+    if (aborted || result.state_ != actionlib::SimpleClientGoalState::SUCCEEDED) {
+      return false;
     }
   }
 
@@ -104,25 +87,9 @@ bool DockingBehavior::approach_docking_point() {
     ROS_INFO_STREAM("Executing Docking Approach");
 
     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;
-      }
+    auto approachResult = waitForResultOrAborted(mbfClientExePath, exePathGoal);
+    if (aborted || approachResult != actionlib::SimpleClientGoalState::SUCCEEDED) {
+      return false;
     }
   }
 

diff --git a/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp
@@ -33,13 +33,13 @@ 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";
+  xbot_msgs::ActionInfo abort_undocking_action;
+  abort_undocking_action.action_id = "abort_undocking";
+  abort_undocking_action.enabled = true;
+  abort_undocking_action.action_name = "Stop Undocking";
 
   actions.clear();
-  actions.push_back(abort_docking_action);
+  actions.push_back(abort_undocking_action);
 }
 
 std::string UndockingBehavior::state_name() {
@@ -75,32 +75,17 @@ Behavior *UndockingBehavior::execute() {
   exePathGoal.tolerance_from_action = true;
   exePathGoal.controller = "DockingFTCPlanner";
 
+  auto result = waitForResultOrAborted(mbfClientExePath, 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;
-    }
-
-    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;
-    }
+  if (aborted) {
+    ROS_INFO_STREAM("Undocking aborted.");
+    stopMoving();
+    return &IdleBehavior::INSTANCE;
   }
 
+  bool success = result.state_ == actionlib::SimpleClientGoalState::SUCCEEDED;
+
   // stop the bot for now
   stopMoving();