diff --git a/astrobee/behaviors/inspection/resources/panorama_granite.txt b/astrobee/behaviors/inspection/resources/panorama_granite.txt
deleted file mode 100644
index 981a4f6a..00000000
--- a/astrobee/behaviors/inspection/resources/panorama_granite.txt
+++ /dev/null
@@ -1,2 +0,0 @@
-# Panorama
-0 0 -0.772 0 0 -90
\ No newline at end of file
diff --git a/astrobee/behaviors/inspection/resources/panorama_granite_bsharp.txt b/astrobee/behaviors/inspection/resources/panorama_granite_bsharp.txt
new file mode 100644
index 00000000..5aed3e6f
--- /dev/null
+++ b/astrobee/behaviors/inspection/resources/panorama_granite_bsharp.txt
@@ -0,0 +1,2 @@
+# Panorama
+0.1 0.6 -0.772 0 0 -90
\ No newline at end of file
diff --git a/astrobee/behaviors/inspection/resources/panorama_granite_wannabee.txt b/astrobee/behaviors/inspection/resources/panorama_granite_wannabee.txt
index 4bfd7d7c..e085c502 100644
--- a/astrobee/behaviors/inspection/resources/panorama_granite_wannabee.txt
+++ b/astrobee/behaviors/inspection/resources/panorama_granite_wannabee.txt
@@ -1,2 +1,2 @@
# Panorama
-0 0 -0.6 180 0 -90
\ No newline at end of file
+0.1 -0.3 -0.6 180 0 -90
\ No newline at end of file
diff --git a/astrobee/behaviors/inspection/src/inspection_node.cc b/astrobee/behaviors/inspection/src/inspection_node.cc
index 8b628483..3882f52b 100644
--- a/astrobee/behaviors/inspection/src/inspection_node.cc
+++ b/astrobee/behaviors/inspection/src/inspection_node.cc
@@ -227,7 +227,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
TOPIC_COMMAND, 1, true);
// Subscribe to the sci camera info topic to make sure a picture was taken
- sub_sci_cam_info_ = nh->subscribe("/hw/cam_sci_info", 1,
+ sub_sci_cam_info_ = nh->subscribe(TOPIC_HARDWARE_SCI_CAM_INFO, 1,
&InspectionNode::SciCamInfoCallback, this);
diff --git a/astrobee/survey_manager/survey_dependencies/ros2_planning_system b/astrobee/survey_manager/survey_dependencies/ros2_planning_system
index 35e23c8f..f826c000 160000
--- a/astrobee/survey_manager/survey_dependencies/ros2_planning_system
+++ b/astrobee/survey_manager/survey_dependencies/ros2_planning_system
@@ -1 +1 @@
-Subproject commit 35e23c8f022920dd47b9c478dc028f0e89e3d283
+Subproject commit f826c000fe14521664d4acb3b4f365fef5b36648
diff --git a/astrobee/survey_manager/survey_planner/data/granite_survey_dynamic.yaml b/astrobee/survey_manager/survey_planner/data/granite_survey_dynamic.yaml
new file mode 100644
index 00000000..65dc0846
--- /dev/null
+++ b/astrobee/survey_manager/survey_planner/data/granite_survey_dynamic.yaml
@@ -0,0 +1,49 @@
+# 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: bsharp, order: 0, location: gra_bay2}
+- {type: stereo, robot: bsharp, order: 1, trajectory: gra_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: bsharp, 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: wannabee, order: 1, location: gra_bay6}
+# This is the other objective we previously had to comment out for POPF to return a decent plan.
+- {type: stereo, robot: wannabee, order: 4, trajectory: gra_bay5_to_bay7}
+- {type: robot_at, robot: wannabee, location: berth2}
+
+init:
+ bsharp:
+ location: berth1
+ wannabee:
+ location: berth2
diff --git a/astrobee/survey_manager/survey_planner/data/granite_survey_static.yaml b/astrobee/survey_manager/survey_planner/data/granite_survey_static.yaml
new file mode 100644
index 00000000..6b84411f
--- /dev/null
+++ b/astrobee/survey_manager/survey_planner/data/granite_survey_static.yaml
@@ -0,0 +1,66 @@
+# 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.
+
+# Static configuration info used when generating a PDDL problem and also when executing actions in a
+# PDDL plan. This info should be static in the sense that it nominally doesn't change during an ISS
+# activity, so the survey manager doesn't have to modify it. However, an edge case is that an
+# operator might want to manually edit something in here (like add a new symbolic location or nudge
+# the position of a named bay away from an obstacle) and restart the survey manager. On the other
+# hand, info that is *expected* to change as part of the survey manager conops belongs in
+# jem_survey_dynamic.yaml.
+
+# Useful reference for positions and stereo survey trajectories:
+# https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure
+
+bays:
+ # 3D coordinates for symbolic bays in ISS Analysis Coordinate System used by Astrobee
+ gra_bay1: [0.1, 0.7, -0.68]
+ gra_bay2: [0.1, 0.6, -0.68]
+ gra_bay3: [0.1, 0.5, -0.68]
+ gra_bay4: [0.1, 0.3, -0.68]
+ gra_bay5: [0.1, 0.1, -0.68]
+ gra_bay6: [0.1, -0.3, -0.68]
+ gra_bay7: [0.1, -0.5, -0.68]
+
+bays_move:
+ gra_bay1: ["-pos", "0.1 0.7 -0.68"]
+ gra_bay2: ["-pos", "0.1 0.6 -0.68"]
+ gra_bay3: ["-pos", "0.1 0.5 -0.68"]
+ gra_bay4: ["-pos", "0.1 0.3 -0.68"]
+ 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"]
+
+
+bays_pano:
+ gra_bay2: "panorama_granite_bsharp.txt"
+ gra_bay6: "panorama_granite_wannabee.txt"
+
+bogus_bays: [gra_bay0, gra_bay8]
+berths: [berth1, berth2]
+robots: [bsharp, wannabee]
+
+stereo:
+ gra_bay1_to_bay3:
+ fplan: "startup"
+ base_location: gra_bay1
+ bound_location: gra_bay3
+ gra_bay5_to_bay7:
+ fplan: "startup"
+ base_location: gra_bay5
+ bound_location: gra_bay7
diff --git a/astrobee/survey_manager/survey_planner/data/survey_static.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml
similarity index 70%
rename from astrobee/survey_manager/survey_planner/data/survey_static.yaml
rename to astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml
index 00a362a4..ff240df3 100644
--- a/astrobee/survey_manager/survey_planner/data/survey_static.yaml
+++ b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml
@@ -38,36 +38,30 @@ bays:
jem_bay7: [11.0, -9.7, 4.8]
bays_move:
- jem_bay1: "-pos '11 -4 4.8'"
- jem_bay2: "-pos '11 -5 4.8'"
- jem_bay3: "-pos '11 -6 4.8'"
- jem_bay4: " -pos '11 -7 4.8'"
- jem_bay5: "-pos '11 -8 4.8'"
- jem_bay6: "-pos '11 -9 4.8'"
- jem_bay7: "-pos '11 -9.7 4.8'"
- jem_hatch_to_nod2: "-move -pos '11 -3.5 4.8' -att '0 0 1 90'"
- jem_hatch_from_nod2: "-move -pos '11 -3.5 4.8' -att '0 0 1 -90'"
- nod2_hatch_from_jem: "-move -pos '11 -1.0 4.8' -att '0 0 1 90'"
- nod2_hatch_to_jem: "-move -pos '11 -1.0 4.8' -att '0 0 1 -90'"
- nod2_bay2: "-pos '11 0 4.8'"
- nod2_bay3: "-pos '10 0 4.8'"
- nod2_bay4: "-pos '9 0 4.8'"
- nod2_hatch_to_usl: "-move -pos '7.8 -3.5 4.8' -att '0 0 1 180'"
- nod2_hatch_from_usl: "-move -pos '7.8 -3.5 4.8' -att '0 0 1 0'"
- usl_hatch_from_nod2: "-move -pos '5.3 -1.0 4.8' -att '0 0 1 180'"
- usl_hatch_to_nod2: "-move -pos '5.3 -1.0 4.8' -att '0 0 1 0'"
- usl_bay1: "-pos '4.7 0 4.8'"
- usl_bay2: "-pos '3.65 0 4.8'"
- usl_bay3: "-pos '2.6 0 4.8'"
- usl_bay4: " -pos '1.55 0 4.8'"
- usl_bay5: "-pos '0.5 0 4.8'"
- usl_bay6: "-pos '-0.5 0 4.8'"
-
- # Granite testing
- gra_p1_bsharp: "-move -pos '-0.3 -0.1 -0.68'"
- gra_p2_bsharp: "-move -pos '0.3 -0.1 -0.68'"
- gra_p1_wannabee: "-move -pos '-0.3 -0.1 -0.68' -att '1 0 0 180'"
- gra_p2_wannabee: "-move -pos '0.3 -0.1 -0.68' -att '1 0 0 180'"
+ jem_bay1: ["-pos", "11 -4 4.8", "-att", "0 0 0 1"]
+ jem_bay2: ["-pos", "11 -5 4.8", "-att", "0 0 0 1"]
+ jem_bay3: ["-pos", "11 -6 4.8", "-att", "0 0 0 1"]
+ jem_bay4: ["-pos", "11 -7 4.8", "-att", "0 0 0 1"]
+ jem_bay5: ["-pos", "11 -8 4.8", "-att", "0 0 0 1"]
+ jem_bay6: ["-pos", "11 -9 4.8", "-att", "0 0 0 1"]
+ jem_bay7: ["-pos", "11 -9.7 4.8", "-att", "0 0 0 1"]
+ jem_hatch_to_nod2: ["-pos", "11 -3.5 4.8", "-att", "0 0 1 90"]
+ jem_hatch_from_nod2: ["-pos", "11 -3.5 4.8", "-att", "0 0 1 -90"]
+ nod2_hatch_from_jem: ["-pos", "11 -1.0 4.8", "-att", "0 0 1 90"]
+ nod2_hatch_to_jem: ["-pos", "11 -1.0 4.8", "-att", "0 0 1 -90"]
+ 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", "0 0 1 180"]
+ nod2_hatch_from_usl: ["-pos", "7.8 -3.5 4.8", "-att", "0 0 1 0"]
+ usl_hatch_from_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "0 0 1 180"]
+ usl_hatch_to_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "0 0 1 0"]
+ 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"]
bays_pano:
jem_bay1: "isaac9/jem_bay1_std_panorama.txt"
@@ -76,7 +70,7 @@ bays_pano:
jem_bay4: "isaac9/jem_bay4_std_panorama.txt"
jem_bay5: "isaac9/jem_bay5_std_panorama.txt"
jem_bay6: "isaac9/jem_bay6_std_panorama.txt"
- jem_bay7: "isaac9/jem_bay7_safe_std_panorama.txt"
+ jem_bay7: "isaac9/jem_bay7_safe_panorama.txt"
nod2_bay2: "isaac10/nod2_bay2_std_panorama.txt"
nod2_bay3: "isaac10/nod2_bay3_std_panorama.txt"
nod2_bay4: "isaac10/nod2_bay4_std_panorama.txt"
@@ -87,10 +81,6 @@ bays_pano:
usl_bay5: "isaac11/usl_bay5_std_panorama.txt"
usl_bay6: "isaac11/usl_bay6_std_panorama.txt"
- # Granite testing
- gra_bsharp: "panorama_granite.txt"
- gra_wannabee: "panorama_granite_wannabee.txt"
-
maps:
jem: "iss.map"
nod2: "isaac.map"
@@ -123,9 +113,3 @@ stereo:
fplan: "ISAAC/jem_stereo_mapping_bay7_to_bay4"
base_location: jem_bay7
bound_location: jem_bay4
-
- # Granite testing
- gra_p1_to_gra_p2:
- fplan: "startup"
- base_location: gra_p1
- bound_location: gra_p2
\ No newline at end of file
diff --git a/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h b/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h
index bf28a343..025b9ae2 100644
--- a/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h
+++ b/astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h
@@ -29,6 +29,7 @@ namespace plansys2_actions {
class IsaacAction : public plansys2::ActionExecutorClient {
public:
IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate);
+ ~IsaacAction();
protected:
void do_work();
diff --git a/astrobee/survey_manager/survey_planner/launch/optic_survey.launch b/astrobee/survey_manager/survey_planner/launch/optic_survey.launch
deleted file mode 100644
index 19bb5f2f..00000000
--- a/astrobee/survey_manager/survey_planner/launch/optic_survey.launch
+++ /dev/null
@@ -1,55 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/astrobee/survey_manager/survey_planner/launch/survey.launch b/astrobee/survey_manager/survey_planner/launch/survey.launch
new file mode 100644
index 00000000..3a1dff55
--- /dev/null
+++ b/astrobee/survey_manager/survey_planner/launch/survey.launch
@@ -0,0 +1,76 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/astrobee/survey_manager/survey_planner/launch/trey_survey.launch b/astrobee/survey_manager/survey_planner/launch/trey_survey.launch
deleted file mode 100644
index 0e9f9bfd..00000000
--- a/astrobee/survey_manager/survey_planner/launch/trey_survey.launch
+++ /dev/null
@@ -1,55 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_granite_survey.ps2.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_granite_survey.ps2.pddl
new file mode 100644
index 00000000..a7807ff2
--- /dev/null
+++ b/astrobee/survey_manager/survey_planner/pddl/problem_granite_survey.ps2.pddl
@@ -0,0 +1,140 @@
+set instance gra_bay0 location
+set instance gra_bay1 location
+set instance gra_bay2 location
+set instance gra_bay3 location
+set instance gra_bay4 location
+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 bsharp robot
+set instance wannabee robot
+set instance o0 order
+set instance o1 order
+set instance o2 order
+set instance o3 order
+set instance o4 order
+set goal (and (completed-panorama bsharp o0 gra_bay2) (completed-stereo bsharp o1 gra_bay1 gra_bay3) (robot-at bsharp berth1) (completed-panorama wannabee o1 gra_bay6) (completed-stereo wannabee o4 gra_bay5 gra_bay7) (robot-at wannabee berth2))
+set predicate (move-connected gra_bay0 gra_bay1)
+set predicate (move-connected gra_bay1 gra_bay0)
+set predicate (move-connected gra_bay1 gra_bay2)
+set predicate (move-connected gra_bay2 gra_bay1)
+set predicate (move-connected gra_bay2 gra_bay3)
+set predicate (move-connected gra_bay3 gra_bay2)
+set predicate (move-connected gra_bay3 gra_bay4)
+set predicate (move-connected gra_bay4 gra_bay3)
+set predicate (move-connected gra_bay4 gra_bay5)
+set predicate (move-connected gra_bay5 gra_bay4)
+set predicate (move-connected gra_bay5 gra_bay6)
+set predicate (move-connected gra_bay6 gra_bay5)
+set predicate (move-connected gra_bay6 gra_bay7)
+set predicate (move-connected gra_bay7 gra_bay6)
+set predicate (move-connected gra_bay7 gra_bay8)
+set predicate (move-connected gra_bay8 gra_bay7)
+set predicate (location-real gra_bay1)
+set predicate (location-real gra_bay2)
+set predicate (location-real gra_bay3)
+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 (robots-different bsharp wannabee)
+set predicate (robots-different wannabee bsharp)
+set predicate (locations-different gra_bay0 gra_bay1)
+set predicate (locations-different gra_bay0 gra_bay2)
+set predicate (locations-different gra_bay0 gra_bay3)
+set predicate (locations-different gra_bay0 gra_bay4)
+set predicate (locations-different gra_bay0 gra_bay5)
+set predicate (locations-different gra_bay0 gra_bay6)
+set predicate (locations-different gra_bay0 gra_bay7)
+set predicate (locations-different gra_bay0 gra_bay8)
+set predicate (locations-different gra_bay1 gra_bay0)
+set predicate (locations-different gra_bay1 gra_bay2)
+set predicate (locations-different gra_bay1 gra_bay3)
+set predicate (locations-different gra_bay1 gra_bay4)
+set predicate (locations-different gra_bay1 gra_bay5)
+set predicate (locations-different gra_bay1 gra_bay6)
+set predicate (locations-different gra_bay1 gra_bay7)
+set predicate (locations-different gra_bay1 gra_bay8)
+set predicate (locations-different gra_bay2 gra_bay0)
+set predicate (locations-different gra_bay2 gra_bay1)
+set predicate (locations-different gra_bay2 gra_bay3)
+set predicate (locations-different gra_bay2 gra_bay4)
+set predicate (locations-different gra_bay2 gra_bay5)
+set predicate (locations-different gra_bay2 gra_bay6)
+set predicate (locations-different gra_bay2 gra_bay7)
+set predicate (locations-different gra_bay2 gra_bay8)
+set predicate (locations-different gra_bay3 gra_bay0)
+set predicate (locations-different gra_bay3 gra_bay1)
+set predicate (locations-different gra_bay3 gra_bay2)
+set predicate (locations-different gra_bay3 gra_bay4)
+set predicate (locations-different gra_bay3 gra_bay5)
+set predicate (locations-different gra_bay3 gra_bay6)
+set predicate (locations-different gra_bay3 gra_bay7)
+set predicate (locations-different gra_bay3 gra_bay8)
+set predicate (locations-different gra_bay4 gra_bay0)
+set predicate (locations-different gra_bay4 gra_bay1)
+set predicate (locations-different gra_bay4 gra_bay2)
+set predicate (locations-different gra_bay4 gra_bay3)
+set predicate (locations-different gra_bay4 gra_bay5)
+set predicate (locations-different gra_bay4 gra_bay6)
+set predicate (locations-different gra_bay4 gra_bay7)
+set predicate (locations-different gra_bay4 gra_bay8)
+set predicate (locations-different gra_bay5 gra_bay0)
+set predicate (locations-different gra_bay5 gra_bay1)
+set predicate (locations-different gra_bay5 gra_bay2)
+set predicate (locations-different gra_bay5 gra_bay3)
+set predicate (locations-different gra_bay5 gra_bay4)
+set predicate (locations-different gra_bay5 gra_bay6)
+set predicate (locations-different gra_bay5 gra_bay7)
+set predicate (locations-different gra_bay5 gra_bay8)
+set predicate (locations-different gra_bay6 gra_bay0)
+set predicate (locations-different gra_bay6 gra_bay1)
+set predicate (locations-different gra_bay6 gra_bay2)
+set predicate (locations-different gra_bay6 gra_bay3)
+set predicate (locations-different gra_bay6 gra_bay4)
+set predicate (locations-different gra_bay6 gra_bay5)
+set predicate (locations-different gra_bay6 gra_bay7)
+set predicate (locations-different gra_bay6 gra_bay8)
+set predicate (locations-different gra_bay7 gra_bay0)
+set predicate (locations-different gra_bay7 gra_bay1)
+set predicate (locations-different gra_bay7 gra_bay2)
+set predicate (locations-different gra_bay7 gra_bay3)
+set predicate (locations-different gra_bay7 gra_bay4)
+set predicate (locations-different gra_bay7 gra_bay5)
+set predicate (locations-different gra_bay7 gra_bay6)
+set predicate (locations-different gra_bay7 gra_bay8)
+set predicate (locations-different gra_bay8 gra_bay0)
+set predicate (locations-different gra_bay8 gra_bay1)
+set predicate (locations-different gra_bay8 gra_bay2)
+set predicate (locations-different gra_bay8 gra_bay3)
+set predicate (locations-different gra_bay8 gra_bay4)
+set predicate (locations-different gra_bay8 gra_bay5)
+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 (location-available gra_bay0)
+set predicate (location-available gra_bay1)
+set predicate (location-available gra_bay2)
+set predicate (location-available gra_bay3)
+set predicate (location-available gra_bay4)
+set predicate (location-available gra_bay5)
+set predicate (location-available gra_bay6)
+set predicate (location-available gra_bay7)
+set predicate (location-available gra_bay8)
+set predicate (need-stereo bsharp o1 gra_bay1 gra_bay3)
+set predicate (need-stereo wannabee o4 gra_bay5 gra_bay7)
+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 bsharp) -1)
+set function (= (robot-order wannabee) -1)
diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl
index cd8061c2..c07c8318 100644
--- a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl
+++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl
@@ -2,7 +2,7 @@
;; Command was: ./tools/problem_generator
;; Working directory was: /home/vagrant/isaac/src/astrobee/survey_manager/survey_planner
;; Problem template: pddl/jem_survey_template.pddl
-;; Config 1: data/survey_static.yaml
+;; Config 1: data/jem_survey_static.yaml
;; Config 2: data/jem_survey_dynamic.yaml
(define (problem jem-survey)
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 b867b4fd..686fcd7b 100644
--- a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp
+++ b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp
@@ -103,6 +103,13 @@ void IsaacAction::do_work() {
}
}
}
+
+IsaacAction::~IsaacAction() {
+ if (pid_ != 0) {
+ // Kill the child process
+ kill(pid_, SIGKILL);
+ }
+}
} // namespace plansys2_actions
@@ -123,7 +130,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(1));
+ auto action_node = std::make_shared(nh, action_name, std::chrono::seconds(2));
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 cde03e18..56d2e5d8 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
@@ -19,18 +19,18 @@
# under the License.
import argparse
+import fcntl
import os
import pathlib
-import select
import socket
import subprocess
import sys
import threading
-from typing import Any, Dict, List
+import time # Add time module for waiting
+from typing import List
import rospkg
import rospy
-import yaml
from ff_msgs.msg import (
AckCompletedStatus,
AckStamped,
@@ -48,11 +48,19 @@
MAX_COUNTER = 10
CHUNK_SIZE = 1024
+# Note: INFO_CONTEXT may get extended below during arg parsing
+INFO_CONTEXT = "command_astrobee"
+
+
+def loginfo(msg: str) -> None:
+ "Call rospy.loginfo() with context."
+ rospy.loginfo(f"{INFO_CONTEXT}: {msg}")
+
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":
- print("CHANGING EXPOSURE TO JEM")
+ loginfo("CHANGING EXPOSURE TO JEM")
return config_static["exposure"]["jem"]
# Going to NOD2
@@ -62,7 +70,7 @@ def exposure_change(config_static, bay_origin, bay_destination):
or bay_origin == "usl_hatch_to_nod2"
and bay_destination == "nod2_hatch_from_usl"
):
- print("CHANGING EXPOSURE TO NOD2")
+ loginfo("CHANGING EXPOSURE TO NOD2")
return config_static["exposure"]["nod2"]
# Going to USL
@@ -75,7 +83,7 @@ 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":
- print("CHANGING MAP TO JEM")
+ loginfo("CHANGING MAP TO JEM")
return config_static["maps"]["jem"]
# Going to NOD2
if (
@@ -84,7 +92,7 @@ def map_change(config_static, bay_origin, bay_destination):
or bay_origin == "usl_hatch_to_nod2"
and bay_destination == "nod2_hatch_from_usl"
):
- print("CHANGING MAP TO NOD2")
+ 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":
@@ -128,131 +136,186 @@ def __init__(self, robot_name):
if os.path.exists(self.output_path):
os.remove(self.output_path)
- # Declare socket for process input
+ # Declare socket for monitor to process input
+ # This socket is used for getting input from the monitor into the process running
+ # or to the current program. This is how the user can control the execution since
+ # this program can be assumed to run on the background
self.sock_input = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
self.sock_input.settimeout(0.05) # Set a timeout for socket operations
self.sock_input.bind(self.input_path)
self.sock_input.listen(1) # Listen for one connection
self.sock_input_connected = False
+ self.sock_input_conn = None
- # Declare socket for process output
+ # Declare socket for process to monitor output
+ # This socket takes output from both the process running and this program
+ # and publishes it to the monitor. This allows the user to have some situational
+ # awareness of what's going on.
self.sock_output = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
self.sock_output.settimeout(0.05) # Set a timeout for socket operations
self.sock_output.bind(self.output_path)
self.sock_output.listen(1) # Listen for one connection
self.sock_output_connected = False
+ self.sock_output_conn = None
# Declare event that will stop input thread
self._stop_event = threading.Event()
def __del__(self):
+ loginfo("closing sockets!")
self.sock_input.close()
self.sock_output.close()
+ def write_output_once(self, output):
+ while not self.sock_output_connected:
+ try:
+ # 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
+ 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
+
def thread_write_output(self, process):
- # print("starting thread_write_output...")
+ # loginfo("starting thread_write_output...")
# Store cumulative output
output_total = ""
try:
- while True:
+ while not self._stop_event.is_set() and process.poll() is None:
# Get output from process
- # print("waiting for output")
+ # loginfo("waiting for output")
output = process.stdout.readline()
- if (
- output == "" and process.poll() is not None
- ) or self._stop_event.is_set():
+ if process.poll() is not None or self._stop_event.is_set():
break
- if output:
- rospy.loginfo(output)
+ 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:
- # print("trying to connect")
- conn, addr = self.sock_output.accept()
- conn.setblocking(False)
+ # loginfo("trying to connect")
+ self.sock_output_conn, addr = self.sock_output.accept()
+ self.sock_output_conn.setblocking(False)
self.sock_output_connected = True
encoded_message = output_total.encode("ascii", errors="replace")
for i in range(0, len(encoded_message), CHUNK_SIZE):
chunk = encoded_message[i : i + CHUNK_SIZE]
- conn.sendall(chunk)
+ self.sock_output_conn.sendall(chunk)
# If socket is already connected, send output
elif self.sock_output_connected:
- conn.send(output.encode("ascii", errors="replace")[:CHUNK_SIZE])
+ self.sock_output_conn.send(
+ output.encode("ascii", errors="replace")[:CHUNK_SIZE]
+ )
except socket.timeout:
continue
except (socket.error, BrokenPipeError):
- print("Error sending data. Receiver may have disconnected.")
+ loginfo("writer can't send data. Receiver may have disconnected.")
+ time.sleep(2)
self.sock_output_connected = False
except Exception as e:
- print("exit output:")
- print(e)
+ loginfo(f"writer exiting on exception: {e}")
# finally:
# # Save total output into a log
- # rospy.loginfo(output_total)
+ # loginfo(output_total)
+
+ def read_input_once(self) -> str:
+ while not (self.sock_input_connected or self._stop_event.is_set()):
+ # 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_connected = True
+ break
+ except socket.timeout:
+ continue
+ while not self._stop_event.is_set():
+ try:
+ request = self.sock_input_conn.recv(CHUNK_SIZE).decode(
+ "ascii", errors="replace"
+ )
+ return request
+ except socket.timeout:
+ continue
+ except ConnectionResetError:
+ # Connection was reset, set sock_input_connected to False
+ self.sock_input_connected = False
+ break
+ return ""
def thread_read_input(self, process):
- # print("starting thread_read_input...")
+ # loginfo("starting thread_read_input...")
try:
- while True:
+ while not self._stop_event.is_set():
+ while not (self.sock_input_connected or self._stop_event.is_set()):
+ # loginfo("waiting for connection")
+ try:
+ self.sock_input_conn, addr = self.sock_input.accept()
+ self.sock_input_conn.settimeout(1)
+ self.sock_input_connected = True
+ break
+ except socket.timeout:
+ continue
+
while not self._stop_event.is_set():
- # print("waiting for connection")
try:
- client_socket, client_address = self.sock_input.accept()
+ request = self.sock_input_conn.recv(CHUNK_SIZE).decode(
+ "ascii", errors="replace"
+ )
break
except socket.timeout:
continue
+ except ConnectionResetError:
+ # Connection was reset, set sock_input_connected to False
+ self.sock_input_connected = False
+ break
if self._stop_event.is_set():
break
- client_socket.settimeout(1) # Set a timeout for socket operations
-
- while True:
- # print("accepted connection:")
- print(client_address)
-
- while not self._stop_event.is_set():
- # print("waiting to receive")
- try:
- request = client_socket.recv(CHUNK_SIZE).decode(
- "ascii", errors="replace"
- )
- break
- except socket.timeout:
- continue
- if self._stop_event.is_set():
- break
- # If broken pipe connect again
- if not request:
- break
- print("got: " + request)
-
- print(request)
- process.stdin.write(request + "\n")
- process.stdin.flush()
+ # If broken pipe connect
+ if not request:
+ break
+ loginfo("reader sending: " + request)
+ process.stdin.write(request + "\n")
+ process.stdin.flush()
except Exception as e:
- print("exit input:")
- print(e)
+ loginfo(f"reader exiting on exception: {e}")
def send_command(self, command):
- print(command)
+ loginfo(f"send_command: {command}")
return_code = 1
try:
# Start the process
process = subprocess.Popen(
command,
- shell=True,
stdin=subprocess.PIPE,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
text=True,
)
+ # Set the stdout stream to non-blocking
+ fcntl.fcntl(process.stdout, fcntl.F_SETFL, os.O_NONBLOCK)
# Start input and output threads
input_thread = threading.Thread(
@@ -264,39 +327,57 @@ def send_command(self, command):
)
output_thread.start()
- while output_thread.is_alive():
+ # When the process finishes, te output thread automatically closes
+ while output_thread.is_alive() and process.poll() is None:
rospy.sleep(1)
- output_thread.join()
# Get the return code of the process
return_code = process.poll()
except Exception as e:
- print("exit main:")
- print(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)
- print("Killing input thread...")
+ loginfo("send_command killing input thread...")
self._stop_event.set()
- input_thread.join()
if output_thread.is_alive():
output_thread.join()
+ input_thread.join()
# Get the final exit code
- return return_code
+ 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
def send_command_recursive(self, command):
- print("Sending recursive command")
+ loginfo(f"Sending recursive command: {command}")
exit_code = self.send_command(command)
- print("Exit code " + str(exit_code))
+ loginfo("send_command exit code " + str(exit_code))
- if exit_code != 0 and not rospy.is_shutdown():
- repeat = input("Do you want to repeat the command? (yes/no): ").lower()
- print(repeat)
+ 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): "
+ )
+ repeat = self.read_input_once().lower()
+ loginfo(f"user input: {repeat}")
if repeat == "yes":
exit_code = exit_code = self.send_command_recursive(command)
+ break
+ if repeat == "no":
+ break
+ if repeat == "skip":
+ exit_code = 0
+ break
return exit_code
@@ -306,12 +387,13 @@ def send_command_recursive(self, command):
class CommandExecutor:
def __init__(self, ns):
self.ns = ns
- rospy.loginfo(self.ns + "/command")
+ 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
)
self.ack_needed = False
+ self.ack_msg = None
self.sub_plan_status = rospy.Subscriber(
self.ns + "/mgt/executive/plan_status",
PlanStatusStamped,
@@ -322,8 +404,10 @@ def __init__(self, ns):
self.pub_command = rospy.Publisher(
self.ns + "/command", CommandStamped, queue_size=5
)
- while self.pub_command.get_num_connections() == 0:
- rospy.loginfo("Waiting for subscriber to connect")
+ while self.pub_command.get_num_connections() == 0 and not rospy.is_shutdown():
+ loginfo(
+ f"Waiting for an astrobee executive to subscribe to {self.ns}/command"
+ )
rospy.sleep(1)
self.unique_cmd_id = ""
@@ -343,6 +427,7 @@ def start_recording(self, bag_description):
cmd.args = [arg1]
# Publish the CommandStamped message
+ loginfo(f"bag recording: Starting with description {bag_description}")
result = self.publish_and_wait_response(cmd)
return result
@@ -356,36 +441,35 @@ def stop_recording(self):
cmd.cmd_origin = "isaac fsw"
# Publish the CommandStamped message
+ loginfo("bag recording: stopping recording")
result = self.publish_and_wait_response(cmd)
return result
def change_exposure(self, val):
# TBD
- rospy.loginfo("Change exposure to " + str(val))
+ loginfo("Change exposure to " + str(val))
return 0
def change_map(self, map_name):
# TBD
- rospy.loginfo("Change map to " + map_name)
+ loginfo("Change map to " + map_name)
return 0
def ack_callback(self, msg):
- if self.ack_needed == True and msg.cmd_id == self.unique_cmd_id:
+ if self.ack_needed is True and msg.cmd_id == self.unique_cmd_id:
self.ack_msg = msg
self.ack_needed = False
def plan_status_callback(self, msg):
- if self.plan_status_needed == True:
- rospy.loginfo("plan_name" + self.plan_name + "; msg name " + msg.name)
+ if self.plan_status_needed is True:
+ loginfo(f"plan_name {self.plan_name}; msg name {msg.name}")
if self.plan_name in msg.name:
- rospy.loginfo(
- "In point " + str(msg.point) + " status " + str(msg.status.status)
- )
+ loginfo(f"In point {msg.point} status {msg.status.status}")
if msg.status.status == 3:
self.plan_status_needed = False
else:
# Plan changed, and previous plan did not complete
- rospy.loginfo("Plan changed, exiting.")
+ loginfo("Plan changed, exiting.")
self.plan_status_needed = False
def publish_and_wait_response(self, cmd):
@@ -399,15 +483,15 @@ def publish_and_wait_response(self, cmd):
counter = 0
while counter < MAX_COUNTER:
# got message
- if self.ack_needed == False:
+ if self.ack_needed is False:
if self.ack_msg.completed_status.status == AckCompletedStatus.NOT:
- rospy.loginfo("Command is being executed and has not completed.")
+ loginfo("Command is being executed and has not completed.")
self.ack_needed = True
elif self.ack_msg.completed_status.status == AckCompletedStatus.OK:
- rospy.loginfo("Command completed successfully!")
+ loginfo("Command completed successfully!")
return 0
else:
- rospy.loginfo("Command failed! Message: " + self.ack_msg.message)
+ loginfo("Command failed! Message: " + self.ack_msg.message)
return 1
else:
rospy.sleep(1)
@@ -421,17 +505,12 @@ def wait_plan(self):
counter = 0
while counter < MAX_COUNTER:
# got message
- if self.plan_status_needed == False:
+ if self.plan_status_needed is False:
return 0
return 1
-def survey_manager_executor(command_names, run, config_static_path: pathlib.Path):
- # Read the static configs that convert constants to values
- config_static = load_yaml(config_static_path)
-
- args = yaml_action_from_pddl(f"[{' '.join(command_names)}]", config_static)
-
+def survey_manager_executor(args, run, config_static, process_executor):
# Start ROS node
rospy.init_node("survey_namager_cmd_" + args["robot"], anonymous=True)
@@ -439,7 +518,7 @@ def survey_manager_executor(command_names, run, config_static_path: pathlib.Path
# Figure out robot name and whether we are in simulation or hardware
current_robot = os.environ.get("ROBOTNAME")
if not current_robot:
- rospy.loginfo("ROBOTNAME not defined. Let's get the robotname using the topic")
+ 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)
@@ -448,39 +527,48 @@ def survey_manager_executor(command_names, run, config_static_path: pathlib.Path
current_robot = ""
sim = True
- ns = ""
+ ns = []
# If we're commanding a robot remotely
if current_robot != args["robot"]:
- rospy.loginfo("We're commanding a namespaced robot!")
- ns = " -remote -ns " + 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
- command_executor = CommandExecutor(args["robot"])
+ command_executor = CommandExecutor("/" + args["robot"])
else:
command_executor = CommandExecutor("")
- process_executor = ProcessExecutor(args["robot"])
# Initialize exit code
exit_code = 0
if args["type"] == "dock":
- exit_code += process_executor.send_command_recursive(
- "rosrun executive teleop_tool -dock"
- + ns
- + " -berth "
- + config_static["berth"][args["berth"]]
- )
+ cmd = [
+ "rosrun",
+ "executive",
+ "teleop_tool",
+ "-dock",
+ "-remote",
+ "-berth",
+ config_static["berth"][args["berth"]],
+ ]
+ cmd.extend(ns) if ns else None
+
+ exit_code += process_executor.send_command_recursive(cmd)
elif args["type"] == "undock":
- exit_code += process_executor.send_command_recursive(
- "rosrun executive teleop_tool -undock" + ns
- )
+ cmd = ["rosrun", "executive", "teleop_tool", "-undock", "-remote"]
+ cmd.extend(ns) if ns else None
+
+ exit_code += process_executor.send_command_recursive(cmd)
elif args["type"] == "move":
- exit_code += process_executor.send_command_recursive(
- "rosrun executive teleop_tool -move "
- + config_static["bays_move"][args["to_name"]]
- + ns
- )
+ 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"]
@@ -496,27 +584,54 @@ def survey_manager_executor(command_names, run, config_static_path: pathlib.Path
exit_code += command_executor.start_recording(
"pano_" + args["location_name"] + "_" + run
)
- exit_code += process_executor.send_command_recursive(
- "rosrun inspection inspection_tool -geometry -geometry_poses /resources/"
- + config_static["bays_pano"][args["location_name"]]
- + ns
- )
+ if exit_code != 0:
+ loginfo(
+ "panorama: Failed to start recording, no point in starting the panorama"
+ )
+ return exit_code
+
+ cmd = [
+ "rosrun",
+ "inspection",
+ "inspection_tool",
+ "-geometry",
+ "-geometry_poses",
+ "/resources/" + config_static["bays_pano"][args["location_name"]],
+ "-remote",
+ ]
+ cmd.extend(ns) if ns else None
+
+ exit_code += process_executor.send_command_recursive(cmd)
+ print("STOP RECORDING")
exit_code += command_executor.stop_recording()
elif args["type"] == "stereo":
exit_code += command_executor.start_recording(
"stereo_" + os.path.basename(args["fplan"]) + "_" + run
)
+ if exit_code != 0:
+ loginfo(
+ "stereo: Failed to start recording, no point in starting the stereo"
+ )
+ return exit_code
+
# This starts the plan
plan_path = get_ops_plan_path()
command_executor.plan_status_needed = True
command_executor.plan_name = os.path.basename(args["fplan"])
- exit_code += process_executor.send_command_recursive(
- "rosrun executive plan_pub "
- + os.path.join(plan_path, args["fplan"] + ".fplan")
- + ns
- )
+
+ cmd = [
+ "rosrun",
+ "executive",
+ "plan_pub",
+ os.path.join(plan_path, args["fplan"] + ".fplan"),
+ "-remote",
+ ]
+ cmd.extend(ns) if ns else None
+
+ exit_code += process_executor.send_command_recursive(cmd)
+
if exit_code == 0:
exit_code += command_executor.wait_plan()
exit_code += command_executor.stop_recording()
@@ -525,53 +640,98 @@ def survey_manager_executor(command_names, run, config_static_path: pathlib.Path
def survey_manager_executor_recursive(
- command_names, run_number, config_static_path: pathlib.Path
+ command_names, run_number, config_static, process_executor
):
exit_code = survey_manager_executor(
- command_names, f"run{run_number}", config_static_path
+ command_names, f"run{run_number}", config_static, process_executor
)
- if exit_code != 0:
- repeat = input("Do you want to repeat the survey? (yes/no): ").lower()
+ 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): "
+ )
+ 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_path
+ command_names, run_number, config_static, process_executor
)
+ break
+ if repeat == "no":
+ break
+ if repeat == "skip":
+ exit_code = 0
+ break
return exit_code
+def command_astrobee(action_args, config_static_paths: List[pathlib.Path]):
+ # Read the static configs that convert constants to values
+ config_static = {}
+ for config_static_path in config_static_paths:
+ loginfo(f"reading config: {config_static_path}")
+ yaml_dict = load_yaml(config_static_path)
+ for key, value in yaml_dict.items():
+ if key not in config_static:
+ config_static[key] = value
+ elif isinstance(value, dict): # Merge nested dictionaries
+ config_static[key].update(value)
+ elif isinstance(value, list): # Extend lists
+ config_static[key].extend(value)
+ else: # Overwrite scalar values
+ config_static[key] = value
+
+ pddl_action = f"({' '.join(action_args)})"
+ args = yaml_action_from_pddl(pddl_action, config_static)
+ global INFO_CONTEXT
+ INFO_CONTEXT = f"command_astrobee {pddl_action}"
+
+ process_executor = ProcessExecutor(args["robot"])
+
+ exit_code = survey_manager_executor_recursive(
+ args, 1, config_static, process_executor
+ )
+
+ loginfo(f"Finished plan action with code {exit_code}")
+ return exit_code
+
+
class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter):
pass
def main():
+ default_config_paths = [
+ os.path.join(
+ rospkg.RosPack().get_path("survey_planner"), "data/jem_survey_static.yaml"
+ ),
+ os.path.join(
+ rospkg.RosPack().get_path("survey_planner"),
+ "data/granite_survey_static.yaml",
+ ),
+ ]
+
parser = argparse.ArgumentParser(
description=__doc__, formatter_class=CustomFormatter
)
parser.add_argument(
- "command_names",
+ "action_args",
nargs="*",
- help="Prefixes for bagfiles to merge. Bags should all be in the current working directory.",
+ help="PDDL action name and its arguments",
)
parser.add_argument(
"--config_static",
help="Path to input static problem config YAML (module geometry, available stereo surveys, etc.)",
type=pathlib.Path,
- default=os.path.join(
- rospkg.RosPack().get_path("survey_planner"), "data/survey_static.yaml"
- ),
+ nargs="+",
+ default=[pathlib.Path(path) for path in default_config_paths],
)
args = parser.parse_args()
- exit_code = survey_manager_executor_recursive(
- args.command_names, 1, args.config_static
- )
-
- print("Finished plan action with code " + str(exit_code))
- return exit_code
+ return command_astrobee(args.action_args, args.config_static)
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 89fc12f7..73b561e2 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
@@ -23,65 +23,95 @@
import socket
import sys
import threading
+import time # Add time module for waiting
# Constants
CHUNK_SIZE = 1024
-
-
-def thread_write_input(sock_input):
- # print("starting thread_write_input...")
- try:
- while True:
- # Get user input dynamically
- user_input = input()
- print("user input: " + user_input)
- # Check if the user wants to exit
- if user_input.lower().strip() == "exit":
- break
- sock_input.send(user_input.encode("ascii", errors="replace")[:CHUNK_SIZE])
-
- except:
- print("exit output")
-
-
-def thread_read_output(sock_output):
- # print("starting thread_read_output...")
- try:
- while True:
- request = sock_output.recv(CHUNK_SIZE)
- request = request.decode("ascii", errors="replace") # convert bytes to str
-
- print(request, end="")
- except:
- print("exit input")
+# Declare event that will stop input thread
+stop_event = threading.Event()
+
+
+def thread_write_input(input_path):
+ print("Start thread_write_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...")
+ time.sleep(5)
+ continue
+
+ try:
+ while not stop_event.is_set():
+ 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]
+ )
+ except Exception as e:
+ print("Exception in thread_write_input:", e)
+
+ # Close the sockets
+ sock_client_input.close()
+
+
+def thread_read_output(output_path):
+ print("Start thread_read_output")
+
+ while not stop_event.is_set():
+ try:
+ # Attempt to connect to the server
+ sock_client_output = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+ sock_client_output.connect(output_path)
+
+ except ConnectionRefusedError:
+ print("Output Connection refused. Retrying in 5 seconds...")
+ time.sleep(5)
+ 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)
+
+ # Close the sockets
+ sock_client_output.close()
def survey_monitor(robot_name):
input_path = "/tmp/input_" + robot_name
output_path = "/tmp/output_" + robot_name
- sock_client_input = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
- sock_client_input.connect(input_path)
- sock_client_output = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
- sock_client_output.connect(output_path)
+ 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=(sock_client_input,)
- )
+ input_thread = threading.Thread(target=thread_write_input, args=(input_path,))
input_thread.start()
- output_thread = threading.Thread(
- target=thread_read_output, args=(sock_client_output,)
- )
+ output_thread = threading.Thread(target=thread_read_output, args=(output_path,))
output_thread.start()
- # Wait for the thread
+ # Wait for the threads to finish
input_thread.join()
- output_thread.join()
- # Close the sockets
- sock_client_input.close()
- sock_client_output.close()
+ output_thread.join()
class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter):
diff --git a/astrobee/survey_manager/survey_planner/src/survey_planner/plan_interpreter.py b/astrobee/survey_manager/survey_planner/src/survey_planner/plan_interpreter.py
index 1c45a3c6..6e3955eb 100755
--- a/astrobee/survey_manager/survey_planner/src/survey_planner/plan_interpreter.py
+++ b/astrobee/survey_manager/survey_planner/src/survey_planner/plan_interpreter.py
@@ -52,7 +52,7 @@
)
DEFAULT_CONFIGS = [
- DATA_DIR / "survey_static.yaml",
+ DATA_DIR / "jem_survey_static.yaml",
# Dynamic config not needed for interpreting the plan
]
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 5bfea135..2e299e85 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
@@ -79,7 +79,7 @@
os.path.relpath(str((THIS_DIR / ".." / ".." / "pddl").resolve()), CWD)
)
DEFAULT_CONFIGS = [
- DATA_DIR / "survey_static.yaml",
+ DATA_DIR / "jem_survey_static.yaml",
DATA_DIR / "jem_survey_dynamic.yaml",
]
@@ -471,7 +471,12 @@ def problem_generator(
location_real_lines = [f"(location-real {bay})" for bay in bays]
writer.declare_predicates(location_real_lines, "static_predicates")
- candidates = (("jem_bay7", "berth1"), ("jem_bay7", "berth2"))
+ candidates = (
+ ("jem_bay7", "berth1"),
+ ("jem_bay7", "berth2"),
+ ("gra_bay3", "berth1"),
+ ("gra_bay5", "berth2"),
+ )
dock_connected_lines = [
f"(dock-connected {bay} {berth})"
for bay, berth in candidates
diff --git a/isaac/launch/sim.launch b/isaac/launch/sim.launch
index 15c790d5..53d3029a 100644
--- a/isaac/launch/sim.launch
+++ b/isaac/launch/sim.launch
@@ -75,13 +75,15 @@
name="pose"
default="0 0 -0.7 0 0 0 1" />
-
-
+
+
+
+
@@ -208,6 +210,9 @@
+
+
+
@@ -217,7 +222,10 @@
-
+
+
@@ -234,6 +242,9 @@
+
+
+
@@ -243,9 +254,9 @@
-
-
+
@@ -263,6 +274,9 @@
+
+
+
@@ -272,7 +286,10 @@
-
+
+
@@ -289,9 +306,9 @@
-
-
-
+
+
+