adding cargo feedback

nasa · Apr 27, 2024 · 733009c · 733009c
1 parent 2da97fd
commit 733009c
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/astrobee/behaviors/cargo/src/cargo_node.cc b/astrobee/behaviors/cargo/src/cargo_node.cc
@@ -246,10 +246,10 @@ class CargoNode : public ff_util::FreeFlyerNodelet {
       [this](FSM::State const& state, FSM::Event const& event) -> FSM::State {
         switch (event) {
         case MOTION_FAILED:
-          Result(RESPONSE::MOTION_FAILED);
+          Result(RESPONSE::MOTION_FAILED, err_msg_);
           break;
         case ARM_FAILED:
-          Result(RESPONSE::ARM_FAILED);
+          Result(RESPONSE::ARM_FAILED, err_msg_);
           break;
         case DETECT_FAILED:
           Result(RESPONSE::DETECT_FAILED);
@@ -542,6 +542,7 @@ class CargoNode : public ff_util::FreeFlyerNodelet {
       ROS_ERROR_STREAM("Invalid result received");
     }
 
+    err_msg_ = "Move Code" + std::to_string(result->response) + ": (" + result->fsm_result + ")";
     return fsm_.Update(MOTION_FAILED);
   }
 
@@ -565,6 +566,7 @@ class CargoNode : public ff_util::FreeFlyerNodelet {
     case ff_util::FreeFlyerActionState::SUCCESS:
       return fsm_.Update(ARM_SUCCESS);
     default:
+      err_msg_ = "Arm Code " + std::to_string(result->response) + ": (" + result->fsm_result + ")";
       return fsm_.Update(ARM_FAILED);
     }
   }