Survey manager execute goals script (#103)

* making spawn changes and adding double handed cargo * adding a proof of concept example of the executor * running through isort * committing before changing things around * sockets working * fix lint * adding example action node * update to scripts with new requirements * thread join safeguard * running isort * Plansys2 on Astrobee Noetic (#107) * Add `survey_manager` subdirectory without layout for survey nodes Includes placeholder directories with hyperlinked `readme.md` files in the documentation hierarchy for the manger, planner, and bridge. Per discussion with @marinagmoreira `survey_manager` lives under `astrobee` to signify the code is intended for cross-compilation. * Improve linking to survey manager docs * Add files via upload Adding planning domain, problem, and xml of the atomic actions of the robotic agents * added files to the correct directory * adding soft constraints problem files * Attempt at a complete MVP domain model * Oops, remove redundant effect * Simplified collision checking predicate * In a working state * Add sample_output_plan.txt * Oops, remove obsolete commented-out code * Fixed stereo survey part of the model based on analysis of old stereo surveys by Marina - unfortunately causes additional planner flakiness * Update sample_output_plan.txt to reflect latest domain/problem * Now generate PDDL problem from higher-level problem specification * Tune panorama estimated duration * Add plan_interpreter.py. Minor cleanup in problem_generator.py * Simplify dynamic config just a bit * Add `survey_manager` subdirectory without layout for survey nodes Includes placeholder directories with hyperlinked `readme.md` files in the documentation hierarchy for the manger, planner, and bridge. Per discussion with @marinagmoreira `survey_manager` lives under `astrobee` to signify the code is intended for cross-compilation. * Improve linking to survey manager docs * Remove `survey_bridge` capability will be added to `astrobee` * Add traclabs plansys2 backport via submodule, thanks @ana-GT * Move sub-modules to `survey_manager` path * Upgrade behaviortree to V4 NOTE: If already installed, remove V3 before installing V4 * Cleanup unused, misplaced sub-modules, again. * Remove `survey_manager` package and organize `survey_planner` * Deprecate Ubuntu-16.04 (xenial) builds of Isaac to support Plansys2 - Remove Ubuntu-16.04 (xenial) CI builds - Update dockerfile ubuntu version defaults to Ubuntu-20.04 (focal) - Update apk build environment to Ubuntu-20.04 (focal) * Fix python formatting * Update to forks of traclabs backports * Add readline development files to setup for plansys2 terminal interface If you have already built your VM, run `sudo apt install libreadline-dev` * Removed outdated PDDL files per @trey0 #107 (comment) * Revert CI upgrades for APK builds --------- Co-authored-by: Abiola Akanni <[email protected]> Co-authored-by: Trey Smith <[email protected]> * bug fixes * fixes * Update ci_pr.yml make survey manager PR's run CI * moving executor to different package hierarchy * consolidating packages; action compiling * tested action * submodule update * move scripts into package * changing static config to be more general * fixing config name + install data folder * restoring comms to readline * getting details sorted * making output more readable * installing script to be found in robot install * adding move action starting correct process * adding parameters for easy testing * fixes on command astrobee * add remaining actions + documentation * running isort * Use exec and waitpid instead of reading output. * Change all action nodes to use the same class. * install all nodes * adding remote option with no parameter adjusting, we'll do panoramas using geometry * reset progress after one action * adding inspection lib; fixing output * fixing robot install * tested all 5 actions in the granite table * remote plansys2 submodules from doxygen * splitting documentation * addressing tons of comments on PR, lab tested * more PR reviews * more PR reviews * more PR reviews --------- Co-authored-by: Bckempa <[email protected]> Co-authored-by: Abiola Akanni <[email protected]> Co-authored-by: Trey Smith <[email protected]> Co-authored-by: Brian Coltin <[email protected]>
nasa · Jan 20, 2024 · f3e5df1 · f3e5df1
1 parent ebc170e
commit f3e5df1
Show file tree

Hide file tree

Showing 32 changed files with 1,467 additions and 135 deletions.
diff --git a/astrobee/behaviors/inspection/CMakeLists.txt b/astrobee/behaviors/inspection/CMakeLists.txt
@@ -106,7 +106,7 @@ target_link_libraries(test_pano
 #############
 
 # Mark executables and/or libraries for installation
-install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}
+install(TARGETS pano_survey ${PROJECT_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

diff --git a/astrobee/behaviors/inspection/src/inspection_node.cc b/astrobee/behaviors/inspection/src/inspection_node.cc
@@ -368,7 +368,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     ff_msgs::MotionResultConstPtr const& result) {
     // Check for invalid results
     if (result == nullptr) {
-      ROS_ERROR_STREAM("Invalid result received Motion");
+      ROS_INFO_STREAM("Invalid result received Motion");
       return fsm_.Update(MOTION_FAILED);
     }
 
@@ -607,7 +607,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
     if (result != nullptr)
       result_.anomaly_result.push_back(result->anomaly_result);
     else
-      ROS_ERROR_STREAM("Invalid result received Image Analysis");
+      ROS_INFO_STREAM("Invalid result received Image Analysis");
     return fsm_.Update(NEXT_INSPECT);
   }
 

diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc
@@ -66,6 +66,7 @@ DEFINE_bool(anomaly, false, "Send the inspection command");
 DEFINE_bool(geometry, false, "Send the inspection command");
 DEFINE_bool(panorama, false, "Send the inspection command");
 DEFINE_bool(volumetric, false, "Send the inspection command");
+DEFINE_bool(remote, false, "Whether target command is remote robot");
 
 // General parameters
 DEFINE_string(camera, "sci_cam", "Camera to use");
@@ -113,6 +114,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 +187,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 +211,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;
 }
@@ -307,10 +311,37 @@ void SendGoal(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
   client->SendGoal(goal);
 }
 
+bool GetlineAsync(std::istream& is, std::string& str, char delim = '\n') {
+    static std::string linesofar;
+    char inchar;
+    int charsread = 0;
+    bool lineread = false;
+    str = "";
+
+    do {
+        charsread = is.readsome(&inchar, 1);
+        if (charsread == 1) {
+            // if the delimiter is read then return the string so far
+            if (inchar == delim) {
+                str = linesofar;
+                linesofar = "";
+                lineread = true;
+            } else {  // otherwise add it to the string so far
+                linesofar.append(1, inchar);
+            }
+        }
+    } while (charsread != 0 && !lineread && !!stopflag_);
+
+    return lineread;
+}
+
 void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *client) {
   while (!stopflag_ && ros::ok()) {
     std::string line, val;
-    std::getline(std::cin, line);
+
+    if (!GetlineAsync(std::cin, line))
+      continue;
+
     std::string s;
     try {
       switch (std::stoi(line)) {
@@ -327,7 +358,7 @@ void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
           SendGoal(client);
           s = "\r Input: " + line + ") Pausing";
           if (s.size() < 80) s.append(80 - s.size(), ' ');
-          std::cout << s << std::flush;
+          std::cout << s << std::endl;
           break;
         case 2:
           FLAGS_pause = false;
@@ -344,7 +375,7 @@ void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
           SendGoal(client);
           s = "\r Input: " + line + ") Pausing and repeating pose (needs resume)";
           if (s.size() < 80) s.append(80 - s.size(), ' ');
-          std::cout << s << std::flush;
+          std::cout << s << std::endl;
           break;
         case 4:
           FLAGS_pause = false;
@@ -354,7 +385,7 @@ void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
           SendGoal(client);
           s = "\r Input: " + line + ") Pausing and skipping pose (needs resume)";
           if (s.size() < 80) s.append(80 - s.size(), ' ');
-          std::cout << s << std::flush;
+          std::cout << s << std::endl;
           break;
         case 5:
           FLAGS_pause = false;
@@ -365,7 +396,7 @@ void GetInput(ff_util::FreeFlyerActionClient<isaac_msgs::InspectionAction> *clie
           SendGoal(client);
           s = "\r Input: " + line + ") Pausing and saving (needs resume)";
           if (s.size() < 80) s.append(80 - s.size(), ' ');
-          std::cout << s << std::flush;
+          std::cout << s << std::endl;
           break;
         default:
           s = "\r Input: " + line + ") Invalid option";
@@ -424,20 +455,22 @@ int main(int argc, char *argv[]) {
   // Create a node handle
   ros::NodeHandle nh(std::string("/") + FLAGS_ns);
   // Setup SWITCH action
-  client.SetConnectedTimeout(FLAGS_connect);
-  client.SetActiveTimeout(FLAGS_active);
+  if (!FLAGS_remote) {
+    client.SetConnectedTimeout(FLAGS_connect);
+    client.SetActiveTimeout(FLAGS_active);
+    client.SetConnectedCallback(std::bind(ConnectedCallback, &client));
+  }
   client.SetResponseTimeout(FLAGS_response);
   if (FLAGS_deadline > 0)
     client.SetDeadlineTimeout(FLAGS_deadline);
   client.SetFeedbackCallback(std::bind(FeedbackCallback,
     std::placeholders::_1));
   client.SetResultCallback(std::bind(ResultCallback,
     std::placeholders::_1, std::placeholders::_2));
-  client.SetConnectedCallback(std::bind(ConnectedCallback, &client));
   client.Create(&nh, ACTION_BEHAVIORS_INSPECTION);
 
   // Configure panorama anomaly parameters
-  if (FLAGS_anomaly) {
+  if (FLAGS_anomaly && !FLAGS_remote) {
     ff_util::ConfigClient cfg(&nh, NODE_INSPECTION);
     cfg.Set<double>("target_distance", FLAGS_target_distance);
     cfg.Set<double>("min_distance", FLAGS_min_distance);
@@ -456,7 +489,7 @@ int main(int argc, char *argv[]) {
   }
 
   // Configure panorama inspection parameters
-  if (FLAGS_panorama) {
+  if (FLAGS_panorama && !FLAGS_remote) {
     ff_util::ConfigClient cfg(&nh, NODE_INSPECTION);
 
     if (FLAGS_panorama_mode == "") {
@@ -503,6 +536,17 @@ int main(int argc, char *argv[]) {
 
   // Start input thread
   boost::thread inp(GetInput, &client);
+
+  if (FLAGS_remote) {
+    ros::Rate loop_rate(10);
+    ros::Time start_time = ros::Time::now();
+
+    // Spin for 3 seconds
+    while (ros::Time::now() - start_time < ros::Duration(3.0))
+        loop_rate.sleep();
+
+    SendGoal(&client);
+  }
   // Synchronous mode
   while (!stopflag_) {
     ros::spinOnce();

diff --git a/astrobee/survey_manager/survey_dependencies/ros2_planning_system b/astrobee/survey_manager/survey_dependencies/ros2_planning_system
diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt
@@ -16,23 +16,83 @@
 # 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(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(isaac_action_node src/isaac_action_node.cpp)
+# Action for move
+add_executable(move_action_node src/move_action_node.cpp)
+target_link_libraries(move_action_node isaac_action_node ${catkin_LIBRARIES} )
+# Action for dock
+add_executable(dock_action_node src/dock_action_node.cpp)
+target_link_libraries(dock_action_node isaac_action_node ${catkin_LIBRARIES} )
+# Action for undock
+add_executable(undock_action_node src/undock_action_node.cpp)
+target_link_libraries(undock_action_node isaac_action_node ${catkin_LIBRARIES} )
+# Action for panorama
+add_executable(panorama_action_node src/panorama_action_node.cpp)
+target_link_libraries(panorama_action_node isaac_action_node ${catkin_LIBRARIES} )
+# Action for stereo
+add_executable(stereo_action_node src/stereo_action_node.cpp)
+target_link_libraries(stereo_action_node isaac_action_node ${catkin_LIBRARIES} )
+
+#############
+## Install ##
+#############
+
+# Allow other packages to use python scripts from this package
+catkin_python_setup()
+
+catkin_package()
+
+catkin_install_python(PROGRAMS
+  tools/survey_planner/command_astrobee
+  tools/survey_planner/monitor_astrobee
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+# Mark executables and/or libraries for installation
+install(TARGETS isaac_action_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Install actions
+install(TARGETS
+  move_action_node
+  dock_action_node
+  undock_action_node
+  panorama_action_node
+  stereo_action_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+foreach( dir pddl launch data)
    install( DIRECTORY ${dir}/
          DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir} )
 endforeach(dir)