adding granite test bench + fixes (#129)

* adding granite testbench + fixes

* small corrections

* updating the submodule with bugfix

* fix ns in sub

* make honey start sim in berth2

* updating exec scripts, not finished

* fixes from lab testing

* Fix problem generator for granite lab.

* updating granite plan to include stereo

* improving robustness

* fixing namespace bug; adding existing astrobee launchfile options to isaac

* disabling pos printing in favor of granite testing working - can't change survey static because of jem plan; hard to change command_astrobee config name

* splitting config files completely from granite and jem

* Fix granite domain bug.

* Add intermediate bays for valid stereo command.

* full survey running in granite lab

* killing command_astrobee on exit

* adding some explanations to the sockets

* Add logging improvements

* making shell false in popen; fix inspection survey name

* updating submodule

---------

Co-authored-by: Brian Coltin <[email protected]>
Co-authored-by: Trey Smith <[email protected]>

astrobee/behaviors/inspection/resources/panorama_granite_bsharp.txt

@@ -0,0 +1,2 @@
+# Panorama
+0.1 0.6 -0.772 0 0 -90

astrobee/behaviors/inspection/resources/panorama_granite_wannabee.txt

@@ -1,2 +1,2 @@
 # Panorama
-0 0 -0.6 180 0 -90
+0.1 -0.3 -0.6 180 0 -90

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);
 
 

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

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

astrobee/survey_manager/survey_planner/data/survey_static.yaml → 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

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();

astrobee/survey_manager/survey_planner/launch/survey.launch

@@ -0,0 +1,76 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- arguments -->
+  <arg name="planner" default="trey" />  <!-- either optic or trey -->
+  <arg name="lifecycle" default="false" />  <!-- lifecycle node -->
+  <arg name="robot1" default="" />  <!-- lifecycle node -->
+  <arg name="robot2" default="honey" />  <!-- lifecycle node -->
+
+
+  <arg name="model_file" default="$(find survey_planner)/pddl/domain_survey.pddl"/>
+  <arg name="namespace" default="" />
+  <arg name="params_file" default="$(find survey_planner)/params/plansys2_$(arg planner)_params.yaml" />
+
+  <!-- true: There is a lifecycle management node for the Lifecycles nodes. If false, the nodes start on their own -->
+  <param name="lifecycle_enabled" value="$(arg lifecycle)"/>
+
+  <!-- Domain Expert -->
+  <include file="$(find plansys2_domain_expert)/launch/domain_expert.launch">
+    <arg name="model_file"   value="$(arg model_file)"/>
+    <arg name="namespace"  value="$(arg namespace)"/>
+    <arg name="params_file" value="$(arg params_file)"/>
+  </include>
+
+  <!-- Problem Expert -->
+  <include file="$(find plansys2_problem_expert)/launch/problem_expert.launch">
+    <arg name="model_file"   value="$(arg model_file)"/>
+    <arg name="namespace"  value="$(arg namespace)"/>
+    <arg name="params_file" value="$(arg params_file)"/>
+  </include>
+
+  <!-- Planner -->
+  <include file="$(find plansys2_planner)/launch/planner.launch">
+    <arg name="namespace"  value="$(arg namespace)"/>
+    <arg name="params_file" value="$(arg params_file)"/>
+  </include>
+
+  <!-- Executor -->
+  <include file="$(find plansys2_executor)/launch/executor.launch">
+    <arg name="namespace"  value="$(arg namespace)"/>
+    <arg name="params_file" value="$(arg params_file)"/>
+  </include>
+
+  <!-- Manager -->
+  <group if="$(arg lifecycle)">
+    <node name="lifecycle_manager_node" pkg="plansys2_lifecycle_manager" type="lifecycle_manager_node" output="screen">
+    </node>
+  </group>
+
+  <!-- Actions -->
+  <node name="move_action_node1" pkg="survey_planner" type="move_action_node" output="screen">
+  </node>
+  <node name="move_action_node2" pkg="survey_planner" type="move_action_node" output="screen">
+  </node>
+
+  <node name="dock_action_node1" pkg="survey_planner" type="dock_action_node" output="screen">
+  </node>
+  <node name="dock_action_node2" pkg="survey_planner" type="dock_action_node" output="screen">
+  </node>
+
+  <node name="undock_action_node1" pkg="survey_planner" type="undock_action_node" output="screen">
+  </node>
+  <node name="undock_action_node2" pkg="survey_planner" type="undock_action_node" output="screen">
+  </node>
+
+  <node name="panorama_action_node1" pkg="survey_planner" type="panorama_action_node" output="screen">
+  </node>
+  <node name="panorama_action_node2" pkg="survey_planner" type="panorama_action_node" output="screen">
+  </node>
+
+  <node name="stereo_action_node1" pkg="survey_planner" type="stereo_action_node" output="screen">
+  </node>
+  <node name="stereo_action_node2" pkg="survey_planner" type="stereo_action_node" output="screen">
+  </node>
+</launch>
+