From d34392c5a25dc7ce21d9e28a76513bc1a86b6074 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Fri, 3 May 2024 19:29:27 -0700 Subject: [PATCH] isaac 15 in usl + fixes to command_astrobee + allowing for exposure and map changes --- ...vey_static.yaml => iss_survey_static.yaml} | 45 +- .../data/jem_survey_dynamic.yaml | 8 +- .../data/usl_survey_dynamic.yaml | 53 ++ .../pddl/problem_usl_survey.ps2.pddl | 594 ++++++++++++++++++ .../src/survey_manager/command_astrobee.py | 141 +++-- .../src/survey_manager/problem_generator.py | 6 +- 6 files changed, 784 insertions(+), 63 deletions(-) rename astrobee/survey/survey_manager/data/{jem_survey_static.yaml => iss_survey_static.yaml} (78%) create mode 100644 astrobee/survey/survey_manager/data/usl_survey_dynamic.yaml create mode 100644 astrobee/survey/survey_manager/pddl/problem_usl_survey.ps2.pddl diff --git a/astrobee/survey/survey_manager/data/jem_survey_static.yaml b/astrobee/survey/survey_manager/data/iss_survey_static.yaml similarity index 78% rename from astrobee/survey/survey_manager/data/jem_survey_static.yaml rename to astrobee/survey/survey_manager/data/iss_survey_static.yaml index 255edf9d..8731812e 100644 --- a/astrobee/survey/survey_manager/data/jem_survey_static.yaml +++ b/astrobee/survey/survey_manager/data/iss_survey_static.yaml @@ -36,6 +36,19 @@ bays: jem_bay5: [11.0, -8.0, 4.8] jem_bay6: [11.0, -9.0, 4.8] jem_bay7: [11.0, -9.7, 4.8] + jem_bay0: [11.0, -9.7, 4.8] + nod2_bay1: [11.0, -9.7, 4.8] + nod2_bay2: [11.0, -9.7, 4.8] + nod2_bay3: [11.0, -9.7, 4.8] + nod2_bay4: [11.0, -9.7, 4.8] + nod2_bay5: [11.0, -9.7, 4.8] + usl_bay0: [11.0, -9.7, 4.8] + usl_bay1: [11.0, -9.7, 4.8] + usl_bay2: [11.0, -9.7, 4.8] + usl_bay3: [11.0, -9.7, 4.8] + usl_bay4: [11.0, -9.7, 4.8] + usl_bay5: [11.0, -9.7, 4.8] + usl_bay6: [11.0, -9.7, 4.8] bays_move: jem_bay1: ["-pos", "11 -4 4.8", "-att", "1.5708 0 0 1"] @@ -45,23 +58,19 @@ bays_move: jem_bay5: ["-pos", "11 -8 4.8", "-att", "-1.5708 0 0 1"] jem_bay6: ["-pos", "11 -9 4.8", "-att", "-1.5708 0 0 1"] jem_bay7: ["-pos", "11 -9.7 4.8", "-att", "-1.5708 0 0 1"] - jem_hatch_to_nod2: ["-pos", "11 -3.5 4.8", "-att", "1.5708 0 0 1"] - jem_hatch_from_nod2: ["-pos", "11 -3.5 4.8", "-att", "-1.5708 0 0 1"] - nod2_hatch_from_jem: ["-pos", "11 -1.0 4.8", "-att", "1.5708 0 0 1"] - nod2_hatch_to_jem: ["-pos", "11 -1.0 4.8", "-att", "-1.5708 0 0 1"] + jem_bay0: ["-pos", "11 -3.5 4.8", "-att", "-1.5708 0 0 1"] + nod2_bay1: ["-pos", "11 -1.0 4.8", "-att", "-1.5708 0 0 1"] nod2_bay2: ["-pos", "11 0 4.8", "-att", "0 0 0 1"] nod2_bay3: ["-pos", "10 0 4.8", "-att", "0 0 0 1"] nod2_bay4: ["-pos", "9 0 4.8", "-att", "0 0 0 1"] - nod2_hatch_to_usl: ["-pos", "7.8 -3.5 4.8", "-att", "3.14 0 0 1"] - nod2_hatch_from_usl: ["-pos", "7.8 -3.5 4.8", "-att", "0 0 0 1"] - usl_hatch_from_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "3.14 0 0 1"] - usl_hatch_to_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "0 0 0 1"] + nod2_bay5: ["-pos", "7.8 0.0 4.8", "-att", "0 0 0 1"] + usl_bay0: ["-pos", "5.3 0.0 4.8", "-att", "0 0 0 1"] usl_bay1: ["-pos", "4.7 0 4.8", "-att", "0 0 0 1"] usl_bay2: ["-pos", "3.65 0 4.8", "-att", "0 0 0 1"] usl_bay3: ["-pos", "2.6 0 4.8", "-att", "0 0 0 1"] - usl_bay4: ["-pos", "1.55 0 4.8", "-att", "0 0 0 1"] - usl_bay5: ["-pos", "0.5 0 4.8", "-att", "0 0 0 1"] - usl_bay6: ["-pos", "-0.5 0 4.8", "-att", "0 0 0 1"] + usl_bay4: ["-pos", "1.55 0 4.8", "-att", "3.14 0 0 1"] + usl_bay5: ["-pos", "0.5 0 4.8", "-att", "3.14 0 0 1"] + usl_bay6: ["-pos", "-0.5 0 4.8", "-att", "3.14 0 0 1"] # berth approach points from gds_configs/IssWorld/BookmarksList.json berth1: ["-pos", "10.5 -9.25 4.5", "-att", "0 0 0 1"] berth2: ["-pos", "10.5 -9.75 4.5", "-att", "3.14 1 0 0"] @@ -97,7 +106,7 @@ berth: berth1: "1" berth2: "2" -bogus_bays: [jem_bay0, jem_bay8] +bogus_bays: [jem_bay0, jem_bay8, nod2_bay1, nod2_bay5, usl_bay0, usl_bay7] berths: [berth1, berth2] robots: [bumble, honey] @@ -116,3 +125,15 @@ stereo: fplan: "ISAAC/jem_stereo_mapping_bay7_to_bay4" base_location: jem_bay7 bound_location: jem_bay4 + usl_bay1_to_bay3: + fplan: "ISAAC/usl_stereo_mapping_bay1_to_bay3" + base_location: usl_bay1 + bound_location: usl_bay3 + usl_bay6_to_bay4: + fplan: "ISAAC/usl_stereo_mapping_bay6_to_bay4" + base_location: usl_bay6 + bound_location: usl_bay4 + usl_bay1_to_bay6: + fplan: "ISAAC/usl_stereo_mapping_start_bay1" + base_location: usl_bay1 + bound_location: usl_bay6 diff --git a/astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml b/astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml index 841764d9..69e8d29a 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_g} +- {type: robot_at, robot: bumble, location: berth1} # 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_g} +- {type: robot_at, robot: honey, location: berth2} init: bumble: - location: berth1_g + location: berth1 honey: - location: berth2_g + location: berth2 diff --git a/astrobee/survey/survey_manager/data/usl_survey_dynamic.yaml b/astrobee/survey/survey_manager/data/usl_survey_dynamic.yaml new file mode 100644 index 00000000..86466314 --- /dev/null +++ b/astrobee/survey/survey_manager/data/usl_survey_dynamic.yaml @@ -0,0 +1,53 @@ +# Copyright (c) 2023, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +# platform" software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# Example dynamic configuration info used when generating a PDDL problem. For now, this is goal +# conditions and initial state. A likely conops is that the initial version of this file for a +# specific activity would be hand-generated, but it might later be automatically regenerated by the +# survey manager when a replan is needed (remove completed/failed goals, add retry goals, update +# initial state to match actual current state, etc.) See also jem_survey_static.yaml. + +goals: + +- {type: panorama, robot: bumble, order: 1, location: usl_bay3} +- {type: panorama, robot: bumble, order: 2, location: usl_bay2} +- {type: panorama, robot: bumble, order: 3, location: usl_bay1} +# - {type: stereo, robot: bumble, order: 4, trajectory: usl_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} + +# 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 +# this constraint, POPF produces a very inefficient plan where bumble never leaves the dock until +# after honey finishes all its tasks and returns to dock. (It's safe to comment this out if the +# planner doesn't need the hint.) +# - {type: let_other_robot_reach, robot: honey, order: 0, location: jem_bay5} + +- {type: panorama, robot: honey, order: 1, location: usl_bay6} +- {type: panorama, robot: honey, order: 2, location: usl_bay5} +- {type: panorama, robot: honey, order: 3, location: usl_bay4} +# 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: usl_bay1_to_bay6} +- {type: robot_at, robot: honey, location: berth2} + +init: + bumble: + location: usl_bay1 + honey: + location: usl_bay6 diff --git a/astrobee/survey/survey_manager/pddl/problem_usl_survey.ps2.pddl b/astrobee/survey/survey_manager/pddl/problem_usl_survey.ps2.pddl new file mode 100644 index 00000000..1611d2a7 --- /dev/null +++ b/astrobee/survey/survey_manager/pddl/problem_usl_survey.ps2.pddl @@ -0,0 +1,594 @@ +set instance jem_bay0 location +set instance jem_bay1 location +set instance jem_bay2 location +set instance jem_bay3 location +set instance jem_bay4 location +set instance jem_bay5 location +set instance jem_bay6 location +set instance jem_bay7 location +set instance jem_bay8 location +set instance nod2_bay1 location +set instance nod2_bay2 location +set instance nod2_bay3 location +set instance nod2_bay4 location +set instance nod2_bay5 location +set instance usl_bay0 location +set instance usl_bay1 location +set instance usl_bay2 location +set instance usl_bay3 location +set instance usl_bay4 location +set instance usl_bay5 location +set instance usl_bay6 location +set instance usl_bay7 location +set instance berth1 location +set instance berth2 location +set instance bumble robot +set instance honey 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 bumble o1 usl_bay3) (completed-panorama bumble o2 usl_bay2) (completed-panorama bumble o3 usl_bay1) (robot-at bumble berth1) (completed-panorama honey o1 usl_bay6) (completed-panorama honey o2 usl_bay5) (completed-panorama honey o3 usl_bay4) (completed-stereo honey o4 usl_bay1 usl_bay6) (robot-at honey berth2)) +set predicate (move-connected jem_bay0 jem_bay1) +set predicate (move-connected jem_bay1 jem_bay0) +set predicate (move-connected jem_bay1 jem_bay2) +set predicate (move-connected jem_bay2 jem_bay1) +set predicate (move-connected jem_bay2 jem_bay3) +set predicate (move-connected jem_bay3 jem_bay2) +set predicate (move-connected jem_bay3 jem_bay4) +set predicate (move-connected jem_bay4 jem_bay3) +set predicate (move-connected jem_bay4 jem_bay5) +set predicate (move-connected jem_bay5 jem_bay4) +set predicate (move-connected jem_bay5 jem_bay6) +set predicate (move-connected jem_bay6 jem_bay5) +set predicate (move-connected jem_bay6 jem_bay7) +set predicate (move-connected jem_bay7 jem_bay6) +set predicate (move-connected jem_bay7 jem_bay8) +set predicate (move-connected jem_bay8 jem_bay7) +set predicate (move-connected jem_bay0 nod2_bay1) +set predicate (move-connected nod2_bay1 jem_bay0) +set predicate (move-connected nod2_bay1 nod2_bay2) +set predicate (move-connected nod2_bay2 nod2_bay1) +set predicate (move-connected nod2_bay2 nod2_bay3) +set predicate (move-connected nod2_bay3 nod2_bay2) +set predicate (move-connected nod2_bay3 nod2_bay4) +set predicate (move-connected nod2_bay4 nod2_bay3) +set predicate (move-connected nod2_bay4 nod2_bay5) +set predicate (move-connected nod2_bay5 nod2_bay4) +set predicate (move-connected nod2_bay5 usl_bay0) +set predicate (move-connected usl_bay0 nod2_bay5) +set predicate (move-connected usl_bay0 usl_bay1) +set predicate (move-connected usl_bay1 usl_bay0) +set predicate (move-connected usl_bay1 usl_bay2) +set predicate (move-connected usl_bay2 usl_bay1) +set predicate (move-connected usl_bay2 usl_bay3) +set predicate (move-connected usl_bay3 usl_bay2) +set predicate (move-connected usl_bay3 usl_bay4) +set predicate (move-connected usl_bay4 usl_bay3) +set predicate (move-connected usl_bay4 usl_bay5) +set predicate (move-connected usl_bay5 usl_bay4) +set predicate (move-connected usl_bay5 usl_bay6) +set predicate (move-connected usl_bay6 usl_bay5) +set predicate (move-connected usl_bay6 usl_bay7) +set predicate (move-connected usl_bay7 usl_bay6) +set predicate (location-real jem_bay0) +set predicate (location-real jem_bay1) +set predicate (location-real jem_bay2) +set predicate (location-real jem_bay3) +set predicate (location-real jem_bay4) +set predicate (location-real jem_bay5) +set predicate (location-real jem_bay6) +set predicate (location-real jem_bay7) +set predicate (location-real nod2_bay1) +set predicate (location-real nod2_bay2) +set predicate (location-real nod2_bay3) +set predicate (location-real nod2_bay4) +set predicate (location-real nod2_bay5) +set predicate (location-real usl_bay0) +set predicate (location-real usl_bay1) +set predicate (location-real usl_bay2) +set predicate (location-real usl_bay3) +set predicate (location-real usl_bay4) +set predicate (location-real usl_bay5) +set predicate (location-real usl_bay6) +set predicate (dock-connected jem_bay7 berth1) +set predicate (dock-connected jem_bay7 berth2) +set predicate (robots-different bumble honey) +set predicate (robots-different honey bumble) +set predicate (locations-different jem_bay0 jem_bay1) +set predicate (locations-different jem_bay0 jem_bay2) +set predicate (locations-different jem_bay0 jem_bay3) +set predicate (locations-different jem_bay0 jem_bay4) +set predicate (locations-different jem_bay0 jem_bay5) +set predicate (locations-different jem_bay0 jem_bay6) +set predicate (locations-different jem_bay0 jem_bay7) +set predicate (locations-different jem_bay0 jem_bay8) +set predicate (locations-different jem_bay0 nod2_bay1) +set predicate (locations-different jem_bay0 nod2_bay2) +set predicate (locations-different jem_bay0 nod2_bay3) +set predicate (locations-different jem_bay0 nod2_bay4) +set predicate (locations-different jem_bay0 nod2_bay5) +set predicate (locations-different jem_bay0 usl_bay0) +set predicate (locations-different jem_bay0 usl_bay1) +set predicate (locations-different jem_bay0 usl_bay2) +set predicate (locations-different jem_bay0 usl_bay3) +set predicate (locations-different jem_bay0 usl_bay4) +set predicate (locations-different jem_bay0 usl_bay5) +set predicate (locations-different jem_bay0 usl_bay6) +set predicate (locations-different jem_bay0 usl_bay7) +set predicate (locations-different jem_bay1 jem_bay0) +set predicate (locations-different jem_bay1 jem_bay2) +set predicate (locations-different jem_bay1 jem_bay3) +set predicate (locations-different jem_bay1 jem_bay4) +set predicate (locations-different jem_bay1 jem_bay5) +set predicate (locations-different jem_bay1 jem_bay6) +set predicate (locations-different jem_bay1 jem_bay7) +set predicate (locations-different jem_bay1 jem_bay8) +set predicate (locations-different jem_bay1 nod2_bay1) +set predicate (locations-different jem_bay1 nod2_bay2) +set predicate (locations-different jem_bay1 nod2_bay3) +set predicate (locations-different jem_bay1 nod2_bay4) +set predicate (locations-different jem_bay1 nod2_bay5) +set predicate (locations-different jem_bay1 usl_bay0) +set predicate (locations-different jem_bay1 usl_bay1) +set predicate (locations-different jem_bay1 usl_bay2) +set predicate (locations-different jem_bay1 usl_bay3) +set predicate (locations-different jem_bay1 usl_bay4) +set predicate (locations-different jem_bay1 usl_bay5) +set predicate (locations-different jem_bay1 usl_bay6) +set predicate (locations-different jem_bay1 usl_bay7) +set predicate (locations-different jem_bay2 jem_bay0) +set predicate (locations-different jem_bay2 jem_bay1) +set predicate (locations-different jem_bay2 jem_bay3) +set predicate (locations-different jem_bay2 jem_bay4) +set predicate (locations-different jem_bay2 jem_bay5) +set predicate (locations-different jem_bay2 jem_bay6) +set predicate (locations-different jem_bay2 jem_bay7) +set predicate (locations-different jem_bay2 jem_bay8) +set predicate (locations-different jem_bay2 nod2_bay1) +set predicate (locations-different jem_bay2 nod2_bay2) +set predicate (locations-different jem_bay2 nod2_bay3) +set predicate (locations-different jem_bay2 nod2_bay4) +set predicate (locations-different jem_bay2 nod2_bay5) +set predicate (locations-different jem_bay2 usl_bay0) +set predicate (locations-different jem_bay2 usl_bay1) +set predicate (locations-different jem_bay2 usl_bay2) +set predicate (locations-different jem_bay2 usl_bay3) +set predicate (locations-different jem_bay2 usl_bay4) +set predicate (locations-different jem_bay2 usl_bay5) +set predicate (locations-different jem_bay2 usl_bay6) +set predicate (locations-different jem_bay2 usl_bay7) +set predicate (locations-different jem_bay3 jem_bay0) +set predicate (locations-different jem_bay3 jem_bay1) +set predicate (locations-different jem_bay3 jem_bay2) +set predicate (locations-different jem_bay3 jem_bay4) +set predicate (locations-different jem_bay3 jem_bay5) +set predicate (locations-different jem_bay3 jem_bay6) +set predicate (locations-different jem_bay3 jem_bay7) +set predicate (locations-different jem_bay3 jem_bay8) +set predicate (locations-different jem_bay3 nod2_bay1) +set predicate (locations-different jem_bay3 nod2_bay2) +set predicate (locations-different jem_bay3 nod2_bay3) +set predicate (locations-different jem_bay3 nod2_bay4) +set predicate (locations-different jem_bay3 nod2_bay5) +set predicate (locations-different jem_bay3 usl_bay0) +set predicate (locations-different jem_bay3 usl_bay1) +set predicate (locations-different jem_bay3 usl_bay2) +set predicate (locations-different jem_bay3 usl_bay3) +set predicate (locations-different jem_bay3 usl_bay4) +set predicate (locations-different jem_bay3 usl_bay5) +set predicate (locations-different jem_bay3 usl_bay6) +set predicate (locations-different jem_bay3 usl_bay7) +set predicate (locations-different jem_bay4 jem_bay0) +set predicate (locations-different jem_bay4 jem_bay1) +set predicate (locations-different jem_bay4 jem_bay2) +set predicate (locations-different jem_bay4 jem_bay3) +set predicate (locations-different jem_bay4 jem_bay5) +set predicate (locations-different jem_bay4 jem_bay6) +set predicate (locations-different jem_bay4 jem_bay7) +set predicate (locations-different jem_bay4 jem_bay8) +set predicate (locations-different jem_bay4 nod2_bay1) +set predicate (locations-different jem_bay4 nod2_bay2) +set predicate (locations-different jem_bay4 nod2_bay3) +set predicate (locations-different jem_bay4 nod2_bay4) +set predicate (locations-different jem_bay4 nod2_bay5) +set predicate (locations-different jem_bay4 usl_bay0) +set predicate (locations-different jem_bay4 usl_bay1) +set predicate (locations-different jem_bay4 usl_bay2) +set predicate (locations-different jem_bay4 usl_bay3) +set predicate (locations-different jem_bay4 usl_bay4) +set predicate (locations-different jem_bay4 usl_bay5) +set predicate (locations-different jem_bay4 usl_bay6) +set predicate (locations-different jem_bay4 usl_bay7) +set predicate (locations-different jem_bay5 jem_bay0) +set predicate (locations-different jem_bay5 jem_bay1) +set predicate (locations-different jem_bay5 jem_bay2) +set predicate (locations-different jem_bay5 jem_bay3) +set predicate (locations-different jem_bay5 jem_bay4) +set predicate (locations-different jem_bay5 jem_bay6) +set predicate (locations-different jem_bay5 jem_bay7) +set predicate (locations-different jem_bay5 jem_bay8) +set predicate (locations-different jem_bay5 nod2_bay1) +set predicate (locations-different jem_bay5 nod2_bay2) +set predicate (locations-different jem_bay5 nod2_bay3) +set predicate (locations-different jem_bay5 nod2_bay4) +set predicate (locations-different jem_bay5 nod2_bay5) +set predicate (locations-different jem_bay5 usl_bay0) +set predicate (locations-different jem_bay5 usl_bay1) +set predicate (locations-different jem_bay5 usl_bay2) +set predicate (locations-different jem_bay5 usl_bay3) +set predicate (locations-different jem_bay5 usl_bay4) +set predicate (locations-different jem_bay5 usl_bay5) +set predicate (locations-different jem_bay5 usl_bay6) +set predicate (locations-different jem_bay5 usl_bay7) +set predicate (locations-different jem_bay6 jem_bay0) +set predicate (locations-different jem_bay6 jem_bay1) +set predicate (locations-different jem_bay6 jem_bay2) +set predicate (locations-different jem_bay6 jem_bay3) +set predicate (locations-different jem_bay6 jem_bay4) +set predicate (locations-different jem_bay6 jem_bay5) +set predicate (locations-different jem_bay6 jem_bay7) +set predicate (locations-different jem_bay6 jem_bay8) +set predicate (locations-different jem_bay6 nod2_bay1) +set predicate (locations-different jem_bay6 nod2_bay2) +set predicate (locations-different jem_bay6 nod2_bay3) +set predicate (locations-different jem_bay6 nod2_bay4) +set predicate (locations-different jem_bay6 nod2_bay5) +set predicate (locations-different jem_bay6 usl_bay0) +set predicate (locations-different jem_bay6 usl_bay1) +set predicate (locations-different jem_bay6 usl_bay2) +set predicate (locations-different jem_bay6 usl_bay3) +set predicate (locations-different jem_bay6 usl_bay4) +set predicate (locations-different jem_bay6 usl_bay5) +set predicate (locations-different jem_bay6 usl_bay6) +set predicate (locations-different jem_bay6 usl_bay7) +set predicate (locations-different jem_bay7 jem_bay0) +set predicate (locations-different jem_bay7 jem_bay1) +set predicate (locations-different jem_bay7 jem_bay2) +set predicate (locations-different jem_bay7 jem_bay3) +set predicate (locations-different jem_bay7 jem_bay4) +set predicate (locations-different jem_bay7 jem_bay5) +set predicate (locations-different jem_bay7 jem_bay6) +set predicate (locations-different jem_bay7 jem_bay8) +set predicate (locations-different jem_bay7 nod2_bay1) +set predicate (locations-different jem_bay7 nod2_bay2) +set predicate (locations-different jem_bay7 nod2_bay3) +set predicate (locations-different jem_bay7 nod2_bay4) +set predicate (locations-different jem_bay7 nod2_bay5) +set predicate (locations-different jem_bay7 usl_bay0) +set predicate (locations-different jem_bay7 usl_bay1) +set predicate (locations-different jem_bay7 usl_bay2) +set predicate (locations-different jem_bay7 usl_bay3) +set predicate (locations-different jem_bay7 usl_bay4) +set predicate (locations-different jem_bay7 usl_bay5) +set predicate (locations-different jem_bay7 usl_bay6) +set predicate (locations-different jem_bay7 usl_bay7) +set predicate (locations-different jem_bay8 jem_bay0) +set predicate (locations-different jem_bay8 jem_bay1) +set predicate (locations-different jem_bay8 jem_bay2) +set predicate (locations-different jem_bay8 jem_bay3) +set predicate (locations-different jem_bay8 jem_bay4) +set predicate (locations-different jem_bay8 jem_bay5) +set predicate (locations-different jem_bay8 jem_bay6) +set predicate (locations-different jem_bay8 jem_bay7) +set predicate (locations-different jem_bay8 nod2_bay1) +set predicate (locations-different jem_bay8 nod2_bay2) +set predicate (locations-different jem_bay8 nod2_bay3) +set predicate (locations-different jem_bay8 nod2_bay4) +set predicate (locations-different jem_bay8 nod2_bay5) +set predicate (locations-different jem_bay8 usl_bay0) +set predicate (locations-different jem_bay8 usl_bay1) +set predicate (locations-different jem_bay8 usl_bay2) +set predicate (locations-different jem_bay8 usl_bay3) +set predicate (locations-different jem_bay8 usl_bay4) +set predicate (locations-different jem_bay8 usl_bay5) +set predicate (locations-different jem_bay8 usl_bay6) +set predicate (locations-different jem_bay8 usl_bay7) +set predicate (locations-different nod2_bay1 jem_bay0) +set predicate (locations-different nod2_bay1 jem_bay1) +set predicate (locations-different nod2_bay1 jem_bay2) +set predicate (locations-different nod2_bay1 jem_bay3) +set predicate (locations-different nod2_bay1 jem_bay4) +set predicate (locations-different nod2_bay1 jem_bay5) +set predicate (locations-different nod2_bay1 jem_bay6) +set predicate (locations-different nod2_bay1 jem_bay7) +set predicate (locations-different nod2_bay1 jem_bay8) +set predicate (locations-different nod2_bay1 nod2_bay2) +set predicate (locations-different nod2_bay1 nod2_bay3) +set predicate (locations-different nod2_bay1 nod2_bay4) +set predicate (locations-different nod2_bay1 nod2_bay5) +set predicate (locations-different nod2_bay1 usl_bay0) +set predicate (locations-different nod2_bay1 usl_bay1) +set predicate (locations-different nod2_bay1 usl_bay2) +set predicate (locations-different nod2_bay1 usl_bay3) +set predicate (locations-different nod2_bay1 usl_bay4) +set predicate (locations-different nod2_bay1 usl_bay5) +set predicate (locations-different nod2_bay1 usl_bay6) +set predicate (locations-different nod2_bay1 usl_bay7) +set predicate (locations-different nod2_bay2 jem_bay0) +set predicate (locations-different nod2_bay2 jem_bay1) +set predicate (locations-different nod2_bay2 jem_bay2) +set predicate (locations-different nod2_bay2 jem_bay3) +set predicate (locations-different nod2_bay2 jem_bay4) +set predicate (locations-different nod2_bay2 jem_bay5) +set predicate (locations-different nod2_bay2 jem_bay6) +set predicate (locations-different nod2_bay2 jem_bay7) +set predicate (locations-different nod2_bay2 jem_bay8) +set predicate (locations-different nod2_bay2 nod2_bay1) +set predicate (locations-different nod2_bay2 nod2_bay3) +set predicate (locations-different nod2_bay2 nod2_bay4) +set predicate (locations-different nod2_bay2 nod2_bay5) +set predicate (locations-different nod2_bay2 usl_bay0) +set predicate (locations-different nod2_bay2 usl_bay1) +set predicate (locations-different nod2_bay2 usl_bay2) +set predicate (locations-different nod2_bay2 usl_bay3) +set predicate (locations-different nod2_bay2 usl_bay4) +set predicate (locations-different nod2_bay2 usl_bay5) +set predicate (locations-different nod2_bay2 usl_bay6) +set predicate (locations-different nod2_bay2 usl_bay7) +set predicate (locations-different nod2_bay3 jem_bay0) +set predicate (locations-different nod2_bay3 jem_bay1) +set predicate (locations-different nod2_bay3 jem_bay2) +set predicate (locations-different nod2_bay3 jem_bay3) +set predicate (locations-different nod2_bay3 jem_bay4) +set predicate (locations-different nod2_bay3 jem_bay5) +set predicate (locations-different nod2_bay3 jem_bay6) +set predicate (locations-different nod2_bay3 jem_bay7) +set predicate (locations-different nod2_bay3 jem_bay8) +set predicate (locations-different nod2_bay3 nod2_bay1) +set predicate (locations-different nod2_bay3 nod2_bay2) +set predicate (locations-different nod2_bay3 nod2_bay4) +set predicate (locations-different nod2_bay3 nod2_bay5) +set predicate (locations-different nod2_bay3 usl_bay0) +set predicate (locations-different nod2_bay3 usl_bay1) +set predicate (locations-different nod2_bay3 usl_bay2) +set predicate (locations-different nod2_bay3 usl_bay3) +set predicate (locations-different nod2_bay3 usl_bay4) +set predicate (locations-different nod2_bay3 usl_bay5) +set predicate (locations-different nod2_bay3 usl_bay6) +set predicate (locations-different nod2_bay3 usl_bay7) +set predicate (locations-different nod2_bay4 jem_bay0) +set predicate (locations-different nod2_bay4 jem_bay1) +set predicate (locations-different nod2_bay4 jem_bay2) +set predicate (locations-different nod2_bay4 jem_bay3) +set predicate (locations-different nod2_bay4 jem_bay4) +set predicate (locations-different nod2_bay4 jem_bay5) +set predicate (locations-different nod2_bay4 jem_bay6) +set predicate (locations-different nod2_bay4 jem_bay7) +set predicate (locations-different nod2_bay4 jem_bay8) +set predicate (locations-different nod2_bay4 nod2_bay1) +set predicate (locations-different nod2_bay4 nod2_bay2) +set predicate (locations-different nod2_bay4 nod2_bay3) +set predicate (locations-different nod2_bay4 nod2_bay5) +set predicate (locations-different nod2_bay4 usl_bay0) +set predicate (locations-different nod2_bay4 usl_bay1) +set predicate (locations-different nod2_bay4 usl_bay2) +set predicate (locations-different nod2_bay4 usl_bay3) +set predicate (locations-different nod2_bay4 usl_bay4) +set predicate (locations-different nod2_bay4 usl_bay5) +set predicate (locations-different nod2_bay4 usl_bay6) +set predicate (locations-different nod2_bay4 usl_bay7) +set predicate (locations-different nod2_bay5 jem_bay0) +set predicate (locations-different nod2_bay5 jem_bay1) +set predicate (locations-different nod2_bay5 jem_bay2) +set predicate (locations-different nod2_bay5 jem_bay3) +set predicate (locations-different nod2_bay5 jem_bay4) +set predicate (locations-different nod2_bay5 jem_bay5) +set predicate (locations-different nod2_bay5 jem_bay6) +set predicate (locations-different nod2_bay5 jem_bay7) +set predicate (locations-different nod2_bay5 jem_bay8) +set predicate (locations-different nod2_bay5 nod2_bay1) +set predicate (locations-different nod2_bay5 nod2_bay2) +set predicate (locations-different nod2_bay5 nod2_bay3) +set predicate (locations-different nod2_bay5 nod2_bay4) +set predicate (locations-different nod2_bay5 usl_bay0) +set predicate (locations-different nod2_bay5 usl_bay1) +set predicate (locations-different nod2_bay5 usl_bay2) +set predicate (locations-different nod2_bay5 usl_bay3) +set predicate (locations-different nod2_bay5 usl_bay4) +set predicate (locations-different nod2_bay5 usl_bay5) +set predicate (locations-different nod2_bay5 usl_bay6) +set predicate (locations-different nod2_bay5 usl_bay7) +set predicate (locations-different usl_bay0 jem_bay0) +set predicate (locations-different usl_bay0 jem_bay1) +set predicate (locations-different usl_bay0 jem_bay2) +set predicate (locations-different usl_bay0 jem_bay3) +set predicate (locations-different usl_bay0 jem_bay4) +set predicate (locations-different usl_bay0 jem_bay5) +set predicate (locations-different usl_bay0 jem_bay6) +set predicate (locations-different usl_bay0 jem_bay7) +set predicate (locations-different usl_bay0 jem_bay8) +set predicate (locations-different usl_bay0 nod2_bay1) +set predicate (locations-different usl_bay0 nod2_bay2) +set predicate (locations-different usl_bay0 nod2_bay3) +set predicate (locations-different usl_bay0 nod2_bay4) +set predicate (locations-different usl_bay0 nod2_bay5) +set predicate (locations-different usl_bay0 usl_bay1) +set predicate (locations-different usl_bay0 usl_bay2) +set predicate (locations-different usl_bay0 usl_bay3) +set predicate (locations-different usl_bay0 usl_bay4) +set predicate (locations-different usl_bay0 usl_bay5) +set predicate (locations-different usl_bay0 usl_bay6) +set predicate (locations-different usl_bay0 usl_bay7) +set predicate (locations-different usl_bay1 jem_bay0) +set predicate (locations-different usl_bay1 jem_bay1) +set predicate (locations-different usl_bay1 jem_bay2) +set predicate (locations-different usl_bay1 jem_bay3) +set predicate (locations-different usl_bay1 jem_bay4) +set predicate (locations-different usl_bay1 jem_bay5) +set predicate (locations-different usl_bay1 jem_bay6) +set predicate (locations-different usl_bay1 jem_bay7) +set predicate (locations-different usl_bay1 jem_bay8) +set predicate (locations-different usl_bay1 nod2_bay1) +set predicate (locations-different usl_bay1 nod2_bay2) +set predicate (locations-different usl_bay1 nod2_bay3) +set predicate (locations-different usl_bay1 nod2_bay4) +set predicate (locations-different usl_bay1 nod2_bay5) +set predicate (locations-different usl_bay1 usl_bay0) +set predicate (locations-different usl_bay1 usl_bay2) +set predicate (locations-different usl_bay1 usl_bay3) +set predicate (locations-different usl_bay1 usl_bay4) +set predicate (locations-different usl_bay1 usl_bay5) +set predicate (locations-different usl_bay1 usl_bay6) +set predicate (locations-different usl_bay1 usl_bay7) +set predicate (locations-different usl_bay2 jem_bay0) +set predicate (locations-different usl_bay2 jem_bay1) +set predicate (locations-different usl_bay2 jem_bay2) +set predicate (locations-different usl_bay2 jem_bay3) +set predicate (locations-different usl_bay2 jem_bay4) +set predicate (locations-different usl_bay2 jem_bay5) +set predicate (locations-different usl_bay2 jem_bay6) +set predicate (locations-different usl_bay2 jem_bay7) +set predicate (locations-different usl_bay2 jem_bay8) +set predicate (locations-different usl_bay2 nod2_bay1) +set predicate (locations-different usl_bay2 nod2_bay2) +set predicate (locations-different usl_bay2 nod2_bay3) +set predicate (locations-different usl_bay2 nod2_bay4) +set predicate (locations-different usl_bay2 nod2_bay5) +set predicate (locations-different usl_bay2 usl_bay0) +set predicate (locations-different usl_bay2 usl_bay1) +set predicate (locations-different usl_bay2 usl_bay3) +set predicate (locations-different usl_bay2 usl_bay4) +set predicate (locations-different usl_bay2 usl_bay5) +set predicate (locations-different usl_bay2 usl_bay6) +set predicate (locations-different usl_bay2 usl_bay7) +set predicate (locations-different usl_bay3 jem_bay0) +set predicate (locations-different usl_bay3 jem_bay1) +set predicate (locations-different usl_bay3 jem_bay2) +set predicate (locations-different usl_bay3 jem_bay3) +set predicate (locations-different usl_bay3 jem_bay4) +set predicate (locations-different usl_bay3 jem_bay5) +set predicate (locations-different usl_bay3 jem_bay6) +set predicate (locations-different usl_bay3 jem_bay7) +set predicate (locations-different usl_bay3 jem_bay8) +set predicate (locations-different usl_bay3 nod2_bay1) +set predicate (locations-different usl_bay3 nod2_bay2) +set predicate (locations-different usl_bay3 nod2_bay3) +set predicate (locations-different usl_bay3 nod2_bay4) +set predicate (locations-different usl_bay3 nod2_bay5) +set predicate (locations-different usl_bay3 usl_bay0) +set predicate (locations-different usl_bay3 usl_bay1) +set predicate (locations-different usl_bay3 usl_bay2) +set predicate (locations-different usl_bay3 usl_bay4) +set predicate (locations-different usl_bay3 usl_bay5) +set predicate (locations-different usl_bay3 usl_bay6) +set predicate (locations-different usl_bay3 usl_bay7) +set predicate (locations-different usl_bay4 jem_bay0) +set predicate (locations-different usl_bay4 jem_bay1) +set predicate (locations-different usl_bay4 jem_bay2) +set predicate (locations-different usl_bay4 jem_bay3) +set predicate (locations-different usl_bay4 jem_bay4) +set predicate (locations-different usl_bay4 jem_bay5) +set predicate (locations-different usl_bay4 jem_bay6) +set predicate (locations-different usl_bay4 jem_bay7) +set predicate (locations-different usl_bay4 jem_bay8) +set predicate (locations-different usl_bay4 nod2_bay1) +set predicate (locations-different usl_bay4 nod2_bay2) +set predicate (locations-different usl_bay4 nod2_bay3) +set predicate (locations-different usl_bay4 nod2_bay4) +set predicate (locations-different usl_bay4 nod2_bay5) +set predicate (locations-different usl_bay4 usl_bay0) +set predicate (locations-different usl_bay4 usl_bay1) +set predicate (locations-different usl_bay4 usl_bay2) +set predicate (locations-different usl_bay4 usl_bay3) +set predicate (locations-different usl_bay4 usl_bay5) +set predicate (locations-different usl_bay4 usl_bay6) +set predicate (locations-different usl_bay4 usl_bay7) +set predicate (locations-different usl_bay5 jem_bay0) +set predicate (locations-different usl_bay5 jem_bay1) +set predicate (locations-different usl_bay5 jem_bay2) +set predicate (locations-different usl_bay5 jem_bay3) +set predicate (locations-different usl_bay5 jem_bay4) +set predicate (locations-different usl_bay5 jem_bay5) +set predicate (locations-different usl_bay5 jem_bay6) +set predicate (locations-different usl_bay5 jem_bay7) +set predicate (locations-different usl_bay5 jem_bay8) +set predicate (locations-different usl_bay5 nod2_bay1) +set predicate (locations-different usl_bay5 nod2_bay2) +set predicate (locations-different usl_bay5 nod2_bay3) +set predicate (locations-different usl_bay5 nod2_bay4) +set predicate (locations-different usl_bay5 nod2_bay5) +set predicate (locations-different usl_bay5 usl_bay0) +set predicate (locations-different usl_bay5 usl_bay1) +set predicate (locations-different usl_bay5 usl_bay2) +set predicate (locations-different usl_bay5 usl_bay3) +set predicate (locations-different usl_bay5 usl_bay4) +set predicate (locations-different usl_bay5 usl_bay6) +set predicate (locations-different usl_bay5 usl_bay7) +set predicate (locations-different usl_bay6 jem_bay0) +set predicate (locations-different usl_bay6 jem_bay1) +set predicate (locations-different usl_bay6 jem_bay2) +set predicate (locations-different usl_bay6 jem_bay3) +set predicate (locations-different usl_bay6 jem_bay4) +set predicate (locations-different usl_bay6 jem_bay5) +set predicate (locations-different usl_bay6 jem_bay6) +set predicate (locations-different usl_bay6 jem_bay7) +set predicate (locations-different usl_bay6 jem_bay8) +set predicate (locations-different usl_bay6 nod2_bay1) +set predicate (locations-different usl_bay6 nod2_bay2) +set predicate (locations-different usl_bay6 nod2_bay3) +set predicate (locations-different usl_bay6 nod2_bay4) +set predicate (locations-different usl_bay6 nod2_bay5) +set predicate (locations-different usl_bay6 usl_bay0) +set predicate (locations-different usl_bay6 usl_bay1) +set predicate (locations-different usl_bay6 usl_bay2) +set predicate (locations-different usl_bay6 usl_bay3) +set predicate (locations-different usl_bay6 usl_bay4) +set predicate (locations-different usl_bay6 usl_bay5) +set predicate (locations-different usl_bay6 usl_bay7) +set predicate (locations-different usl_bay7 jem_bay0) +set predicate (locations-different usl_bay7 jem_bay1) +set predicate (locations-different usl_bay7 jem_bay2) +set predicate (locations-different usl_bay7 jem_bay3) +set predicate (locations-different usl_bay7 jem_bay4) +set predicate (locations-different usl_bay7 jem_bay5) +set predicate (locations-different usl_bay7 jem_bay6) +set predicate (locations-different usl_bay7 jem_bay7) +set predicate (locations-different usl_bay7 jem_bay8) +set predicate (locations-different usl_bay7 nod2_bay1) +set predicate (locations-different usl_bay7 nod2_bay2) +set predicate (locations-different usl_bay7 nod2_bay3) +set predicate (locations-different usl_bay7 nod2_bay4) +set predicate (locations-different usl_bay7 nod2_bay5) +set predicate (locations-different usl_bay7 usl_bay0) +set predicate (locations-different usl_bay7 usl_bay1) +set predicate (locations-different usl_bay7 usl_bay2) +set predicate (locations-different usl_bay7 usl_bay3) +set predicate (locations-different usl_bay7 usl_bay4) +set predicate (locations-different usl_bay7 usl_bay5) +set predicate (locations-different usl_bay7 usl_bay6) +set predicate (robot-available bumble) +set predicate (robot-available honey) +set predicate (robot-at bumble usl_bay1) +set predicate (robot-at honey usl_bay6) +set predicate (location-available berth1) +set predicate (location-available berth2) +set predicate (location-available jem_bay0) +set predicate (location-available jem_bay1) +set predicate (location-available jem_bay2) +set predicate (location-available jem_bay3) +set predicate (location-available jem_bay4) +set predicate (location-available jem_bay5) +set predicate (location-available jem_bay6) +set predicate (location-available jem_bay7) +set predicate (location-available jem_bay8) +set predicate (location-available nod2_bay1) +set predicate (location-available nod2_bay2) +set predicate (location-available nod2_bay3) +set predicate (location-available nod2_bay4) +set predicate (location-available nod2_bay5) +set predicate (location-available usl_bay0) +set predicate (location-available usl_bay2) +set predicate (location-available usl_bay3) +set predicate (location-available usl_bay4) +set predicate (location-available usl_bay5) +set predicate (location-available usl_bay7) +set predicate (need-stereo honey o4 usl_bay1 usl_bay6) +set function (= (order-identity o0) 0) +set function (= (order-identity o1) 1) +set function (= (order-identity o2) 2) +set function (= (order-identity o3) 3) +set function (= (order-identity o4) 4) +set function (= (robot-order bumble) -1) +set function (= (robot-order honey) -1) 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 3bde26cd..4da351b9 100755 --- a/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py +++ b/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py @@ -71,22 +71,22 @@ def first_non_zero(a: int, b: int) -> int: 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": + if bay_origin == "nod2_bay1" and bay_destination == "jem_bay0": loginfo("CHANGING EXPOSURE TO JEM") return config_static["exposure"]["jem"] # Going to NOD2 if ( - bay_origin == "jem_hatch_to_nod2" - and bay_destination == "nod2_hatch_from_jem" - or bay_origin == "usl_hatch_to_nod2" - and bay_destination == "nod2_hatch_from_usl" + bay_origin == "jem_bay0" + and bay_destination == "nod2_bay1" + or bay_origin == "usl_bay0" + and bay_destination == "nod2_bay5" ): loginfo("CHANGING EXPOSURE TO NOD2") return config_static["exposure"]["nod2"] # Going to USL - if bay_origin == "nod2_hatch_to_usl" and bay_destination == "usl_hatch_from_nod2": + if bay_origin == "nod2_bay5" and bay_destination == "usl_bay0": return config_static["exposure"]["usl"] return 0 @@ -94,20 +94,20 @@ def exposure_change(config_static, bay_origin, bay_destination): def map_change(config_static, bay_origin, bay_destination): # Going to JEM - if bay_origin == "nod2_hatch_to_jem" and bay_destination == "jem_hatch_from_nod2": + if bay_origin == "nod2_bay1" and bay_destination == "jem_bay0": loginfo("CHANGING MAP TO JEM") return config_static["maps"]["jem"] # Going to NOD2 if ( - bay_origin == "jem_hatch_to_nod2" - and bay_destination == "nod2_hatch_from_jem" - or bay_origin == "usl_hatch_to_nod2" - and bay_destination == "nod2_hatch_from_usl" + bay_origin == "jem_bay0" + and bay_destination == "nod2_bay1" + or bay_origin == "usl_bay0" + and bay_destination == "nod2_bay5" ): loginfo("CHANGING MAP TO NOD2") return config_static["maps"]["nod2"] # Going to USL - if bay_origin == "nod2_hatch_to_usl" and bay_destination == "usl_hatch_from_nod2": + if bay_origin == "nod2_bay5" and bay_destination == "usl_bay0": return config_static["maps"]["usl"] return "" @@ -423,10 +423,38 @@ 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: str): - self.ns = ns + def __init__(self, robot: str): + self.robot = robot + self.ns = None # Initialize ns, will be set later + + def initialize(self): + + # Figure out robot name and whether we are in simulation or hardware + current_robot = os.environ.get("ROBOTNAME") + if not current_robot: + loginfo( + "ROBOTNAME was not defined. Inferring robot from ROS topic /robot_name" + ) + # This is a latching messge so it shouldn't take long + try: + data = rospy.wait_for_message("/robot_name", String, timeout=5) + current_robot = data.data.lower() + except: + current_robot = "" + sim = True + + cmd_exec_ns = "" + # If we're commanding a robot remotely + if current_robot != self.robot: + loginfo( + f"We're commanding a namespaced robot! From '{current_robot}' to '{self.robot}'" + ) + # Command executor will add namespace for bridge forwarding + cmd_exec_ns = "/" + self.robot + + self.ns = cmd_exec_ns loginfo(f"command topic: {self.ns}/command") - # Declare guest science command publisher + self.sub_ack = rospy.Subscriber( self.ns + "/mgt/ack", AckStamped, self.ack_callback ) @@ -442,12 +470,21 @@ def __init__(self, ns: str): self.pub_command = rospy.Publisher( self.ns + "/command", CommandStamped, queue_size=5 ) + + wait_count = 0 while self.pub_command.get_num_connections() == 0 and not rospy.is_shutdown(): + wait_count += 1 + if wait_count > 10: + loginfo(f"Restarting initialization") + return False + loginfo( f"Waiting for an astrobee executive to subscribe to {self.ns}/command" ) rospy.sleep(1) + self.unique_cmd_id = "" + return True def start_recording(self, bag_description): # Arg is bagfile name description @@ -484,14 +521,44 @@ def stop_recording(self): return result def change_exposure(self, val): - # TBD + # Arg is exposure value + arg1 = CommandArg() + arg1.data_type = CommandArg.DATA_TYPE_FLOAT + arg1.s = val + + cmd = CommandStamped() + cmd.header = Header(stamp=rospy.Time.now()) + cmd.cmd_name = CommandConstants.CMD_NAME_SET_EXPOSURE + cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec()) + self.unique_cmd_id = cmd.cmd_id + cmd.cmd_src = "isaac fsw" + cmd.cmd_origin = "isaac fsw" + cmd.args = [arg1] + + # Publish the CommandStamped message loginfo("Change exposure to " + str(val)) - return 0 + result = self.publish_and_wait_response(cmd) + return result def change_map(self, map_name): - # TBD + # Arg is map name + arg1 = CommandArg() + arg1.data_type = CommandArg.DATA_TYPE_STRING + arg1.s = map_name + + cmd = CommandStamped() + cmd.header = Header(stamp=rospy.Time.now()) + cmd.cmd_name = CommandConstants.CMD_NAME_SET_MAP + cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec()) + self.unique_cmd_id = cmd.cmd_id + cmd.cmd_src = "isaac fsw" + cmd.cmd_origin = "isaac fsw" + cmd.args = [arg1] + + # Publish the CommandStamped message loginfo("Change map to " + map_name) - return 0 + result = self.publish_and_wait_response(cmd) + return result def ack_callback(self, msg): if self.ack_needed is True and msg.cmd_id == self.unique_cmd_id: @@ -636,32 +703,18 @@ def survey_manager_executor(args, run, config_static, process_executor, quick: b rospy.init_node("survey_namager_cmd_" + args["robot"], anonymous=True) sim = False - # Figure out robot name and whether we are in simulation or hardware - current_robot = os.environ.get("ROBOTNAME") - if not current_robot: - loginfo("ROBOTNAME was not defined. Inferring robot from ROS topic /robot_name") - # This is a latching messge so it shouldn't take long - try: - data = rospy.wait_for_message("/robot_name", String, timeout=5) - current_robot = data.data.lower() - except: - current_robot = "" - sim = True - - ns = [] - cmd_exec_ns = "" - # If we're commanding a robot remotely - if current_robot != args["robot"]: - loginfo( - f"We're commanding a namespaced robot! From '{current_robot}' to '{args['robot']}'" - ) - ns = ["-ns", args["robot"]] - # Command executor will add namespace for bridge forwarding - cmd_exec_ns = "/" + args["robot"] - command_executor = CommandExecutor(cmd_exec_ns) + command_executor = CommandExecutor(args["robot"]) + if not command_executor.initialize(): + return -1 + + ns = ( + ["-ns"] + command_executor.ns.lstrip("/").split("/") + if command_executor.ns + else [] + ) sm_exec = SurveyManagerExecutor( - process_executor, command_executor, config_static, ns, cmd_exec_ns + process_executor, command_executor, config_static, ns, command_executor.ns ) # Initialize exit code @@ -874,7 +927,7 @@ class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter): def main(): default_config_paths = [ os.path.join( - rospkg.RosPack().get_path("survey_manager"), "data/jem_survey_static.yaml" + rospkg.RosPack().get_path("survey_manager"), "data/iss_survey_static.yaml" ), os.path.join( rospkg.RosPack().get_path("survey_manager"), diff --git a/astrobee/survey/survey_manager/src/survey_manager/problem_generator.py b/astrobee/survey/survey_manager/src/survey_manager/problem_generator.py index 6d0e1f20..e601b03c 100755 --- a/astrobee/survey/survey_manager/src/survey_manager/problem_generator.py +++ b/astrobee/survey/survey_manager/src/survey_manager/problem_generator.py @@ -80,7 +80,7 @@ os.path.relpath(pathlib.Path(rospack.get_path("survey_manager")) / "pddl", CWD) ) DEFAULT_CONFIGS = [ - DATA_DIR / "jem_survey_static.yaml", + DATA_DIR / "iss_survey_static.yaml", DATA_DIR / "jem_survey_dynamic.yaml", ] @@ -479,8 +479,8 @@ def problem_generator( candidates = ( ("jem_bay7", "berth1"), ("jem_bay7", "berth2"), - ("gra_bay3", "berth1"), - ("gra_bay5", "berth2"), + ("gra_bay3", "berth1_g"), + ("gra_bay5", "berth2_g"), ) dock_connected_lines = [ f"(dock-connected {bay} {berth})"