diff --git a/astrobee/survey_manager/survey_planner/CMakeLists.txt b/astrobee/survey_manager/survey_planner/CMakeLists.txt index 46c15a67..7850f60e 100644 --- a/astrobee/survey_manager/survey_planner/CMakeLists.txt +++ b/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 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 025b9ae2..330e72c0 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 @@ -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_; 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 686fcd7b..4ab1d44a 100644 --- a/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp +++ b/astrobee/survey_manager/survey_planner/src/isaac_action_node.cpp @@ -21,55 +21,176 @@ #include +#include +#include +#include #include #include +#include #include +#include +#include #include #include -#include #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(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 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& 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& 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; @@ -77,29 +198,31 @@ void IsaacAction::do_work() { } } - 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"); } } } diff --git a/astrobee/survey_manager/survey_planner/src/survey_planner/pddl_query.py b/astrobee/survey_manager/survey_planner/src/survey_planner/pddl_query.py new file mode 100755 index 00000000..d9fe95c3 --- /dev/null +++ b/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 + +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() diff --git a/astrobee/survey_manager/survey_planner/tools/pddl_query b/astrobee/survey_manager/survey_planner/tools/pddl_query new file mode 100644 index 00000000..c243960f --- /dev/null +++ b/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()