Isaac13 qol fixes + release 0.2.8 (#160)

* don't update focus if not needed + reset focus start panorama

* make monitor exit cleanly

* force stop if something happens to retry command; declutter output

* update submodule

* making berth names unique such that pos is not overwritten

* make move output not flood the entire output

* adding another \r

* upping fsw version to only have one PR

* reset focus for geometry too

RELEASE.md

@@ -1,5 +1,10 @@
 # Releases
 
+## Release 0.2.8
+
+  * Improvements to survey_manager output
+  * Bug fixes
+
 ## Release 0.2.7
 
   * Survey manager

astrobee/behaviors/inspection/src/inspection_node.cc

@@ -482,9 +482,9 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     pub_guest_sci_.publish(cmd);
   }
 
-  void SendPicture(double focus_distance) {
-    ROS_DEBUG_STREAM("Send picture");
-    // Take picture
+
+  void SendSciCamCommand(std::string command) {
+    // Chnage focus
     ff_msgs::CommandArg arg;
     std::vector<ff_msgs::CommandArg> cmd_args;
 
@@ -496,7 +496,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     cmd_args.push_back(arg);
 
     arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING;
-    arg.s = "{\"name\": \"takePicture\", \"haz_dist\": " + std::to_string(focus_distance) +"}";
+    arg.s = command;
     cmd_args.push_back(arg);
 
     ff_msgs::CommandStamped cmd;
@@ -508,7 +508,12 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     cmd.args = cmd_args;
 
     pub_guest_sci_.publish(cmd);
+  }
+
 
+  void SendPicture(double focus_distance) {
+    ROS_DEBUG_STREAM("Send picture");
+    SendSciCamCommand("{\"name\": \"takePicture\", \"haz_dist\": " + std::to_string(focus_distance) +"}");
     // Timer for the sci cam camera
     sci_cam_timeout_.start();
   }
@@ -571,6 +576,8 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
         if (flashlight_intensity_current_ != cfg_.Get<double>("toggle_flashlight")) {
           flashlight_intensity_current_ = cfg_.Get<double>("toggle_flashlight");
           Flashlight(flashlight_intensity_current_);
+          sci_cam_req_ = 1;
+          SendPicture(-1);
         } else {
           // Move on in focus distance iteration
           flashlight_intensity_current_ = 0.0;
@@ -588,9 +595,9 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
             // When the anomaly detection returns a result it will go to the next inspection
             finished_anomaly_ = true;
           }
+          sci_cam_req_ = 1;
+          SendPicture(focus_distance_current_);
         }
-        sci_cam_req_ = 1;
-        SendPicture(focus_distance_current_);
       } else {
         return fsm_.Update(NEXT_INSPECT);
       }
@@ -748,14 +755,22 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     // Geometry command
     case isaac_msgs::InspectionGoal::GEOMETRY:
       NODELET_DEBUG("Received Goal Geometry");
-      if (inspection_->GenerateGeometrySurvey(goal_.inspect_poses))
+      if (inspection_->GenerateGeometrySurvey(goal_.inspect_poses)) {
+        // Reset the focus distance
+        SendSciCamCommand("{\"name\": \"setFocusDistance\", \"distance\": " +
+                          std::to_string(cfg_.Get<double>("sci_cam_startup_focus")) + "}");
         return fsm_.Update(GOAL_INSPECT);
+      }
       break;
     // Panorama command
     case isaac_msgs::InspectionGoal::PANORAMA:
       NODELET_DEBUG("Received Goal Panorama");
-      if (inspection_->GeneratePanoramaSurvey(goal_.inspect_poses))
+      if (inspection_->GeneratePanoramaSurvey(goal_.inspect_poses)) {
+        // Reset the focus distance
+        SendSciCamCommand("{\"name\": \"setFocusDistance\", \"distance\": " +
+                          std::to_string(cfg_.Get<double>("sci_cam_startup_focus")) + "}");
         return fsm_.Update(GOAL_INSPECT);
+      }
       break;
     // Volumetric command
     case isaac_msgs::InspectionGoal::VOLUMETRIC:

astrobee/survey/survey_manager/data/granite_survey_static.yaml

@@ -45,14 +45,18 @@ 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"]
+  berth1_g: ["-pos", "0.1 0.3 -0.68"]
+  berth2_g: ["-pos", "0.1 -0.3 -0.68", "-att", "3.14 1 0 0"]
 
 
 bays_pano:
   gra_bay2: "panorama_granite_bsharp.txt"
   gra_bay6: "panorama_granite_wannabee.txt"
 
+berth:
+  berth1_g: "1"
+  berth2_g: "2"
+
 bogus_bays: [gra_bay0, gra_bay8]
 berths: [berth1, berth2]
 robots: [bsharp, wannabee]

astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml

@@ -31,7 +31,7 @@ goals:
 - {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3}
 # This is one of the goals we previously had to comment out for POPF to return a halfway decent
 # plan. Adding a let_other_robot_reach goal mostly fixed the problem.
-- {type: robot_at, robot: bumble, location: berth1}
+- {type: robot_at, robot: bumble, location: berth1_g}
 
 # This let_other_robot_reach goal is effectively a very specific kind of between-robot ordering
 # constraint. It tells honey to let bumble get to bay 5 before taking its first panorama. Without
@@ -45,10 +45,10 @@ goals:
 - {type: panorama, robot: honey, order: 3, location: jem_bay5}
 # This is the other objective we previously had to comment out for POPF to return a decent plan.
 - {type: stereo, robot: honey, order: 4, trajectory: jem_bay7_to_bay4}
-- {type: robot_at, robot: honey, location: berth2}
+- {type: robot_at, robot: honey, location: berth2_g}
 
 init:
   bumble:
-    location: berth1
+    location: berth1_g
   honey:
-    location: berth2
+    location: berth2_g

astrobee/survey/survey_manager/pddl/problem_granite_survey.ps2.pddl

@@ -7,16 +7,16 @@ set instance gra_bay5 location
 set instance gra_bay6 location
 set instance gra_bay7 location
 set instance gra_bay8 location
-set instance berth1 location
-set instance berth2 location
+set instance berth1_g location
+set instance berth2_g location
 set instance bsharp robot
 set instance wannabee robot
 set instance o0 order
 set instance o1 order
 set instance o2 order
 set instance o3 order
 set instance o4 order
-set goal (and (completed-panorama bsharp o0 gra_bay2) (completed-stereo bsharp o1 gra_bay1 gra_bay3) (robot-at bsharp berth1) (completed-panorama wannabee o1 gra_bay6) (completed-stereo wannabee o4 gra_bay5 gra_bay7) (robot-at wannabee berth2))
+set goal (and (completed-panorama bsharp o0 gra_bay2) (completed-stereo bsharp o1 gra_bay1 gra_bay3) (robot-at bsharp berth1_g) (completed-panorama wannabee o1 gra_bay6) (completed-stereo wannabee o4 gra_bay5 gra_bay7) (robot-at wannabee berth2_g))
 set predicate (move-connected gra_bay0 gra_bay1)
 set predicate (move-connected gra_bay1 gra_bay0)
 set predicate (move-connected gra_bay1 gra_bay2)
@@ -40,8 +40,8 @@ set predicate (location-real gra_bay4)
 set predicate (location-real gra_bay5)
 set predicate (location-real gra_bay6)
 set predicate (location-real gra_bay7)
-set predicate (dock-connected gra_bay3 berth1)
-set predicate (dock-connected gra_bay5 berth2)
+set predicate (dock-connected gra_bay3 berth1_g)
+set predicate (dock-connected gra_bay5 berth2_g)
 set predicate (robots-different bsharp wannabee)
 set predicate (robots-different wannabee bsharp)
 set predicate (locations-different gra_bay0 gra_bay1)
@@ -118,8 +118,8 @@ set predicate (locations-different gra_bay8 gra_bay6)
 set predicate (locations-different gra_bay8 gra_bay7)
 set predicate (robot-available bsharp)
 set predicate (robot-available wannabee)
-set predicate (robot-at bsharp berth1)
-set predicate (robot-at wannabee berth2)
+set predicate (robot-at bsharp berth1_g)
+set predicate (robot-at wannabee berth2_g)
 set predicate (location-available gra_bay0)
 set predicate (location-available gra_bay1)
 set predicate (location-available gra_bay2)

astrobee/survey/survey_manager/src/isaac_action_node.cpp

@@ -207,7 +207,8 @@ void IsaacAction::do_work() {
   progress_ = (ros::Time::now() - start_time_).toSec() / action_duration_;
   send_feedback(progress_, command_ + " running");
 
-  printf("\t ** %s [%5.1f%%]  \n", command_.c_str(), progress_ * 100.0);
+  // Status gets printed on terminal
+  // printf("\t ** %s [%5.1f%%]  \n", command_.c_str(), progress_ * 100.0);=
   int status;
   int result = waitpid(-1, &status, WNOHANG);
   // printf("Result: %d %d %d\n", result, pid_, status);

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

@@ -183,26 +183,27 @@ def __del__(self):
         self.sock_output.close()
 
     def write_output_once(self, output):
-        while not self.sock_output_connected:
+        while not self._stop_event.is_set():
             try:
-                # If socket is not connected try to connect
-                self.sock_output_conn, addr = self.sock_output.accept()
-                self.sock_output_conn.setblocking(False)
+                if not self.sock_output_connected:
+                    # If socket is not connected try to connect
+                    self.sock_output_conn, addr = self.sock_output.accept()
+                    self.sock_output_conn.setblocking(False)
 
-                self.sock_output_connected = True
+                    self.sock_output_connected = True
             except socket.timeout:
                 continue
 
-        try:
-            if self.sock_output_connected:
-                encoded_message = output.encode("ascii", errors="replace")
-                for i in range(0, len(encoded_message), CHUNK_SIZE):
-                    chunk = encoded_message[i : i + CHUNK_SIZE]
-                    self.sock_output_conn.sendall(chunk)
-
-        except (socket.error, BrokenPipeError):
-            loginfo("Error sending data. Receiver may have disconnected.")
-            self.sock_output_connected = False
+            try:
+                if self.sock_output_connected:
+                    encoded_message = output.encode("ascii", errors="replace")
+                    for i in range(0, len(encoded_message), CHUNK_SIZE):
+                        chunk = encoded_message[i : i + CHUNK_SIZE]
+                        self.sock_output_conn.sendall(chunk)
+                break
+            except (socket.error, BrokenPipeError):
+                loginfo("Error sending data. Receiver may have disconnected.")
+                self.sock_output_connected = False
 
     def thread_write_output(self, process):
         # loginfo("starting thread_write_output...")
@@ -218,13 +219,13 @@ def thread_write_output(self, process):
                 if output == "":
                     continue
                 if output and not output.startswith("pos: x:"):
-                    loginfo(f"writer received: {output}")
                     output_total += output
 
                 try:
                     # If socket is not connected try to connect
                     if not self.sock_output_connected:
-                        # loginfo("trying to connect")
+                        if not output.startswith("pos: x:"):
+                            loginfo(f"writer received: {output}")
                         self.sock_output_conn, addr = self.sock_output.accept()
                         self.sock_output_conn.setblocking(False)
 
@@ -258,9 +259,7 @@ def read_input_once(self) -> str:
             # loginfo("waiting for connection")
             try:
                 self.sock_input_conn, addr = self.sock_input.accept()
-                self.sock_input_conn.settimeout(
-                    1
-                )  # Set a timeout for socket operations
+                self.sock_input_conn.settimeout(1)
                 self.sock_input_connected = True
                 break
             except socket.timeout:
@@ -270,11 +269,17 @@ def read_input_once(self) -> str:
                 request = self.sock_input_conn.recv(CHUNK_SIZE).decode(
                     "ascii", errors="replace"
                 )
-                return request
+                if request:
+                    return request
+                else:
+                    self.sock_input_connected = False
+                    break
+                loginfo("request")
             except socket.timeout:
                 continue
             except ConnectionResetError:
                 # Connection was reset, set sock_input_connected to False
+                loginfo("disconnected")
                 self.sock_input_connected = False
                 break
         return ""
@@ -311,6 +316,8 @@ def thread_read_input(self, process):
                 # If broken pipe connect
                 if not request:
                     break
+                if request == "stop":
+                    return
                 loginfo("reader sending: " + request)
                 process.stdin.write(request + "\n")
                 process.stdin.flush()
@@ -347,7 +354,11 @@ def send_command(self, command):
             output_thread.start()
 
             # When the process finishes, te output thread automatically closes
-            while output_thread.is_alive() and process.poll() is None:
+            while (
+                output_thread.is_alive()
+                and input_thread.is_alive()
+                and process.poll() is None
+            ):
                 rospy.sleep(1)
 
         except Exception as e:
@@ -802,11 +813,12 @@ def survey_manager_executor_recursive(
     )
 
     while exit_code != 0 and not rospy.is_shutdown():
+        loginfo(f"Exit code non-zero: Do you want to repeat the survey? (yes/no/skip):")
         process_executor.write_output_once(
             "Exit code non-zero: Do you want to repeat the survey? (yes/no/skip): \n"
         )
         repeat = process_executor.read_input_once().lower()
-
+        loginfo(f"Got response: {repeat}")
         if repeat == "yes":
             run_number += 1
             exit_code = survey_manager_executor_recursive(