Improve action result (#163)

* passing on sub action results for cargo and inspection beh

* refining output, fixing bug, lab tested

* adding cargo feedback

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);
     }
   }
@@ -835,6 +837,8 @@ class CargoNode : public ff_util::FreeFlyerNodelet {
   isaac_msgs::CargoGoal goal_;
   std::string cargo_id_;
   isaac_msgs::CargoResult result_;
+  std::string err_msg_;
+
   // Flag to wait for sci camera
   bool sci_cam_req_ = false;
   bool ground_active_ = false;

astrobee/behaviors/inspection/src/inspection_node.cc

@@ -146,7 +146,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
       });
     fsm_.Add(STATE::MOVING_TO_APPROACH_POSE,
       DOCK_FAILED, [this](FSM::Event const& event) -> FSM::State {
-        Result(RESPONSE::DOCK_FAILED);
+        Result(RESPONSE::DOCK_FAILED, err_msg_);
         return STATE::WAITING;
       });
     // [3]
@@ -165,7 +165,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
       [this](FSM::State const& state, FSM::Event const& event) -> FSM::State {
         switch (event) {
         case MOTION_FAILED:
-          Result(RESPONSE::MOTION_APPROACH_FAILED);
+          Result(RESPONSE::MOTION_APPROACH_FAILED, err_msg_);
           break;
         case INSPECT_FAILED:
           Result(RESPONSE::VISUAL_INSPECTION_FAILED);
@@ -414,6 +414,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     }
     ROS_DEBUG_STREAM("Motion failed result error: " << result->response);
 
+    err_msg_ = "Move Code" + std::to_string(result->response) + ": (" + result->fsm_result + ")";
     return fsm_.Update(MOTION_FAILED);
   }
 
@@ -450,6 +451,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     case ff_util::FreeFlyerActionState::SUCCESS:
       return fsm_.Update(DOCK_SUCCESS);
     default:
+      err_msg_ = "Dock Code " + std::to_string(result->response) + ": (" + result->fsm_result + ")";
       return fsm_.Update(DOCK_FAILED);
     }
   }
@@ -606,11 +608,15 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
   }
   void SciCamTimeout(const ros::TimerEvent& event) {
     sci_cam_timeout_.stop();
+    // If the action was cancelled stop taking more pictures
+    if (fsm_.GetState() == STATE::WAITING) return;
+
     // The sci cam image was not received
     if (sci_cam_req_ < cfg_.Get<int>("sci_cam_max_trials")) {
       ROS_WARN_STREAM("Scicam didn't repond, resending it again");
       // Send the command
       SendPicture(focus_distance_current_);
+      ++sci_cam_req_;
       return;
     } else {
       return fsm_.Update(INSPECT_FAILED);
@@ -926,6 +932,8 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
   std::string i_fsm_substate_;
   isaac_msgs::InspectionResult result_;
   int motion_retry_number_= 0;
+  std::string err_msg_;
+
   // Flag to wait for sci camera
   int sci_cam_req_ = 0;
   bool ground_active_ = false;

astrobee/behaviors/inspection/tools/inspection_tool.cc

@@ -244,7 +244,7 @@ void ResultCallback(ff_util::FreeFlyerActionState::Enum code,
   // Check that result var is valid
   if (result != nullptr) {
     // If we get there then we have some response data
-    s += result->fsm_result + " (Code " + std::to_string(result->response) + ")";
+    s += result->fsm_result + " (Inspection Code " + std::to_string(result->response) + ")";
   }
   // Limit line to the maximum amout of characters
   if (s.size() < 71) s.append(71 - s.size(), ' ');

isaac/config/behaviors/inspection.config

@@ -353,7 +353,7 @@ parameters = {
     id = "sci_cam_max_trials",
     reconfigurable = true,
     type = "integer",
-    default = 3,
+    default = 10,
     min = 0,
     max = 10,
     unit = "",