Allow aborting Docking/Undocking

.devcontainer/devcontainer.json

@@ -9,9 +9,11 @@
         "ms-azuretools.vscode-docker",
         "ms-python.python",
         "ms-vscode.cpptools",
+        "ms-vscode.cpptools-extension-pack",
         "ms-iot.vscode-ros"
       ]
     }
   },
-  "remoteUser": "dev"
+  "remoteUser": "dev",
+  "postCreateCommand": "echo 'source /opt/ros/noetic/setup.bash' >> ~/.bashrc"
 }

devenv/Dockerfile

@@ -9,8 +9,10 @@ RUN groupadd -g ${MY_GID} -o ${MY_USER}
 # Create a new user, assign it to the group, and set its shell to bash
 RUN useradd -m -u ${MY_UID} -g ${MY_GID} -o -s /bin/bash ${MY_USER} && yes ${MY_USER} | passwd ${MY_USER}
 
+RUN echo $MY_USER ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$MY_USER \
+    && chmod 0440 /etc/sudoers.d/$MY_USER
+
 RUN apt-get update && apt-get install -y zsh git sudo ssh gdb rsync
-RUN echo "$MY_USER ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
 
 RUN ( \
     echo 'LogLevel DEBUG2'; \
@@ -22,6 +24,8 @@ RUN ( \
   && mkdir /run/sshd
 RUN bash -c "ssh-keygen -A"
 
+RUN echo "$MY_USER ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
+
 # Update rosdep package manager for ROS
 RUN rosdep update --rosdistro $ROS_DISTRO
 

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,32 @@ class Behavior {
     enter();
   }
 
+  template <typename ActionSpec>
+  actionlib::SimpleClientGoalState sendGoalAndWaitUnlessAborted(
+      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();
+
+      auto state = client->getState();
+      if (aborted) {
+        client->cancelGoal();
+        return state;
+      }
+      if (state.isDone()) {
+        return state;
+      }
+    }
+  }
+
   /**
    * 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

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,8 +59,9 @@ 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) {
+
+    auto result = sendGoalAndWaitUnlessAborted(mbfClient, moveBaseGoal);
+    if (aborted || result.state_ != result.SUCCEEDED) {
       return false;
     }
   }
@@ -73,8 +86,8 @@ bool DockingBehavior::approach_docking_point() {
     exePathGoal.controller = "FTCPlanner";
     ROS_INFO_STREAM("Executing Docking Approach");
 
-    auto approachResult = mbfClientExePath->sendGoalAndWait(exePathGoal);
-    if (approachResult.state_ != approachResult.SUCCEEDED) {
+    auto approachResult = sendGoalAndWaitUnlessAborted(mbfClientExePath, exePathGoal);
+    if (aborted || approachResult.state_ != approachResult.SUCCEEDED) {
       return false;
     }
   }
@@ -179,12 +192,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.");
 
@@ -209,6 +234,12 @@ Behavior *DockingBehavior::execute() {
 
   bool docked = dock_straight();
 
+  if (aborted) {
+    ROS_INFO_STREAM("Docking aborted.");
+    stopMoving();
+    return &IdleBehavior::INSTANCE;
+  }
+
   if (!docked) {
     ROS_ERROR("Error during docking.");
 
@@ -241,9 +272,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 +304,7 @@ void DockingBehavior::command_home() {
 }
 
 void DockingBehavior::command_start() {
+  this->abort();
 }
 
 void DockingBehavior::command_s1() {
@@ -284,4 +325,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();
+  }
 }

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;

src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp

@@ -268,7 +268,7 @@ bool MowingBehavior::execute_mowing_plan() {
       paused = true;
       mowerEnabled = false;
       u_int8_t last_requested_pause_flags = 0;
-      while (requested_pause_flag)  // while emergency and/or manual pause not asked to continue, we wait
+      while (requested_pause_flag && !aborted)  // while emergency and/or manual pause not asked to continue, we wait
       {
         if (last_requested_pause_flags != requested_pause_flag) {
           update_actions();
@@ -293,7 +293,7 @@ bool MowingBehavior::execute_mowing_plan() {
     }
     if (paused) {
       paused_time = ros::Time::now();
-      while (!this->hasGoodGPS())  // while no good GPS we wait
+      while (!this->hasGoodGPS() && !aborted)  // while no good GPS we wait
       {
         ROS_INFO_STREAM("MowingBehavior: PAUSED (" << (ros::Time::now() - paused_time).toSec()
                                                    << "s) (waiting for GPS)");

src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp

@@ -27,9 +27,21 @@ 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_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_undocking_action);
+}
+
 std::string UndockingBehavior::state_name() {
   return "UNDOCKING";
 }
@@ -63,7 +75,13 @@ Behavior *UndockingBehavior::execute() {
   exePathGoal.tolerance_from_action = true;
   exePathGoal.controller = "DockingFTCPlanner";
 
-  auto result = mbfClientExePath->sendGoalAndWait(exePathGoal);
+  auto result = sendGoalAndWaitUnlessAborted(mbfClientExePath, exePathGoal);
+
+  if (aborted) {
+    ROS_INFO_STREAM("Undocking aborted.");
+    stopMoving();
+    return &IdleBehavior::INSTANCE;
+  }
 
   bool success = result.state_ == actionlib::SimpleClientGoalState::SUCCEEDED;
 
@@ -103,9 +121,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 +179,7 @@ UndockingBehavior::UndockingBehavior(Behavior *next) {
 }
 
 void UndockingBehavior::command_home() {
+  this->abort();
 }
 
 void UndockingBehavior::command_start() {
@@ -175,4 +203,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();
+  }
 }

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;