From 759fddaab866f18dc2689a869206c3b92a2d5431 Mon Sep 17 00:00:00 2001 From: Jeremy Salwen Date: Sun, 11 Aug 2024 05:11:58 +0000 Subject: [PATCH] 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. --- .../mower_logic/behaviors/DockingBehavior.cpp | 99 +++++++++++++++++-- .../mower_logic/behaviors/DockingBehavior.h | 5 + .../behaviors/UndockingBehavior.cpp | 56 ++++++++++- .../mower_logic/behaviors/UndockingBehavior.h | 5 + 4 files changed, 157 insertions(+), 8 deletions(-) diff --git a/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp index 8fd745f1..de71551a 100644 --- 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 &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 index 36795683..b82cd4d3 100644 --- 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 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 index a061d94e..eb0c2ec4 100644 --- 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 &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 index 54bdd432..f51aeb13 100644 --- 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,6 +39,8 @@ class UndockingBehavior : public Behavior { UndockingBehavior(Behavior* nextBehavior); private: + std::vector actions; + Behavior* nextBehavior; geometry_msgs::PoseStamped docking_pose_stamped; bool gpsRequired; @@ -45,6 +48,8 @@ class UndockingBehavior : public Behavior { bool waitForGPS(); public: + UndockingBehavior(); + std::string state_name() override; Behavior* execute() override;