Improve action node feedback (#141)

astrobee/survey_manager/survey_planner/CMakeLists.txt

@@ -74,6 +74,7 @@ catkin_package()
 catkin_install_python(PROGRAMS
   tools/command_astrobee
   tools/monitor_astrobee
+  tools/pddl_query
   tools/plan_interpreter
   tools/plan_survey
   tools/problem_generator

astrobee/survey_manager/survey_planner/include/survey_planner/isaac_action_node.h

@@ -34,6 +34,8 @@ class IsaacAction : public plansys2::ActionExecutorClient {
  protected:
   void do_work();
 
+  double action_duration_;
+  ros::Time start_time_;
   float progress_;
   std::string robot_name_, action_name_;
   int pid_;

astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp

@@ -21,85 +21,208 @@
 
 #include <plansys2_executor/ActionExecutorClient.hpp>
 
+#include <stdio.h>
+#include <sys/resource.h>
+#include <sys/time.h>
 #include <sys/types.h>
 #include <sys/wait.h>
+#include <unistd.h>
 
 #include <algorithm>
+#include <array>
+#include <iostream>
 #include <string>
 #include <vector>
-#include <iostream>
 
 #include "survey_planner/isaac_action_node.h"
 
 namespace plansys2_actions {
 
+// Class for running commands and collecting their stdout output, roughly following Python
+// subprocess.Popen but focused on one special case.
+class PopenReader {
+ public:
+  // Start running `cmd` using /bin/sh, inheriting the current process's environment variables. An
+  // internal reader pipe will be used to collect its stdout output.
+  explicit PopenReader(const char* cmd) : reader_(nullptr), child_pid_(-1), child_status_(127) {
+    int pipe_fds[2];
+    if (pipe(pipe_fds) < 0) {
+      return;
+    }
+    const char* argv[4];
+    argv[0] = "sh";
+    argv[1] = "-c";
+    argv[2] = cmd;
+    argv[3] = nullptr;
+
+    int pid;
+    switch (pid = fork()) {
+      case -1:  // fork error
+        close(pipe_fds[0]);
+        close(pipe_fds[1]);
+        return;
+      case 0:  // child
+        if (pipe_fds[1] != STDOUT_FILENO) {
+          dup2(pipe_fds[1], STDOUT_FILENO);
+        }
+        {
+          int fdlimit = static_cast<int>(sysconf(_SC_OPEN_MAX));
+          for (int i = STDERR_FILENO + 1; i < fdlimit; i++) {
+            close(i);
+          }
+        }
+        execve("/bin/sh", (char* const*)argv, environ);
+        exit(127);  // execve() error, exit child
+      default:      // parent
+        close(pipe_fds[1]);
+        reader_ = fdopen(pipe_fds[0], "r");
+        child_pid_ = pid;
+    }
+  }
+
+  // Collect stdout output from running child process and wait for it to terminate. Does
+  // nothing if reader pipe wasn't successfully opened.
+  void communicate() {
+    if (reader_ == nullptr) {
+      return;
+    }
+
+    std::array<char, 128> chunk;
+    while (fgets(chunk.data(), chunk.size(), reader_) != nullptr) {
+      stdout_ += chunk.data();
+    }
+    fclose(reader_);
+
+    int wait_result;
+    int status;
+    do {
+      wait_result = wait4(child_pid_, &status, 0, nullptr);
+    } while (wait_result == -1 && errno == EINTR);
+    child_status_ = status;
+  }
+
+  // Run `cmd` using /bin/sh, inheriting the current process's environment variables, wait for child
+  // to terminate, and return its status. Set `stdout` to child's stdout output, if any.  Return 127
+  // in case of internal failures prior to starting child. Roughly similar to Python
+  // subprocess.getstatusoutput().
+  static int get_status_output(const char* cmd, std::string& stdout) {
+    PopenReader proc(cmd);
+    proc.communicate();
+    stdout = proc.stdout_;
+    return proc.child_status_;
+  }
+
+ protected:
+  FILE* reader_;
+  pid_t child_pid_;
+  int child_status_;
+  std::string stdout_;
+};
+
+// Return the estimated duration (seconds) of action `action_name`. Queried from PDDL model. On
+// query error, return a default value of one minute.
+double get_action_duration(const std::string& action_name) {
+  std::string cmd = std::string("rosrun survey_planner pddl_query action_duration ") + action_name;
+  printf("get_action_duration %s: running query: %s\n", action_name.c_str(), cmd.c_str());
+
+  const double default_duration = 60.0;  // one minute
+  std::string duration_buf;
+  int query_status = PopenReader::get_status_output(cmd.c_str(), duration_buf);
+
+  if (query_status != EXIT_SUCCESS) {
+    printf("get_action_duration %s: query returned with non-zero exit code %d, using default duration of %.1lf",
+           action_name.c_str(), query_status, default_duration);
+    return default_duration;
+  }
+
+  char* ending;
+  double duration = strtod(duration_buf.c_str(), &ending);
+
+  if (ending == duration_buf.c_str()) {
+    printf("get_action_duration %s: couldn't parse query output '%s', using default duration of %.1lf\n",
+           action_name.c_str(), duration_buf.c_str(), default_duration);
+    return default_duration;
+  }
+
+  printf("get_action_duration %s: estimated duration is %.1lf\n", action_name.c_str(), duration);
+  return duration;
+}
+
 IsaacAction::IsaacAction(ros::NodeHandle nh, const std::string& action, const std::chrono::nanoseconds& rate)
     : ActionExecutorClient(nh, action, rate) {
   action_name_ = action;
   progress_ = 0.0;
   pid_ = 0;
   command_ = "";
+
+  // Get estimated action duration
+  action_duration_ = get_action_duration(action_name_);
 }
 
 void IsaacAction::do_work() {
   std::string from, towards;
 
-  const std::vector<std::string>& command_args = get_arguments();
-  if (command_args.size() < 3) {
-    finish(false, 1.0, "Not enough arguments for [MOVE] command");
-  }
-
   // Start process if not started yet
   if (progress_ == 0.0) {
+    const std::vector<std::string>& command_args = get_arguments();
+    if (command_args.size() < 3) {
+      finish(false, 1.0, "Not enough arguments for [MOVE] command");
+    }
+
+    std::string args_str = action_name_;
+    for (auto arg : command_args) {
+      args_str += " " + arg;
+    }
+    command_ = std::string("(") + args_str + ")";
+    std::string command_astrobee_call = std::string("rosrun survey_planner command_astrobee ") + args_str;
+
+    start_time_ = ros::Time::now();
     pid_ = fork();
     if (pid_ < 0) {
-      perror("Fork failed.");
+      perror("isaac_action_node: Fork failed");
       finish(false, 1.0, "Failed to start the process");
-    } else if (pid_ == 0) {
+    } else if (pid_ == 0) {  // child
       const char* args[4];
       args[0] = "sh";
       args[1] = "-c";
-      command_ = "rosrun survey_planner command_astrobee ";
-      command_ += action_name_ + " ";
-      for (unsigned int i = 0; i < command_args.size(); i++) {
-          command_ += command_args[i] + " ";
-      }
-      args[2] = command_.c_str();
+      args[2] = command_astrobee_call.c_str();
       args[3] = NULL;
-      printf("%s\n", args[2]);
+      printf("isaac_action_node: Running: %s\n", args[2]);
       execvpe("sh", (char* const*)args, environ);
-      perror("Failed to execute command.");
-      printf("EXITING FAILURE %d\n", getpid());
+      perror("isaac_action_node: Failed to execute command");
+      printf("isaac_action_node: %s: EXITING FAILURE %d\n", command_.c_str(), getpid());
       exit(-1);
     } else {
       progress_ = 0.02;
       return;
     }
   }
 
-  if (progress_ < 1.0) {
-    progress_ += 0.02;
-    send_feedback(progress_, action_name_ + " running");
-  }
+  // Note: This progress metric can intentionally exceed 1.0 if the action takes longer than
+  // expected. This is useful because continued changes in progress verify that the action node is
+  // still ticking, and the operator can tell at a glance that the action is running long.
+  // (Hopefully it doesn't violate any assumptions in PlanSys2.)
+  progress_ = (ros::Time::now() - start_time_).toSec() / action_duration_;
+  send_feedback(progress_, command_ + " running");
 
-  std::cout << "\t ** [" << action_name_ << "] " << command_ << " [" << std::min(100.0, progress_ * 100.0) << "%]  "
-            << std::endl;
+  printf("\t ** %s [%5.1f%%]  \n", command_.c_str(), progress_ * 100.0);
   int status;
   int result = waitpid(-1, &status, WNOHANG);
-  printf("Result: %d %d %d\n", result, pid_, status);
+  // printf("Result: %d %d %d\n", result, pid_, status);
   if (result < 0) {
-    perror("Failed to wait for pid.");
+    perror("isaac_action_node: Failed to wait for pid");
     progress_ = 0.0;
     finish(false, 1.0, "Unexpected error waiting for process.");
   } else if (result == pid_) {
     if (status == 0) {
-      std::cout << "Command exited with status success " << std::endl;
+      std::cout << "isaac_action_node: " << command_ << ": Success " << std::endl;
       progress_ = 0.0;
-      finish(true, 1.0, action_name_ + " completed");
+      finish(true, 1.0, command_ + " completed");
     } else {
-      std::cout << "Command terminated with status fail:  " << status << std::endl;
+      std::cout << "isaac_action_node: " << command_ << " : Command terminated with non-zero exit code:  " << status
+                << std::endl;
       progress_ = 0.0;
-      finish(false, 1.0, action_name_ + " terminated by signal");
+      finish(false, 1.0, command_ + " terminated by signal");
     }
   }
 }

astrobee/survey_manager/survey_planner/src/survey_planner/pddl_query.py

@@ -0,0 +1,106 @@
+#!/usr/bin/env python3
+
+# 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.
+
+"""
+Answers a query about the PDDL domain.
+
+Available query types:
+- action_duration <action_name>
+
+Example:
+$ pddl_query.py --domain=domain_survey.pddl action_duration panorama
+"""
+
+import argparse
+import enum
+import pathlib
+from typing import List
+
+from survey_planner.plan_survey import get_action_durations, parse_pddl
+from survey_planner.problem_generator import PDDL_DIR
+
+
+@enum.unique
+class QueryType(enum.Enum):
+    "Represents available query types."
+    ACTION_DURATION = "action_duration"
+
+
+QUERY_TYPES = [qtype.value for qtype in QueryType]
+
+
+def pddl_query(
+    domain_path: pathlib.Path,
+    query_type: QueryType,
+    query_args: List[str],
+) -> None:
+    """
+    The main function that answers a query.
+    """
+    if query_type == QueryType.ACTION_DURATION:
+        if len(query_args) != 1:
+            raise ValueError(
+                "action_duration query requires exactly 1 argument (the action name)"
+            )
+        [action_name] = query_args
+        durations = get_action_durations(parse_pddl(domain_path))
+        if action_name not in durations:
+            raise KeyError(
+                f"Expected action_name in {list(durations.keys())}, got '{action_name}'"
+            )
+        print(durations[action_name])
+    else:
+        assert False, "Never reach this point."
+
+
+class CustomFormatter(
+    argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter
+):
+    "Custom formatter for argparse that combines mixins."
+
+
+def main() -> None:
+    "Parse arguments and invoke pddl_query()."
+    parser = argparse.ArgumentParser(
+        description=__doc__, formatter_class=CustomFormatter
+    )
+    parser.add_argument(
+        "--domain",
+        help="path for input PDDL domain",
+        type=pathlib.Path,
+        default=PDDL_DIR / "domain_survey.pddl",
+    )
+    parser.add_argument(
+        "query_arg",
+        help="query type and arguments",
+        nargs="+",
+    )
+    args = parser.parse_args()
+
+    try:
+        query_type = QueryType(args.query_arg[0])
+    except ValueError:
+        parser.error(f"Expected query type in {QUERY_TYPES}, got '{args.query_arg[0]}'")
+
+    pddl_query(args.domain, query_type, args.query_arg[1:])
+
+
+if __name__ == "__main__":
+    main()

astrobee/survey_manager/survey_planner/tools/pddl_query

@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2021, 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.
+
+import sys
+
+from survey_planner import pddl_query
+
+if __name__ == "__main__":
+    pddl_query.main()