Survey manager execute goals script

astrobee/behaviors/inspection/tools/inspection_tool.cc

@@ -113,6 +113,7 @@ DEFINE_double(deadline, -1.0, "Action deadline timeout");
 // Match the internal states and responses with the message definition
 using STATE = isaac_msgs::InspectionState;
 bool stopflag_ = false;
+std::string feedback_old = "";
 
 bool has_only_whitespace_or_comments(const std::string & str) {
   for (std::string::const_iterator it = str.begin(); it != str.end(); it++) {
@@ -185,7 +186,7 @@ geometry_msgs::PoseArray ReadPosesFile(std::string file) {
           quat_robot.setRPY(euler_roll * DEG2RAD, euler_pitch * DEG2RAD, euler_yaw * DEG2RAD);
 
         } else {
-          std::cout << "Ignoring invalid line: " << line  << std::endl;
+          // std::cout << "Ignoring invalid line: " << line  << std::endl;
           continue;
         }
       }
@@ -209,6 +210,8 @@ void FeedbackCallback(isaac_msgs::InspectionFeedbackConstPtr const& feedback) {
     + " -> " + feedback->state.fsm_state
     + " (" + feedback->state.fsm_subevent
     + " -> " + feedback->state.fsm_substate + ")";
+  if (s == feedback_old) return;
+  feedback_old = s;
   if (s.size() < 70) s.append(70 - s.size(), ' ');
   std::cout << "\r" << s.substr(0, 70) << "|Input: " << std::flush;
 }

astrobee/survey_manager/survey_planner/CMakeLists.txt

@@ -16,23 +16,64 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(survey_planner)
 
-add_compile_options(-std=c++17)
+set(CMAKE_CXX_STANDARD 17)
 
 find_package(catkin REQUIRED)
 SET(catkin2_DIR "${CMAKE_SOURCE_DIR}/../../../cmake")
 find_package(catkin2 REQUIRED COMPONENTS
   roscpp
   ff_msgs
+  lifecycle
+  plansys2_executor
 )
 
 catkin_package(
   CATKIN_DEPENDS
 )
 
-foreach( dir pddl launch)
+###########
+## Build ##
+###########
+
+# Specify additional locations of header files
+# Your package locations should be listed before other locations
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+# Action for move
+add_executable(move_action_node src/move_action_node.cpp)
+target_link_libraries(move_action_node ${catkin_LIBRARIES} )
+# Action for dock
+add_executable(dock_action_node src/dock_action_node.cpp)
+target_link_libraries(dock_action_node ${catkin_LIBRARIES} )
+# Action for undock
+add_executable(undock_action_node src/undock_action_node.cpp)
+target_link_libraries(undock_action_node ${catkin_LIBRARIES} )
+# Action for panorama
+add_executable(panorama_action_node src/panorama_action_node.cpp)
+target_link_libraries(panorama_action_node ${catkin_LIBRARIES} )
+# Action for stereo
+add_executable(stereo_action_node src/stereo_action_node.cpp)
+target_link_libraries(stereo_action_node ${catkin_LIBRARIES} )
+
+#############
+## Install ##
+#############
+
+# Allow other packages to use python scripts from this package
+catkin_python_setup()
+
+catkin_package()
+
+# Install actions
+install(TARGETS move_action_node
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+foreach( dir pddl launch data)
    install( DIRECTORY ${dir}/
          DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir} )
 endforeach(dir)

astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml → astrobee/survey_manager/survey_planner/data/survey_static.yaml

@@ -37,6 +37,51 @@ bays:
   jem_bay6: [11.0, -9.0, 4.8]
   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
+  # jem_hatch_to_nod2:   "-move -pos '-0.3 -0.1 -0.68' -att '0 0 0'"
+  # jem_hatch_from_nod2: "-move -pos '-0.3 -0.1 -0.68' -att '0 0 180'"
+  # nod2_hatch_from_jem: "-move -pos '0.3  -0.1 -0.68' -att '0 0 0'"
+  # nod2_hatch_to_jem:   "-move -pos '0.3  -0.1 -0.68' -att '0 0 180'"
+
+maps:
+  jem: "iss.map"
+  nod2: "isaac.map"
+  usl: "usl_only.map"
+
+exposure:
+  jem: 175
+  nod2: 300
+  usl: 300
+berth:
+  berth1: "1"
+  berth2: "2"
+
 bogus_bays: [jem_bay0, jem_bay8]
 berths: [berth1, berth2]
 robots: [bumble, honey]
@@ -53,6 +98,6 @@ stereo:
     # check purposes.
     bound_location: jem_bay4  # The survey flies into bay4 even though it only covers up to bay3
   jem_bay4_to_bay7:
-    fplan: "jem_stereo_mapping_bay4_to_bay7.fplan"
+    fplan: "jem_stereo_mapping_bay7_to_bay4.fplan"
     base_location: jem_bay7
     bound_location: jem_bay4

astrobee/survey_manager/survey_planner/launch/survey_domain.launch

@@ -39,5 +39,17 @@
   <node name="lifecycle_manager_node" pkg="plansys2_lifecycle_manager" type="lifecycle_manager_node" output="screen">
   </node>
 
+  <!-- Actions -->
+  <node name="move_action_node" pkg="survey_planner" type="move_action_node" output="screen">
+  </node>
+  <node name="dock_action_node" pkg="survey_planner" type="dock_action_node" output="screen">
+  </node>
+  <node name="undock_action_node" pkg="survey_planner" type="undock_action_node" output="screen">
+  </node>
+  <node name="panorama_action_node" pkg="survey_planner" type="panorama_action_node" output="screen">
+  </node>
+  <node name="stereo_action_node" pkg="survey_planner" type="stereo_action_node" output="screen">
+  </node>
+
 </launch>
 

astrobee/survey_manager/survey_planner/package.xml

@@ -13,11 +13,14 @@
     ISAAC Flight Software
   </maintainer>
 
-  <buildtool_depend>catkin</buildtool_depend>
 
-  <depend>roscpp</depend>
-  <depend>std_srvs</depend>
-  <depend>lifecycle_msgs</depend>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>lifecycle</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>lifecycle</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>lifecycle</exec_depend>
 
   <depend>plansys2_domain_expert</depend>
   <depend>plansys2_problem_expert</depend>

astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl

@@ -2,7 +2,7 @@
 ;; Command was: ./tools/problem_generator.py
 ;; Working directory was: /home/vagrant/isaac/astrobee/survey_manager/survey_planner
 ;; Problem template: pddl/jem_survey_template.pddl
-;; Config 1: data/jem_survey_static.yaml
+;; Config 1: data/survey_static.yaml
 ;; Config 2: data/jem_survey_dynamic.yaml
 
 (define (problem jem-survey)
@@ -246,7 +246,7 @@
 ;; # 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.
+;; # initial state to match actual current state, etc.)  See also survey_static.yaml.
 ;;
 ;; goals:
 ;;

astrobee/survey_manager/survey_planner/readme.md

@@ -3,3 +3,127 @@
 Planning and scheduling of queued survey actions using Playsys2 for PDDL solutions and behavior trees for execution.
 
 Based on [preliminary work](https://github.com/traclabs/astrobee_task_planning_ws) by [Ana](https://github.com/ana-GT) at [Traclabs](https://traclabs.com).
+
+## Starting suvey planner
+
+The survey_planner as of now starts separately from the isaac fsw. This is necessary because the planner crashes very easily, so having the ability to easily restart this component is necessary.
+
+To start the planner and the terminal interface (in the robot, add the prefix `/opt/isaac/env_wrapper.sh`):
+
+    roslaunch survey_planner survey_domain.launch
+    rosrun plansys2_terminal plansys2_terminal
+
+To monitor an execution and to input commands to the execution:
+
+    rosrun survey_planner monitor_astrobee $ROBOTNAME
+
+## Running actions manually
+
+There are 5 different actions at the moment 'move', 'dock', 'undock', 'panorama', 'stereo'.
+To run each action individually through the survey_planner you must add the correct predicates before sending the action command or else the plansys2 executor will crash. Also, if you want to re-send the action you'll want to re-send the predicates too.
+
+Don't forget to change the commands to use the target robot's name (bumble is what is used in simulation)
+
+If the survey_planner crashes sometimes the simulation has to be restarted, for it to restart in a good state.
+
+### Move
+
+    set instance bumble robot
+    set instance jem_bay7 location
+    set instance jem_bay6 location
+    set instance jem_bay5 location
+
+    set predicate (robot-available bumble)
+    set predicate (robot-at bumble jem_bay7)
+    set predicate (location-available jem_bay6)
+
+    set predicate (move-connected jem_bay7 jem_bay6)
+    set predicate (location-real jem_bay6)
+    set predicate (locations-different jem_bay5 jem_bay7)
+    set predicate (move-connected jem_bay5 jem_bay6)
+    set predicate (location-available jem_bay5)
+
+
+    run action (move bumble jem_bay7 jem_bay6 jem_bay5)
+
+### Dock
+
+    set instance bumble robot
+    set instance jem_bay7 location
+    set instance berth1 location
+
+    set predicate (robot-available bumble)
+    set predicate (robot-at bumble jem_bay7)
+    set predicate (dock-connected jem_bay7 berth1)
+    set predicate (location-available berth1)
+
+    run action (dock bumble jem_bay7 berth1)
+
+
+### Undock
+
+
+    set instance bumble robot
+    set instance jem_bay6 location
+    set instance jem_bay7 location
+    set instance jem_bay8 location
+    set instance berth1 location
+
+    set predicate (robot-available bumble)
+    set predicate (robot-at bumble berth1)
+    set predicate (dock-connected jem_bay7 berth1)
+    set predicate (location-available jem_bay7)
+    set predicate (location-real jem_bay7)
+    set predicate (locations-different jem_bay8 jem_bay6)
+    set predicate (move-connected jem_bay8 jem_bay7)
+    set predicate (move-connected jem_bay6 jem_bay7)
+    set predicate (location-available jem_bay8)
+    set predicate (location-available jem_bay6)
+
+    run action (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6)
+
+
+### Panorama
+
+    set instance bumble robot
+    set instance jem_bay6 location
+    set instance  o0 order
+
+    set predicate (robot-available bumble)
+
+    set function (= (order-identity o0) 0)
+    set function (= (robot-order bumble) -1)
+
+    set predicate (robot-at bumble jem_bay6)
+
+    run action (panorama bumble o0 jem_bay6)
+
+
+### Stereo
+
+
+    set instance bumble robot
+    set instance jem_bay7 location
+    set instance jem_bay6 location
+    set instance jem_bay5 location
+    set instance jem_bay4 location
+    set instance jem_bay3 location
+    set instance  o0 order
+
+
+    set predicate (robot-available bumble)
+    set predicate (robot-at bumble jem_bay7)
+    set predicate (location-real jem_bay4)
+    set predicate (need-stereo bumble o0 jem_bay7 jem_bay4)
+    set predicate (location-available jem_bay4)
+    set predicate (locations-different jem_bay5 jem_bay3)
+    set predicate (move-connected jem_bay5 jem_bay4)
+    set predicate (move-connected jem_bay3 jem_bay4)
+    set predicate (location-available jem_bay5)
+    set predicate (location-available jem_bay3)
+
+    set function (= (order-identity o0) 0)
+    set function (= (robot-order bumble) -1)
+
+
+    run action (stereo bumble o0 jem_bay7 jem_bay4 jem_bay5 jem_bay3)

astrobee/survey_manager/survey_planner/setup.py

@@ -0,0 +1,7 @@
+from distutils.core import setup
+
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(packages=["survey_planner"], package_dir={"": "tools"})
+
+setup(**d)