Multiple improvements from testing survey_manager (#156)

* improvements on feedback

* make task time more reasonable + improve output

* adding berth positions for granite table

* updating submodule

* reverting pano distance

astrobee/behaviors/inspection/include/inspection/inspection.h

@@ -105,6 +105,9 @@ class Inspection {
   bool RedoInspectionPose();
   geometry_msgs::PoseArray GetInspectionPoses();
 
+  int GetCurrentCounter() {return inspection_counter_ + 1;}
+  int GetSurveySize() {return points_.size();}
+
   // Get distance from camera to target
   double GetDistanceToTarget();
 

astrobee/behaviors/inspection/src/inspection_node.cc

@@ -549,7 +549,14 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
       sci_cam_req_ = 0;
 
       // If the action was cancelled stop taking more pictures
-      if (fsm_.GetState() == STATE::WAITING) return;
+      if (fsm_.GetState() == STATE::WAITING) {
+        // If we stopped inspection while the flashlight was on, turn it off
+        if (flashlight_intensity_current_ != 0) {
+          flashlight_intensity_current_ = 0.0;
+          Flashlight(flashlight_intensity_current_);
+        }
+        return;
+      }
 
       result_.inspection_result.push_back(isaac_msgs::InspectionResult::PIC_ACQUIRED);
       result_.picture_time.push_back(msg->header.stamp);
@@ -844,7 +851,11 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     case STATE::INIT_INSPECTION:
       msg.fsm_state = "INIT_INSPECTION";                   break;
     case STATE::MOVING_TO_APPROACH_POSE:
-      msg.fsm_state = "MOVING_TO_APPROACH_POSE";           break;
+      msg.fsm_state = "MOVING_TO_APPROACH_POSE ";
+      msg.fsm_state += std::to_string(inspection_->GetCurrentCounter());
+      msg.fsm_state += "/";
+      msg.fsm_state += std::to_string(inspection_->GetSurveySize());
+                                                           break;
     case STATE::VISUAL_INSPECTION:
       msg.fsm_state = "VISUAL_INSPECTION";                 break;
     case STATE::RETURN_INSPECTION:

astrobee/survey/survey_manager/data/granite_survey_static.yaml

@@ -45,6 +45,8 @@ bays_move:
   gra_bay5: ["-pos", "0.1 0.1 -0.68", "-att", "3.14 1 0 0"]
   gra_bay6: ["-pos", "0.1 -0.3 -0.68", "-att", "3.14 1 0 0"]
   gra_bay7: ["-pos", "0.1 -0.5 -0.68", "-att", "3.14 1 0 0"]
+  berth1: ["-pos", "0.1 0.3 -0.68"]
+  berth2: ["-pos", "0.1 -0.3 -0.68", "-att", "3.14 1 0 0"]
 
 
 bays_pano:

astrobee/survey/survey_manager/pddl/domain_survey.pddl

@@ -107,7 +107,7 @@
 
     (:durative-action dock
         :parameters (?robot - robot ?from ?to - location)  ;; from bay7 to berth1 or berth2
-        :duration (= ?duration 30)
+        :duration (= ?duration 90)
         :condition
             (and
                 ;; Check robot mutex

astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py

@@ -318,7 +318,10 @@ def thread_read_input(self, process):
             loginfo(f"reader exiting on exception: {e}")
 
     def send_command(self, command):
-        loginfo(f"send_command: {command}")
+        formatted_command = " ".join(
+            f'"{arg}"' if " " in arg else arg for arg in command
+        )
+        loginfo(f"send_command: {formatted_command}")
         return_code = 1
 
         try:
@@ -385,8 +388,6 @@ def send_command(self, command):
             return return_code
 
     def send_command_recursive(self, command):
-        loginfo(f"Sending recursive command: {command}")
-
         exit_code = self.send_command(command)
         loginfo("send_command exit code " + str(exit_code))
 
@@ -689,6 +690,12 @@ def survey_manager_executor(args, run, config_static, process_executor, quick: b
         exit_code = sm_exec.move(args["from_name"], args["to_name"])
 
     elif args["type"] == "panorama":
+        # In some cases if the robot is not at the module position
+        # it might not perform a localization maneuver
+        exit_code = first_non_zero(
+            exit_code, sm_exec.move(args["location_name"], args["location_name"])
+        )
+
         exit_code = first_non_zero(
             exit_code,
             command_executor.start_recording(