From 2550c1001fba87b8fca56e35e1035d94c0038920 Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Mon, 1 Apr 2024 17:13:19 -0700 Subject: [PATCH] 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 | 5 ++ .../inspection/src/inspection_node.cc | 31 +++++-- .../survey/survey_dependencies/ros1_lifecycle | 2 +- .../data/granite_survey_static.yaml | 8 +- .../data/jem_survey_dynamic.yaml | 8 +- .../pddl/problem_granite_survey.ps2.pddl | 14 +-- .../survey_manager/src/isaac_action_node.cpp | 3 +- .../src/survey_manager/command_astrobee.py | 58 ++++++++----- .../src/survey_manager/monitor_astrobee.py | 87 +++++++++++-------- debian/changelog | 7 ++ isaac.doxyfile | 2 +- isaac/config/behaviors/inspection.config | 10 +++ 12 files changed, 151 insertions(+), 84 deletions(-) diff --git a/RELEASE.md b/RELEASE.md index 88a40905..63c73fa8 100644 --- a/RELEASE.md +++ b/RELEASE.md @@ -1,5 +1,10 @@ # Releases +## Release 0.2.8 + + * Improvements to survey_manager output + * Bug fixes + ## Release 0.2.7 * Survey manager diff --git a/astrobee/behaviors/inspection/src/inspection_node.cc b/astrobee/behaviors/inspection/src/inspection_node.cc index 235e0cb5..fa1f940f 100644 --- a/astrobee/behaviors/inspection/src/inspection_node.cc +++ b/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 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("toggle_flashlight")) { flashlight_intensity_current_ = cfg_.Get("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("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("sci_cam_startup_focus")) + "}"); return fsm_.Update(GOAL_INSPECT); + } break; // Volumetric command case isaac_msgs::InspectionGoal::VOLUMETRIC: diff --git a/astrobee/survey/survey_dependencies/ros1_lifecycle b/astrobee/survey/survey_dependencies/ros1_lifecycle index 39f5ac69..1b98542f 160000 --- a/astrobee/survey/survey_dependencies/ros1_lifecycle +++ b/astrobee/survey/survey_dependencies/ros1_lifecycle @@ -1 +1 @@ -Subproject commit 39f5ac693555481d67b9898a0ee172db2692b92d +Subproject commit 1b98542f66b9488c8a3a082df339733091a30f58 diff --git a/astrobee/survey/survey_manager/data/granite_survey_static.yaml b/astrobee/survey/survey_manager/data/granite_survey_static.yaml index 52229c3d..675e9c25 100644 --- a/astrobee/survey/survey_manager/data/granite_survey_static.yaml +++ b/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] diff --git a/astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml b/astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml index 69e8d29a..841764d9 100644 --- a/astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml +++ b/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 diff --git a/astrobee/survey/survey_manager/pddl/problem_granite_survey.ps2.pddl b/astrobee/survey/survey_manager/pddl/problem_granite_survey.ps2.pddl index a7807ff2..1575bf06 100644 --- a/astrobee/survey/survey_manager/pddl/problem_granite_survey.ps2.pddl +++ b/astrobee/survey/survey_manager/pddl/problem_granite_survey.ps2.pddl @@ -7,8 +7,8 @@ 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 @@ -16,7 +16,7 @@ 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) diff --git a/astrobee/survey/survey_manager/src/isaac_action_node.cpp b/astrobee/survey/survey_manager/src/isaac_action_node.cpp index b2891af5..a826c7f4 100644 --- a/astrobee/survey/survey_manager/src/isaac_action_node.cpp +++ b/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); diff --git a/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py b/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py index eb16ce9c..3bde26cd 100755 --- a/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py +++ b/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( diff --git a/astrobee/survey/survey_manager/src/survey_manager/monitor_astrobee.py b/astrobee/survey/survey_manager/src/survey_manager/monitor_astrobee.py index 3be260db..1b9bd31e 100755 --- a/astrobee/survey/survey_manager/src/survey_manager/monitor_astrobee.py +++ b/astrobee/survey/survey_manager/src/survey_manager/monitor_astrobee.py @@ -43,7 +43,7 @@ def thread_write_input(input_path): sock_client_input.connect(input_path) except ConnectionRefusedError: # print("Input Connection refused. Retrying in 5 seconds...") - time.sleep(5) + time.sleep(1) continue try: @@ -53,6 +53,7 @@ def thread_write_input(input_path): # print("user input: " + user_input) if user_input.lower().strip() == "exit": stop_event.set() + if stop_event.is_set(): break sock_client_input.send( user_input.encode("ascii", errors="replace")[:CHUNK_SIZE] @@ -61,8 +62,8 @@ def thread_write_input(input_path): except Exception as e: # print("Exception in thread_write_input:", e) pass - # Close the sockets + print("Close input socket") sock_client_input.close() @@ -73,51 +74,63 @@ def thread_read_output(output_path): try: # Attempt to connect to the server sock_client_output = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + sock_client_output.settimeout(1) # Set timeout to 1 second sock_client_output.connect(output_path) + while not stop_event.is_set(): + try: + data = sock_client_output.recv(CHUNK_SIZE) + if not data: + print("Server disconnected") + break + + data_decoded = data.decode("ascii", errors="replace") + if not data_decoded.startswith("pos: x:"): + print(data_decoded, end="") + else: + print( + "\r" + data_decoded.replace("\n", ""), end="\r", flush=True + ) + + except socket.timeout: + continue # Timeout reached, check stop event and try again + except Exception as e: + break # Handle other exceptions or break except ConnectionRefusedError: - # print("Output Connection refused. Retrying in 5 seconds...") - time.sleep(5) + time.sleep(5) # Wait a bit before retrying to connect continue - - print("connected") - - try: - while not stop_event.is_set(): - # print("get data") - request = sock_client_output.recv(CHUNK_SIZE) - request = request.decode("ascii", errors="replace") - print(request, end="") - if not request: # If no data received, it means the server disconnected - print("Server disconnected") - break - except Exception as e: - # print("Exception in thread_read_output:", e) - pass - - # Close the sockets + except socket.timeout: + continue + print("Close output socket") sock_client_output.close() def survey_monitor(robot_name): - input_path = "/tmp/input_" + robot_name - output_path = "/tmp/output_" + robot_name + try: + input_path = "/tmp/input_" + robot_name + output_path = "/tmp/output_" + robot_name - while not (os.path.exists(input_path) and os.path.exists(output_path)): - print("Files don't exist yet. Waiting for 5 seconds...") - time.sleep(5) - continue - - # Start input and output threads - input_thread = threading.Thread(target=thread_write_input, args=(input_path,)) - input_thread.start() - output_thread = threading.Thread(target=thread_read_output, args=(output_path,)) - output_thread.start() - - # Wait for the threads to finish - input_thread.join() + while not (os.path.exists(input_path) and os.path.exists(output_path)): + print("Files don't exist yet. Waiting for 5 seconds...") + time.sleep(5) + continue - output_thread.join() + # Start input and output threads + input_thread = threading.Thread(target=thread_write_input, args=(input_path,)) + output_thread = threading.Thread(target=thread_read_output, args=(output_path,)) + input_thread.start() + output_thread.start() + + # Wait for the threads to finish + input_thread.join() + output_thread.join() + + except KeyboardInterrupt: + print("\nGracefully shutting down, Press Enter...") + stop_event.set() + input_thread.join() + output_thread.join() + print("Shutdown complete.") class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter): diff --git a/debian/changelog b/debian/changelog index 50d13c95..7b8a045b 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,10 @@ +isaac (0.2.8) testing; urgency=medium + + * Improvements to survey_manager output + * Bug fixes + + -- ISAAC Flight Software Wed, 27 Mar 2024 19:26:57 -0700 + isaac (0.2.7) testing; urgency=medium * Survey manager diff --git a/isaac.doxyfile b/isaac.doxyfile index 7c9a2636..eaf3733a 100644 --- a/isaac.doxyfile +++ b/isaac.doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "ISAAC" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 0.2.7 +PROJECT_NUMBER = 0.2.8 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/isaac/config/behaviors/inspection.config b/isaac/config/behaviors/inspection.config index cad7908b..bd86325f 100644 --- a/isaac/config/behaviors/inspection.config +++ b/isaac/config/behaviors/inspection.config @@ -359,6 +359,16 @@ parameters = { unit = "", description = "Times it will retry to take a picture after timeout before sending fail" }, + { + id = "sci_cam_startup_focus", + reconfigurable = true, + type = "double", + default = 0.39, + min = 0, + max = 15, + unit = "", + description = "Times it will retry to take a picture after timeout before sending fail" + }, }