diff --git a/src/mower_logic/src/mower_logic/behaviors/Behavior.h b/src/mower_logic/src/mower_logic/behaviors/Behavior.h index 05266401..dec2d640 100644 --- 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 + #include #include @@ -86,7 +88,7 @@ class Behavior { requested_pause_flag |= reason; } - void start(mower_logic::MowerLogicConfig &c, std::shared_ptr s) { + void start(mower_logic::MowerLogicConfig& c, std::shared_ptr s) { ROS_INFO_STREAM(""); ROS_INFO_STREAM(""); ROS_INFO_STREAM("--------------------------------------"); @@ -103,11 +105,34 @@ class Behavior { enter(); } + template + actionlib::SimpleClientGoalState waitForResultOrAborted( + actionlib::SimpleActionClient* 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 index 4213781c..2153a4b6 100644 --- 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; } } @@ -103,26 +86,9 @@ bool DockingBehavior::approach_docking_point() { exePathGoal.controller = "FTCPlanner"; 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 index 729fd041..26821315 100644 --- 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,16 @@ Behavior *UndockingBehavior::execute() { exePathGoal.tolerance_from_action = true; exePathGoal.controller = "DockingFTCPlanner"; - mbfClientExePath->sendGoal(exePathGoal); + auto result = waitForResultOrAborted(mbfClientExePath, 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();