diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc index 2749f107..0a69adc2 100644 --- a/astrobee/behaviors/inspection/tools/inspection_tool.cc +++ b/astrobee/behaviors/inspection/tools/inspection_tool.cc @@ -271,8 +271,9 @@ void ResultCallback(ff_util::FreeFlyerActionState::Enum code, teardown: std::cout << std::endl; - stopflag_ = true; ros::shutdown(); + bool success = (code == ff_util::FreeFlyerActionState::Enum::SUCCESS); + exit(success ? EXIT_SUCCESS : EXIT_FAILURE); } // Send the inspection goal to the server @@ -311,37 +312,10 @@ void SendGoal(ff_util::FreeFlyerActionClient *clie client->SendGoal(goal); } -bool GetlineAsync(std::istream& is, std::string& str, char delim = '\n') { - static std::string linesofar; - char inchar; - int charsread = 0; - bool lineread = false; - str = ""; - - do { - charsread = is.readsome(&inchar, 1); - if (charsread == 1) { - // if the delimiter is read then return the string so far - if (inchar == delim) { - str = linesofar; - linesofar = ""; - lineread = true; - } else { // otherwise add it to the string so far - linesofar.append(1, inchar); - } - } - } while (charsread != 0 && !lineread && !!stopflag_); - - return lineread; -} - void GetInput(ff_util::FreeFlyerActionClient *client) { - while (!stopflag_ && ros::ok()) { + while (ros::ok()) { std::string line, val; - - if (!GetlineAsync(std::cin, line)) - continue; - + std::getline(std::cin, line); std::string s; try { switch (std::stoi(line)) { @@ -352,7 +326,7 @@ void GetInput(ff_util::FreeFlyerActionClient *clie if (s.size() < 80) s.append(80 - s.size(), ' '); std::cout << s << std::endl; stopflag_ = true; - break; + return; case 1: FLAGS_pause = true; SendGoal(client); @@ -538,24 +512,27 @@ int main(int argc, char *argv[]) { boost::thread inp(GetInput, &client); if (FLAGS_remote) { - ros::Rate loop_rate(10); - ros::Time start_time = ros::Time::now(); - - // Spin for 3 seconds - while (ros::Time::now() - start_time < ros::Duration(3.0)) - loop_rate.sleep(); - + ros::Duration(3.0).sleep(); SendGoal(&client); } // Synchronous mode - while (!stopflag_) { + while (ros::ok() && !stopflag_) { ros::spinOnce(); } + // Finish commandline flags + google::ShutDownCommandLineFlags(); + + // Clean up threads and flush streams + if (!stopflag_) { + const char* msg = "inspection_tool: Exiting when stopflag not set, exit code 1"; + ROS_ERROR("%s", msg); + fprintf(stderr, "%s\n", msg); + exit(1); + } + // Wait for thread to exit inp.join(); - // Finish commandline flags - google::ShutDownCommandLineFlags(); // Make for great success return 0; } diff --git a/astrobee/description/description/urdf/sensor_RFID_receiver.urdf.xacro b/astrobee/description/description/urdf/sensor_RFID_receiver.urdf.xacro index 02e04af4..c118274f 100644 --- a/astrobee/description/description/urdf/sensor_RFID_receiver.urdf.xacro +++ b/astrobee/description/description/urdf/sensor_RFID_receiver.urdf.xacro @@ -22,7 +22,7 @@ 1 - 62.5 + 1 true /${ns}/ diff --git a/astrobee/description/description/urdf/sensor_RFID_transmitter.urdf.xacro b/astrobee/description/description/urdf/sensor_RFID_transmitter.urdf.xacro index 4c143b92..c929f82d 100644 --- a/astrobee/description/description/urdf/sensor_RFID_transmitter.urdf.xacro +++ b/astrobee/description/description/urdf/sensor_RFID_transmitter.urdf.xacro @@ -36,7 +36,7 @@ 1 - 62.5 + 1 true @@ -46,7 +46,7 @@ true - 62.5 + 1 world body false diff --git a/astrobee/description/description/urdf/sensor_wifi_receiver.urdf.xacro b/astrobee/description/description/urdf/sensor_wifi_receiver.urdf.xacro index 68e58b9e..2a899411 100644 --- a/astrobee/description/description/urdf/sensor_wifi_receiver.urdf.xacro +++ b/astrobee/description/description/urdf/sensor_wifi_receiver.urdf.xacro @@ -23,7 +23,7 @@ 0 0 0 0 0 0 true - 62.5 + 5 true + @@ -48,29 +49,29 @@ - + - + - + - + - + - + - + - + - + - + diff --git a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp index 4ab1d44a..64466071 100644 --- a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp @@ -148,8 +148,9 @@ double get_action_duration(const std::string& action_name) { return duration; } -IsaacAction::IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate) - : ActionExecutorClient(nh, action, rate) { +IsaacAction::IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate, + bool quick) + : ActionExecutorClient(nh, action, rate), quick_(quick) { action_name_ = action; progress_ = 0.0; pid_ = 0; @@ -174,7 +175,8 @@ void IsaacAction::do_work() { args_str += " " + arg; } command_ = std::string("(") + args_str + ")"; - std::string command_astrobee_call = std::string("rosrun survey_planner command_astrobee ") + args_str; + std::string quick_str = quick_ ? "--quick " : ""; + std::string command_astrobee_call = std::string("rosrun survey_planner command_astrobee ") + quick_str + args_str; start_time_ = ros::Time::now(); pid_ = fork(); @@ -241,6 +243,12 @@ int isaac_action_main(int argc, char *argv[], const char* action_name) { // Initialize a ros node ros::init(argc, argv, (std::string(action_name) + "_action").c_str()); + bool quick = false; + if (argc == 2 && std::string(argv[1]) == "--quick") { + printf("isaac_action_node[%s]: Got --quick; running longer actions in quick mode\n", action_name); + quick = true; + } + std::string name = ros::this_node::getName(); if (name.empty() || (name.size() == 1 && name[0] == '/')) name = "default"; @@ -253,7 +261,7 @@ int isaac_action_main(int argc, char *argv[], const char* action_name) { // Start action node // We could actually add multiple action nodes here being aware that we might need a ros::AsyncSpinner // (https://github.com/Bckempa/ros2_planning_system/blob/noetic-devel/plansys2_bt_actions/src/bt_action_node.cpp#L41) - auto action_node = std::make_shared(nh, action_name, std::chrono::seconds(2)); + auto action_node = std::make_shared(nh, action_name, std::chrono::seconds(2), quick); action_node->trigger_transition(ros::lifecycle::CONFIGURE); // Synchronous mode diff --git a/astrobee/survey_manager/survey_planner/src/survey_planner/command_astrobee.py b/astrobee/survey_manager/survey_planner/src/survey_planner/command_astrobee.py index 56d2e5d8..313fce21 100755 --- a/astrobee/survey_manager/survey_planner/src/survey_planner/command_astrobee.py +++ b/astrobee/survey_manager/survey_planner/src/survey_planner/command_astrobee.py @@ -27,6 +27,7 @@ import sys import threading import time # Add time module for waiting +from dataclasses import dataclass from typing import List import rospkg @@ -42,7 +43,11 @@ from std_msgs.msg import Header, String # Imports from survey_planner package -from survey_planner.problem_generator import load_yaml, yaml_action_from_pddl +from survey_planner.problem_generator import ( + YamlMapping, + load_yaml, + yaml_action_from_pddl, +) # Constants MAX_COUNTER = 10 @@ -57,6 +62,13 @@ def loginfo(msg: str) -> None: rospy.loginfo(f"{INFO_CONTEXT}: {msg}") +def first_non_zero(a: int, b: int) -> int: + "Return the first non-zero between `a` and `b`, or zero if both are zero." + if a != 0: + return a + return b + + def exposure_change(config_static, bay_origin, bay_destination): # Going to JEM if bay_origin == "nod2_hatch_to_jem" and bay_destination == "jem_hatch_from_nod2": @@ -123,6 +135,10 @@ def get_ops_plan_path(): return None +def get_ops_path() -> pathlib.Path: + return pathlib.Path(get_ops_plan_path()).parent.parent.resolve() + + # This class starts a new process and lets you monitor the input and output # Mostly used for actions where user inteference might be required class ProcessExecutor: @@ -330,14 +346,9 @@ def send_command(self, command): # When the process finishes, te output thread automatically closes while output_thread.is_alive() and process.poll() is None: rospy.sleep(1) - # Get the return code of the process - return_code = process.poll() except Exception as e: loginfo(f"send_command exiting on exception: {e}") - # Get the return code of the process - - process.kill() finally: # Forcefully stop the thread (not recommended) loginfo("send_command killing input thread...") @@ -346,17 +357,32 @@ def send_command(self, command): output_thread.join() input_thread.join() - # Get the final exit code + # Reset for subsequent send_command() call self._stop_event.clear() - print("check if process running ") - if process.poll() is None: - print("process is still opened, closing ") - # Try sending SIGTERM - os.kill(process.pid, signal.SIGTERM) - if process.poll() is None: - os.kill(process.pid, signal.SIGKILL) - return return_code + loginfo(f"Check if child process {process.pid} running ") + return_code = process.poll() + if return_code is not None: + loginfo(f"Child exited with exit status {return_code}") + return return_code + + kill_time = 2.0 + loginfo( + f"Child process {process.pid} is still running, kill -TERM then wait up to {kill_time} seconds" + ) + process.terminate() + try: + return_code = process.wait(timeout=kill_time) + loginfo(f"Child exited with exit status {return_code}") + return return_code + except subprocess.TimeoutExpired: + loginfo( + f"Child process {process.pid} is still running after {kill_time} seconds, kill -KILL" + ) + process.send_signal(signal.SIGKILL) + return_code = 1 + loginfo(f"Child killed, returning exit status {return_code}") + return return_code def send_command_recursive(self, command): loginfo(f"Sending recursive command: {command}") @@ -366,12 +392,12 @@ def send_command_recursive(self, command): while exit_code != 0 and not rospy.is_shutdown(): self.write_output_once( - "Exit code non-zero: Do you want to repeat the survey? (yes/no/skip): " + "Exit code non-zero: Do you want to repeat the survey? (yes/no/skip): \n" ) repeat = self.read_input_once().lower() loginfo(f"user input: {repeat}") if repeat == "yes": - exit_code = exit_code = self.send_command_recursive(command) + exit_code = self.send_command_recursive(command) break if repeat == "no": break @@ -385,7 +411,7 @@ def send_command_recursive(self, command): # Mostly used for short actions that should be immediate and require no feedback # This method is needed on actions that run remotely and are not controlled by topics class CommandExecutor: - def __init__(self, ns): + def __init__(self, ns: str): self.ns = ns loginfo(f"command topic: {self.ns}/command") # Declare guest science command publisher @@ -394,12 +420,12 @@ def __init__(self, ns): ) self.ack_needed = False self.ack_msg = None + self.plan_status_needed = False self.sub_plan_status = rospy.Subscriber( self.ns + "/mgt/executive/plan_status", PlanStatusStamped, self.plan_status_callback, ) - self.plan_status_needed = False self.plan_name = "" self.pub_command = rospy.Publisher( self.ns + "/command", CommandStamped, queue_size=5 @@ -510,7 +536,87 @@ def wait_plan(self): return 1 -def survey_manager_executor(args, run, config_static, process_executor): +def write_quick_pano(pano_path: pathlib.Path) -> pathlib.Path: + """ + Return path to abbreviated version of `pano_path`. Generate the abbreviated version if it + doesn't already exist. + """ + # Example: "foo.txt" -> "foo-quick.txt" + quick_path = pano_path.parent / (pano_path.stem + "-quick" + pano_path.suffix) + if False: # quick_path.exists(): # May be smarter to write every time + loginfo(f"write_quick_pano: using existing quick target list {quick_path}") + return quick_path + + # A bit arbitrary how to generate a "quick" version of a pano. Let's take + # two frames. + input_lines = pano_path.read_text(encoding="utf-8").splitlines() + if len(input_lines) >= 39: + # For our usual JEM panos, frames 37-38 should have pitch near zero and + # require a bit less rotation than some others. + output_lines = input_lines[37:39] + else: + # Must be some different pano type, best effort. + output_lines = input_lines[:2] + output_text = "".join([line + "\n" for line in output_lines]) + quick_path.write_text(output_text, encoding="utf-8") + loginfo(f"write_quick_pano: wrote quick target list {quick_path}") + return quick_path + + +def get_quick_stereo_survey(fplan_path: pathlib.Path) -> pathlib.Path: + """ + Return path to abbreviated version of `fplan_path`, if it exists. These fplans are generated by + manually editing the full fplan using the GDS plan editor. + """ + # Example: "foo.fplan" -> "foo-quick.fplan" + quick_path = fplan_path.parent / (fplan_path.stem + "-quick" + fplan_path.suffix) + if quick_path.exists(): + loginfo(f"get_quick_stereo_survey: found quick survey {quick_path}") + return quick_path + + loginfo(f"get_quick_stereo_survey: no quick survey available, using {fplan_path}") + return fplan_path + + +@dataclass +class SurveyManagerExecutor: + process_executor: ProcessExecutor + config_static: YamlMapping + fsw_ns_args: List[str] + cmd_exec_ns: str + + def move(self, from_name: str, to_name: str) -> int: + # Execute actual move + move_args = self.config_static["bays_move"][to_name] + cmd = ( + ["rosrun", "executive", "teleop_tool", "-remote", "-move"] + + move_args + + self.fsw_ns_args + ) + exit_code = self.process_executor.send_command_recursive(cmd) + if exit_code != 0: + return exit_code + + # Change exposure if needed + exposure_value = exposure_change( + self.config_static, + from_name, + to_name, + ) + if exposure_value != 0: + cexec = CommandExecutor(self.cmd_exec_ns) + exit_code = first_non_zero(exit_code, cexec.change_exposure(exposure_value)) + + # Change map if needed + map_name = map_change(self.config_static, from_name, to_name) + if map_name != "": + cexec = CommandExecutor(self.cmd_exec_ns) + exit_code = first_non_zero(exit_code, cexec.change_map(map_name)) + + return exit_code + + +def survey_manager_executor(args, run, config_static, process_executor, quick: bool): # Start ROS node rospy.init_node("survey_namager_cmd_" + args["robot"], anonymous=True) @@ -528,6 +634,7 @@ def survey_manager_executor(args, run, config_static, process_executor): sim = True ns = [] + cmd_exec_ns = "" # If we're commanding a robot remotely if current_robot != args["robot"]: loginfo( @@ -535,14 +642,21 @@ def survey_manager_executor(args, run, config_static, process_executor): ) ns = ["-ns", args["robot"]] # Command executor will add namespace for bridge forwarding - command_executor = CommandExecutor("/" + args["robot"]) - else: - command_executor = CommandExecutor("") + cmd_exec_ns = "/" + args["robot"] + command_executor = CommandExecutor(cmd_exec_ns) + + sm_exec = SurveyManagerExecutor(process_executor, config_static, ns, cmd_exec_ns) # Initialize exit code exit_code = 0 if args["type"] == "dock": + # Avoid "Executive: Command failed with message: Dock goal failed with + # response: Too far from dock" + exit_code = sm_exec.move(args["from_name"], args["berth"]) + if exit_code != 0: + return exit_code + cmd = [ "rosrun", "executive", @@ -552,37 +666,29 @@ def survey_manager_executor(args, run, config_static, process_executor): "-berth", config_static["berth"][args["berth"]], ] - cmd.extend(ns) if ns else None + cmd.extend(ns) - exit_code += process_executor.send_command_recursive(cmd) + exit_code = first_non_zero( + exit_code, process_executor.send_command_recursive(cmd) + ) elif args["type"] == "undock": cmd = ["rosrun", "executive", "teleop_tool", "-undock", "-remote"] - cmd.extend(ns) if ns else None + cmd.extend(ns) - exit_code += process_executor.send_command_recursive(cmd) + exit_code = first_non_zero( + exit_code, process_executor.send_command_recursive(cmd) + ) elif args["type"] == "move": - cmd = ["rosrun", "executive", "teleop_tool", "-remote", "-move"] - cmd.extend(config_static["bays_move"][args["to_name"]]) - cmd.extend(ns) if ns else None - - exit_code += process_executor.send_command_recursive(cmd) - - # Change exposure if needed - exposure_value = exposure_change( - config_static, args["from_name"], args["to_name"] - ) - if exposure_value != 0: - exit_code += command_executor.change_exposure(exposure_value) - # Change map if needed - map_name = map_change(config_static, args["from_name"], args["to_name"]) - if map_name != "": - exit_code += command_executor.change_map(map_name) + exit_code = sm_exec.move(args["from_name"], args["to_name"]) elif args["type"] == "panorama": - exit_code += command_executor.start_recording( - "pano_" + args["location_name"] + "_" + run + exit_code = first_non_zero( + exit_code, + command_executor.start_recording( + "pano_" + args["location_name"] + "_" + run + ), ) if exit_code != 0: loginfo( @@ -590,24 +696,47 @@ def survey_manager_executor(args, run, config_static, process_executor): ) return exit_code + pano_path = pathlib.Path("resources") / pathlib.Path( + config_static["bays_pano"][args["location_name"]] + ) + if quick: + inspection_path = pathlib.Path(rospkg.RosPack().get_path("inspection")) + pano_path = write_quick_pano(inspection_path / pano_path).relative_to( + inspection_path + ) + cmd = [ "rosrun", "inspection", "inspection_tool", "-geometry", "-geometry_poses", - "/resources/" + config_static["bays_pano"][args["location_name"]], + "/" + str(pano_path), "-remote", ] - cmd.extend(ns) if ns else None + cmd.extend(ns) - exit_code += process_executor.send_command_recursive(cmd) - print("STOP RECORDING") - exit_code += command_executor.stop_recording() + exit_code = first_non_zero( + exit_code, process_executor.send_command_recursive(cmd) + ) + loginfo("STOP RECORDING") + exit_code = first_non_zero(exit_code, command_executor.stop_recording()) + + # Sometimes the plan has a robot idle for an extended period after + # completing a panorama. By default, when the inspection tool finishes + # a panorama, the robot will be left in the pose it had for the final pano + # frame, which might not be ideal for localization. This final move should + # ensure the robot ends in the preferred attitude. + exit_code = first_non_zero( + exit_code, sm_exec.move(args["location_name"], args["location_name"]) + ) elif args["type"] == "stereo": - exit_code += command_executor.start_recording( - "stereo_" + os.path.basename(args["fplan"]) + "_" + run + exit_code = first_non_zero( + exit_code, + command_executor.start_recording( + "stereo_" + os.path.basename(args["fplan"]) + "_" + run + ), ) if exit_code != 0: loginfo( @@ -616,46 +745,81 @@ def survey_manager_executor(args, run, config_static, process_executor): return exit_code # This starts the plan - plan_path = get_ops_plan_path() + plan_path = pathlib.Path(get_ops_plan_path()) + fplan_path = plan_path / pathlib.Path(args["fplan"] + ".fplan") + + if quick: + fplan_path = get_quick_stereo_survey(fplan_path) command_executor.plan_status_needed = True - command_executor.plan_name = os.path.basename(args["fplan"]) + command_executor.plan_name = fplan_path.stem + + use_astrobee_ops = True + if use_astrobee_ops: + # Use astrobee_ops tool which provides for user interaction + ops_path = get_ops_path() + cmd_path = ops_path / "dock_scripts" / "hsc" / "cmd" + old_env = os.environ.copy() + try: + os.environ["TOPIC_PREFIX"] = cmd_exec_ns - cmd = [ - "rosrun", - "executive", - "plan_pub", - os.path.join(plan_path, args["fplan"] + ".fplan"), - "-remote", - ] - cmd.extend(ns) if ns else None + cmd = [str(cmd_path), "-c", "plan", "-load", str(fplan_path)] + exit_code = first_non_zero( + exit_code, process_executor.send_command_recursive(cmd) + ) + + cmd = [str(cmd_path), "-c", "plan", "-run"] + exit_code = first_non_zero( + exit_code, process_executor.send_command_recursive(cmd) + ) + finally: + os.environ.clear() + os.environ.update(old_env) + else: + # Use FSW tools + cmd = [ + "rosrun", + "executive", + "plan_pub", + str(fplan_path), + "-remote", + ] + cmd.extend(ns) + + exit_code = first_non_zero( + exit_code, process_executor.send_command_recursive(cmd) + ) - exit_code += process_executor.send_command_recursive(cmd) + if exit_code == 0: + exit_code = first_non_zero(exit_code, command_executor.wait_plan()) - if exit_code == 0: - exit_code += command_executor.wait_plan() - exit_code += command_executor.stop_recording() + exit_code = first_non_zero(exit_code, command_executor.stop_recording()) + + # Ensure robot ends in preferred attitude for its bay. + exit_code = first_non_zero( + exit_code, sm_exec.move(args["base_name"], args["base_name"]) + ) return exit_code def survey_manager_executor_recursive( - command_names, run_number, config_static, process_executor + command_names, run_number, config_static, process_executor, quick: bool ): exit_code = survey_manager_executor( - command_names, f"run{run_number}", config_static, process_executor + command_names, f"run{run_number}", config_static, process_executor, quick ) while exit_code != 0 and not rospy.is_shutdown(): process_executor.write_output_once( - "Exit code non-zero: Do you want to repeat the survey? (yes/no/skip): " + "Exit code non-zero: Do you want to repeat the survey? (yes/no/skip): \n" ) repeat = process_executor.read_input_once().lower() if repeat == "yes": run_number += 1 exit_code = survey_manager_executor_recursive( - command_names, run_number, config_static, process_executor + command_names, run_number, config_static, process_executor, quick ) break if repeat == "no": @@ -667,9 +831,11 @@ def survey_manager_executor_recursive( return exit_code -def command_astrobee(action_args, config_static_paths: List[pathlib.Path]): +def command_astrobee( + action_args, config_static_paths: List[pathlib.Path], quick: bool +) -> int: # Read the static configs that convert constants to values - config_static = {} + config_static: YamlMapping = {} for config_static_path in config_static_paths: loginfo(f"reading config: {config_static_path}") yaml_dict = load_yaml(config_static_path) @@ -691,7 +857,7 @@ def command_astrobee(action_args, config_static_paths: List[pathlib.Path]): process_executor = ProcessExecutor(args["robot"]) exit_code = survey_manager_executor_recursive( - args, 1, config_static, process_executor + args, 1, config_static, process_executor, quick ) loginfo(f"Finished plan action with code {exit_code}") @@ -729,9 +895,15 @@ def main(): nargs="+", default=[pathlib.Path(path) for path in default_config_paths], ) + parser.add_argument( + "--quick", + help="use quick versions of longer actions", + action="store_true", + default=False, + ) args = parser.parse_args() - return command_astrobee(args.action_args, args.config_static) + return command_astrobee(args.action_args, args.config_static, args.quick) if __name__ == "__main__": diff --git a/astrobee/survey_manager/survey_planner/src/survey_planner/monitor_astrobee.py b/astrobee/survey_manager/survey_planner/src/survey_planner/monitor_astrobee.py index 73b561e2..3be260db 100755 --- a/astrobee/survey_manager/survey_planner/src/survey_planner/monitor_astrobee.py +++ b/astrobee/survey_manager/survey_planner/src/survey_planner/monitor_astrobee.py @@ -27,6 +27,7 @@ # Constants CHUNK_SIZE = 1024 + # Declare event that will stop input thread stop_event = threading.Event() @@ -34,28 +35,32 @@ def thread_write_input(input_path): print("Start thread_write_input") + user_input = "" while not stop_event.is_set(): try: # Attempt to connect to the server sock_client_input = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) sock_client_input.connect(input_path) except ConnectionRefusedError: - print("Input Connection refused. Retrying in 5 seconds...") + # print("Input Connection refused. Retrying in 5 seconds...") time.sleep(5) continue try: while not stop_event.is_set(): - user_input = input() - print("user input: " + user_input) + if user_input == "": + user_input = input() + # print("user input: " + user_input) if user_input.lower().strip() == "exit": stop_event.set() break sock_client_input.send( user_input.encode("ascii", errors="replace")[:CHUNK_SIZE] ) + user_input = "" except Exception as e: - print("Exception in thread_write_input:", e) + # print("Exception in thread_write_input:", e) + pass # Close the sockets sock_client_input.close() @@ -71,7 +76,7 @@ def thread_read_output(output_path): sock_client_output.connect(output_path) except ConnectionRefusedError: - print("Output Connection refused. Retrying in 5 seconds...") + # print("Output Connection refused. Retrying in 5 seconds...") time.sleep(5) continue @@ -79,7 +84,7 @@ def thread_read_output(output_path): try: while not stop_event.is_set(): - print("get data") + # print("get data") request = sock_client_output.recv(CHUNK_SIZE) request = request.decode("ascii", errors="replace") print(request, end="") @@ -87,7 +92,8 @@ def thread_read_output(output_path): print("Server disconnected") break except Exception as e: - print("Exception in thread_read_output:", e) + # print("Exception in thread_read_output:", e) + pass # Close the sockets sock_client_output.close() diff --git a/astrobee/survey_manager/survey_planner/src/survey_planner/problem_generator.py b/astrobee/survey_manager/survey_planner/src/survey_planner/problem_generator.py index 2e299e85..8d3db54e 100755 --- a/astrobee/survey_manager/survey_planner/src/survey_planner/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/src/survey_planner/problem_generator.py @@ -128,9 +128,13 @@ def yaml_action_from_pddl(action: str, config: YamlMapping) -> Optional[YamlMapp ), f"Expected action type in {ACTION_TYPE_OPTIONS}, got {action_type}" if action_type == "dock": - robot, _from_bay, to_berth = action_args[1:] - # Can discard from_bay - return {"type": "dock", "robot": robot, "berth": to_berth} + robot, from_bay, to_berth = action_args[1:] + return { + "type": "dock", + "robot": robot, + "from_name": from_bay, + "berth": to_berth, + } if action_type == "undock": robot, _from_berth, _to_bay, _check1, _check2 = action_args[1:] diff --git a/isaac/launch/isaac_astrobee.launch b/isaac/launch/isaac_astrobee.launch index c03b154c..630a9d63 100644 --- a/isaac/launch/isaac_astrobee.launch +++ b/isaac/launch/isaac_astrobee.launch @@ -31,6 +31,7 @@ + @@ -260,6 +261,7 @@ + diff --git a/isaac/launch/robot/GLP.launch b/isaac/launch/robot/GLP.launch index 591c862c7..63b8dc3d 100644 --- a/isaac/launch/robot/GLP.launch +++ b/isaac/launch/robot/GLP.launch @@ -19,17 +19,18 @@ - - - - - - - - + + + + + + + + - - + + + @@ -38,15 +39,17 @@ - - - - - - - - - + + + + + + + + + + + diff --git a/isaac/launch/sim.launch b/isaac/launch/sim.launch index 53d3029a..1b3ed092 100644 --- a/isaac/launch/sim.launch +++ b/isaac/launch/sim.launch @@ -51,6 +51,7 @@ + @@ -209,6 +210,7 @@ + @@ -241,6 +243,7 @@ + @@ -273,6 +276,7 @@ + @@ -305,6 +309,7 @@ +