From 7c1f38e342f742a6dc648d225251d41a73c59d86 Mon Sep 17 00:00:00 2001
From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com>
Date: Mon, 26 Feb 2024 17:46:25 -0800
Subject: [PATCH] making the picture taking stop when the goal is cancelled;
 increasing the inspection too timeout (#150)

---
 .../inspection/src/inspection_node.cc         | 29 +++++++++++++++----
 .../inspection/tools/inspection_tool.cc       |  2 +-
 2 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/astrobee/behaviors/inspection/src/inspection_node.cc b/astrobee/behaviors/inspection/src/inspection_node.cc
index 3882f52b..3bb61c20 100644
--- a/astrobee/behaviors/inspection/src/inspection_node.cc
+++ b/astrobee/behaviors/inspection/src/inspection_node.cc
@@ -522,6 +522,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
       goal.type = isaac_msgs::ImageInspectionGoal::VENT;
       client_i_.SendGoal(goal);
     }
+    finished_anomaly_ = false;
 
     // Allow image to stabilize
     ros::Duration(cfg_.Get<double>("station_time")).sleep();
@@ -531,7 +532,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     flashlight_intensity_current_ = 0.0;
 
     // Signal an imminent sci cam image
-    sci_cam_req_ = sci_cam_req_ + 1;
+    sci_cam_req_ += 1;
 
     // Send the command
     SendPicture(focus_distance_calculated_);
@@ -546,12 +547,16 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
       // Clear local variables
       sci_cam_timeout_.stop();
       sci_cam_req_ = 0;
+
+      // If the action was cancelled stop taking more pictures
+      if (fsm_.GetState() == STATE::WAITING) return;
+
       result_.inspection_result.push_back(isaac_msgs::InspectionResult::PIC_ACQUIRED);
       result_.picture_time.push_back(msg->header.stamp);
       ros::Duration(cfg_.Get<double>("station_time")).sleep();
 
       if (goal_.command == isaac_msgs::InspectionGoal::ANOMALY) {
-        ROS_DEBUG_STREAM("Scicam picture acquired - Timestamp: " << msg->header.stamp
+        ROS_INFO_STREAM("Scicam picture acquired - Timestamp: " << msg->header.stamp
                       << ", Focus distance (m): " << focus_distance_current_
                       << ", Focal distance : " << 1.6 * std::pow(focus_distance_current_, -1.41)
                       << ", Flashlight: " << flashlight_intensity_current_);
@@ -568,9 +573,13 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
           } else if (focus_distance_current_ <
                        cfg_.Get<double>("target_distance") + cfg_.Get<double>("focus_distance_range") - EPS) {
             focus_distance_current_ += cfg_.Get<double>("focus_distance_step");
-          } else {
-            // Finish inspection
+          } else if (!ground_active_ || finished_anomaly_) {
+            // If no anomaly detectiuon active or if anomaly detection finished, go to next inspection
+            finished_anomaly_ = false;
             return fsm_.Update(NEXT_INSPECT);
+          } else {
+            // When the anomaly detection returns a result it will go to the next inspection
+            finished_anomaly_ = true;
           }
         }
         sci_cam_req_ = 1;
@@ -608,7 +617,16 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
       result_.anomaly_result.push_back(result->anomaly_result);
     else
       ROS_INFO_STREAM("Invalid result received Image Analysis");
-    return fsm_.Update(NEXT_INSPECT);
+
+    // If we finished taking pictures, go to next inspection
+    // If not, return and when the pictures are all taken, it go to the next inspection
+    if (finished_anomaly_) {
+      finished_anomaly_ = false;
+      return fsm_.Update(NEXT_INSPECT);
+    } else {
+      finished_anomaly_ = true;
+      return;
+    }
   }
 
   // INSPECTION ACTION SERVER
@@ -886,6 +904,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
   int sci_cam_req_ = 0;
   bool ground_active_ = false;
   bool sim_mode_ = false;
+  bool finished_anomaly_ = false;
 
   // Inspection library
   Inspection* inspection_;
diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc
index da2f208f..0e70e781 100644
--- a/astrobee/behaviors/inspection/tools/inspection_tool.cc
+++ b/astrobee/behaviors/inspection/tools/inspection_tool.cc
@@ -108,7 +108,7 @@ DEFINE_string(volumetric_poses, "/resources/volumetric_iss.txt", "Wifi poses lis
 // Timeout values for action
 DEFINE_double(connect, 10.0, "Action connect timeout");
 DEFINE_double(active, 10.0, "Action active timeout");
-DEFINE_double(response, 200.0, "Action response timeout");
+DEFINE_double(response, 500.0, "Action response timeout");
 DEFINE_double(deadline, -1.0, "Action deadline timeout");
 
 // Match the internal states and responses with the message definition