From 48d42b3b1b3d281707269609ad8af3826bb763e2 Mon Sep 17 00:00:00 2001 From: Dharmin Date: Sun, 5 Jan 2020 02:04:19 +0100 Subject: [PATCH 1/2] [planner interface] [lama] Added lama planner interface --- rosplan_planning_system/CMakeLists.txt | 4 + .../PlannerInterface/LAMAPlannerInterface.h | 39 ++++ .../PlannerInterface/LAMAPlannerInterface.cpp | 166 ++++++++++++++++++ 3 files changed, 209 insertions(+) create mode 100644 rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/LAMAPlannerInterface.h create mode 100644 rosplan_planning_system/src/PlannerInterface/LAMAPlannerInterface.cpp diff --git a/rosplan_planning_system/CMakeLists.txt b/rosplan_planning_system/CMakeLists.txt index a8933214b..422102a68 100644 --- a/rosplan_planning_system/CMakeLists.txt +++ b/rosplan_planning_system/CMakeLists.txt @@ -50,6 +50,7 @@ add_executable(problemInterface src/ProblemGeneration/ProblemInterface.cpp src/P src/ProblemGeneration/RDDLProblemGenerator.cpp src/ProblemGeneration/ProblemGeneratorFactory.cpp) add_executable(popf_planner_interface src/PlannerInterface/POPFPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) +add_executable(lama_planner_interface src/PlannerInterface/LAMAPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(ff_planner_interface src/PlannerInterface/FFPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(rddlsim_planner_interface src/PlannerInterface/RDDLSIMPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(metricff_planner_interface src/PlannerInterface/FFPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) @@ -70,6 +71,7 @@ add_executable(simulatedAction src/ActionInterface/RPSimulatedActionInterface.cp ## Add dependencies add_dependencies(problemInterface ${catkin_EXPORTED_TARGETS}) add_dependencies(popf_planner_interface ${catkin_EXPORTED_TARGETS}) +add_dependencies(lama_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(ff_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(rddlsim_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(metricff_planner_interface ${catkin_EXPORTED_TARGETS}) @@ -90,6 +92,7 @@ add_dependencies(simulatedAction ${catkin_EXPORTED_TARGETS}) ## Specify libraries against which to link a library or executable target target_link_libraries(problemInterface ${catkin_LIBRARIES}) target_link_libraries(popf_planner_interface ${catkin_LIBRARIES}) +target_link_libraries(lama_planner_interface ${catkin_LIBRARIES}) target_link_libraries(ff_planner_interface ${catkin_LIBRARIES}) target_link_libraries(rddlsim_planner_interface ${catkin_LIBRARIES}) target_link_libraries(metricff_planner_interface ${catkin_LIBRARIES}) @@ -139,6 +142,7 @@ install(DIRECTORY launch/ install(TARGETS problemInterface popf_planner_interface + lama_planner_interface ff_planner_interface metricff_planner_interface smt_planner_interface diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/LAMAPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/LAMAPlannerInterface.h new file mode 100644 index 000000000..6c7a4435a --- /dev/null +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/LAMAPlannerInterface.h @@ -0,0 +1,39 @@ +#include "PlannerInterface.h" +#include +#include +#include +#include +#include +#include + +#ifndef KCL_LAMA_planner_interface +#define KCL_LAMA_planner_interface + +/** + * This file contains an interface to the planner. + */ +namespace KCL_rosplan { + + class LAMAPlannerInterface: public PlannerInterface + { + private: + + /* runs external commands */ + std::string runCommand(std::string cmd); + + /* remove all white space from a string */ + std::string strip(std::string str); + + protected: + + bool runPlanner(); + + public: + + LAMAPlannerInterface(ros::NodeHandle& nh); + virtual ~LAMAPlannerInterface(); + }; + +} // close namespace + +#endif diff --git a/rosplan_planning_system/src/PlannerInterface/LAMAPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/LAMAPlannerInterface.cpp new file mode 100644 index 000000000..d559f2015 --- /dev/null +++ b/rosplan_planning_system/src/PlannerInterface/LAMAPlannerInterface.cpp @@ -0,0 +1,166 @@ +#include "rosplan_planning_system/PlannerInterface/LAMAPlannerInterface.h" + +namespace KCL_rosplan { + + /*-------------*/ + /* constructor */ + /*-------------*/ + + LAMAPlannerInterface::LAMAPlannerInterface(ros::NodeHandle& nh) + { + node_handle = &nh; + + plan_server = new actionlib::SimpleActionServer((*node_handle), "start_planning", boost::bind(&PlannerInterface::runPlanningServerAction, this, _1), false); + + // publishing raw planner output + std::string plannerTopic = "planner_output"; + node_handle->getParam("planner_topic", plannerTopic); + plan_publisher = node_handle->advertise(plannerTopic, 1, true); + + // start planning action server + plan_server->start(); + } + + LAMAPlannerInterface::~LAMAPlannerInterface() + { + delete plan_server; + } + + /** + * Runs external commands + */ + std::string LAMAPlannerInterface::runCommand(std::string cmd) { + /* the command should be as follows */ + /* EXECUTABLE --search-time-limit TIMELIMIT --alias seq-sat-lama-2011 --plan-file DATA_PATH DOMAIN PROBLEM */ + std::string data; + FILE *stream; + char buffer[1000]; + stream = popen(cmd.c_str(), "r"); + while ( fgets(buffer, 1000, stream) != NULL ) + data.append(buffer); + pclose(stream); + return data; + } + + /*------------------*/ + /* Plan and process */ + /*------------------*/ + + /** + * passes the problem to the Planner; the plan to post-processing. + */ + bool LAMAPlannerInterface::runPlanner() { + + // save problem to file for LAMA + if(use_problem_topic && problem_instance_received) { + ROS_INFO("KCL: (%s) (%s) Writing problem to file.", ros::this_node::getName().c_str(), problem_name.c_str()); + std::ofstream dest; + dest.open((problem_path).c_str()); + dest << problem_instance; + dest.close(); + } + + /* create a temporary directory in data_path directory */ + std::string cd_cmd = "mkdir -p " + data_path + "plan"; + std::string cd_cmd_output = runCommand(cd_cmd.c_str()); + + // prepare the planner command line + std::string str = planner_command; + std::size_t dit = str.find("DOMAIN"); + if(dit!=std::string::npos) str.replace(dit,6,domain_path); + std::size_t pit = str.find("PROBLEM"); + if(pit!=std::string::npos) str.replace(pit,7,problem_path); + /* change current directory to the temp directory then call planner */ + std::string commandString = "cd " + data_path + "plan/; " + str + " > " + data_path + "plan.pddl"; + + // call the planner + ROS_INFO("KCL: (%s) (%s) Running: %s", ros::this_node::getName().c_str(), problem_name.c_str(), commandString.c_str()); + std::string plan = runCommand(commandString.c_str()); + std::cout << plan << std::endl; + ROS_INFO("KCL: (%s) (%s) Planning complete", ros::this_node::getName().c_str(), problem_name.c_str()); + + /* get the best plan file (LAMA generates multiple plans) */ + std::string best_plan_cmd = "ls " + data_path + "plan | grep sas_plan | tail -n 1"; + std::string best_plan_cmd_output = runCommand(best_plan_cmd.c_str()); + std::string best_plan_file = strip(best_plan_cmd_output); + std::cout << "Best plan is:" << best_plan_file << ";" << std::endl; + + // check if planner solved the problem or not + bool solved = best_plan_file.length() > 0; + + if (solved) { + std::ifstream planfile; + planfile.open((data_path + "plan/" + best_plan_file).c_str()); + std::stringstream ss; + ss << planfile.rdbuf(); + planner_output = ss.str(); + planfile.close(); + } + + /* remove the temporary directory and all files inside it */ + std::string rm_cmd = "rm -r " + data_path + "plan"; + std::string rm_cmd_output = runCommand(rm_cmd.c_str()); + + + if(!solved) ROS_INFO("KCL: (%s) (%s) Plan was unsolvable.", ros::this_node::getName().c_str(), problem_name.c_str()); + else ROS_INFO("KCL: (%s) (%s) Plan was solved.", ros::this_node::getName().c_str(), problem_name.c_str()); + + return solved; + } + + /** + * strips all white characters from a string + * source: https://stackoverflow.com/a/58090517/10460994 + */ + std::string LAMAPlannerInterface::strip(std::string str){ + if (!(str.length() == 0)) { + auto w = std::string(" ") ; + auto n = std::string("\n") ; + auto t = std::string("\t") ; + auto r = std::string("\r") ; + auto v = std::string(1, str.front()); + while((v == w) or (v==t) or (v==r) or (v==n)) { + str.erase(str.begin()); + v = std::string(1, str.front()); + } + + v = std::string(1 , str.back()); + + while((v ==w) or (v==t) or (v==r) or (v==n)) { + str.erase(str.end() - 1); + v = std::string(1, str.back()); + } + } + return str; + } + + +} // close namespace + + /*-------------*/ + /* Main method */ + /*-------------*/ + + int main(int argc, char **argv) { + + srand (static_cast (time(0))); + + ros::init(argc,argv,"rosplan_planner_interface"); + ros::NodeHandle nh("~"); + + KCL_rosplan::LAMAPlannerInterface pi(nh); + + // subscribe to problem instance + std::string problemTopic = "problem_instance"; + nh.getParam("problem_topic", problemTopic); + ros::Subscriber problem_sub = nh.subscribe(problemTopic, 1, &KCL_rosplan::PlannerInterface::problemCallback, dynamic_cast(&pi)); + + // start the planning services + ros::ServiceServer service1 = nh.advertiseService("planning_server", &KCL_rosplan::PlannerInterface::runPlanningServerDefault, dynamic_cast(&pi)); + ros::ServiceServer service2 = nh.advertiseService("planning_server_params", &KCL_rosplan::PlannerInterface::runPlanningServerParams, dynamic_cast(&pi)); + + ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str()); + ros::spin(); + + return 0; + } From d9448a5e0de80a3ba42cc9efedd0ff83f1da4796 Mon Sep 17 00:00:00 2001 From: Dharmin Date: Sun, 5 Jan 2020 03:46:59 +0100 Subject: [PATCH 2/2] [plan parser] [lama] Added plan parser for lama (more basic than simple parser) --- rosplan_planning_system/CMakeLists.txt | 4 + .../PlanParsing/PDDLLAMAPlanParser.h | 46 ++++++ .../src/PlanParsing/PDDLLAMAPlanParser.cpp | 155 ++++++++++++++++++ 3 files changed, 205 insertions(+) create mode 100644 rosplan_planning_system/include/rosplan_planning_system/PlanParsing/PDDLLAMAPlanParser.h create mode 100644 rosplan_planning_system/src/PlanParsing/PDDLLAMAPlanParser.cpp diff --git a/rosplan_planning_system/CMakeLists.txt b/rosplan_planning_system/CMakeLists.txt index 422102a68..884305b2e 100644 --- a/rosplan_planning_system/CMakeLists.txt +++ b/rosplan_planning_system/CMakeLists.txt @@ -62,6 +62,7 @@ add_executable(upm_planner_interface src/PlannerInterface/UPMPlannerInterface.cp add_executable(pprp_planner_interface src/PlannerInterface/PPRPPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(online_planner_interface src/PlannerInterface/OnlinePlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(pddl_simple_plan_parser src/PlanParsing/PDDLSimplePlanParser.cpp src/PlanParsing/PlanParser.cpp) +add_executable(pddl_lama_plan_parser src/PlanParsing/PDDLLAMAPlanParser.cpp src/PlanParsing/PlanParser.cpp) add_executable(pddl_esterel_plan_parser src/PlanParsing/PDDLEsterelPlanParser.cpp src/PlanParsing/PlanParser.cpp) add_executable(pddl_simple_plan_dispatcher src/PlanDispatch/SimplePlanDispatcher.cpp src/PlanDispatch/PlanDispatcher.cpp) add_executable(pddl_esterel_plan_dispatcher src/PlanDispatch/EsterelPlanDispatcher.cpp src/PlanDispatch/PlanDispatcher.cpp) @@ -83,6 +84,7 @@ add_dependencies(upm_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(pprp_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(online_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(pddl_simple_plan_parser ${catkin_EXPORTED_TARGETS}) +add_dependencies(pddl_lama_plan_parser ${catkin_EXPORTED_TARGETS}) add_dependencies(pddl_esterel_plan_parser ${catkin_EXPORTED_TARGETS}) add_dependencies(pddl_simple_plan_dispatcher ${catkin_EXPORTED_TARGETS}) add_dependencies(pddl_esterel_plan_dispatcher ${catkin_EXPORTED_TARGETS}) @@ -104,6 +106,7 @@ target_link_libraries(upm_planner_interface ${catkin_LIBRARIES}) target_link_libraries(pprp_planner_interface ${catkin_LIBRARIES}) target_link_libraries(online_planner_interface ${catkin_LIBRARIES}) target_link_libraries(pddl_simple_plan_parser ${catkin_LIBRARIES}) +target_link_libraries(pddl_lama_plan_parser ${catkin_LIBRARIES}) target_link_libraries(pddl_esterel_plan_parser ${catkin_LIBRARIES}) target_link_libraries(pddl_simple_plan_dispatcher ${catkin_LIBRARIES}) target_link_libraries(pddl_esterel_plan_dispatcher ${catkin_LIBRARIES}) @@ -152,6 +155,7 @@ install(TARGETS upm_planner_interface pprp_planner_interface pddl_simple_plan_parser + pddl_lama_plan_parser pddl_esterel_plan_parser pddl_simple_plan_dispatcher pddl_esterel_plan_dispatcher diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlanParsing/PDDLLAMAPlanParser.h b/rosplan_planning_system/include/rosplan_planning_system/PlanParsing/PDDLLAMAPlanParser.h new file mode 100644 index 000000000..06502328c --- /dev/null +++ b/rosplan_planning_system/include/rosplan_planning_system/PlanParsing/PDDLLAMAPlanParser.h @@ -0,0 +1,46 @@ +/** + * This file parses standard PDDL output and generates a list of ActionDispatch messages. + */ +#include "PlanParser.h" + +#include "rosplan_dispatch_msgs/ActionDispatch.h" +#include "rosplan_dispatch_msgs/CompletePlan.h" +#include "rosplan_knowledge_msgs/GetDomainOperatorDetailsService.h" +#include "diagnostic_msgs/KeyValue.h" + +#include +#include + +#ifndef KCL_pddl_lama_plan_parser +#define KCL_pddl_lama_plan_parser + +namespace KCL_rosplan { + + class PDDLLAMAPlanParser: public PlanParser + { + private: + + ros::ServiceClient get_operator_details_client; + + protected: + + /* post process plan */ + void processPDDLParameters(rosplan_dispatch_msgs::ActionDispatch &msg, std::vector ¶ms); + + /* virtual methods */ + void reset(); + void preparePlan(); + void publishPlan(); + + public: + + PDDLLAMAPlanParser(ros::NodeHandle& nh); + ~PDDLLAMAPlanParser(); + + /* ROS interface */ + ros::Publisher plan_publisher; + + }; +} // close namespace + +#endif diff --git a/rosplan_planning_system/src/PlanParsing/PDDLLAMAPlanParser.cpp b/rosplan_planning_system/src/PlanParsing/PDDLLAMAPlanParser.cpp new file mode 100644 index 000000000..ca11bc5dd --- /dev/null +++ b/rosplan_planning_system/src/PlanParsing/PDDLLAMAPlanParser.cpp @@ -0,0 +1,155 @@ +#include "rosplan_planning_system/PlanParsing/PDDLLAMAPlanParser.h" + +namespace KCL_rosplan { + + PDDLLAMAPlanParser::PDDLLAMAPlanParser(ros::NodeHandle& nh) + { + node_handle = &nh; + + // fetching problem info for TILs + std::string kb = "knowledge_base"; + node_handle->getParam("knowledge_base", kb); + + std::stringstream ss; + ss << "/" << kb << "/domain/operator_details"; + get_operator_details_client = nh.serviceClient(ss.str().c_str()); + ss.str(""); + + // publishing parsed plan + std::string planTopic = "complete_plan"; + node_handle->getParam("plan_topic", planTopic); + plan_publisher = node_handle->advertise(planTopic, 1, true); + } + + PDDLLAMAPlanParser::~PDDLLAMAPlanParser() + { + + } + + void PDDLLAMAPlanParser::reset() { + action_list.clear(); + } + + void PDDLLAMAPlanParser::publishPlan() { + rosplan_dispatch_msgs::CompletePlan msg; + msg.plan = action_list; + plan_publisher.publish(msg); + } + + /*----------------------*/ + /* Post processing plan */ + /*----------------------*/ + + /** + * parses plan generated by LAMA planner, generating a list of action messages. + */ + void PDDLLAMAPlanParser::preparePlan() { + + int curr, next; + std::string line; + std::istringstream planfile(planner_output); + + size_t planFreeActionID = 0; + + while (std::getline(planfile, line)) { + + if (line.length() < 2) + break; + + // check to see if the line is a comment + if (line.find(";", 0) == 0) + continue; + + rosplan_dispatch_msgs::ActionDispatch msg; + + // action ID + msg.action_id = planFreeActionID; + planFreeActionID++; + + // check for parameters + curr = line.find("(") + 1; + bool paramsExist = (line.find(" ", curr) < line.find(")", curr)); + + if(paramsExist) { + + // name + next = line.find(" ", curr); + std::string name = line.substr(curr, next-curr).c_str(); + msg.name = name; + + // parameters + std::vector params; + curr = next + 1; + next = line.find(")", curr); + int at = curr; + while(at < next) { + int cc = line.find(" ", curr); + int cc1 = line.find(")", curr); + curr = cc < cc1 && cc != -1 ? cc : cc1; + std::string param = line.substr(at, curr-at); + params.push_back(param); + ++curr; + at = curr; + } + processPDDLParameters(msg, params); + + + } else { + + // name + next=line.find(")",curr); + std::string name = line.substr(curr,next-curr).c_str(); + msg.name = name; + + } + + action_list.push_back(msg); + } + } + + /** + * processes the parameters of a single PDDL action into an ActionDispatch message + */ + void PDDLLAMAPlanParser::processPDDLParameters(rosplan_dispatch_msgs::ActionDispatch &msg, std::vector ¶ms) { + + rosplan_knowledge_msgs::GetDomainOperatorDetailsService srv; + srv.request.name = msg.name; + if(!get_operator_details_client.call(srv)) { + ROS_ERROR("KCL: (%s) could not call Knowledge Base for operator details, %s", ros::this_node::getName().c_str(), msg.name.c_str()); + } else { + std::vector opParams = srv.response.op.formula.typed_parameters; + for(size_t i=0; i(&pp)); + + // start the plan parsing services + ros::ServiceServer service1 = nh.advertiseService("parse_plan", &KCL_rosplan::PlanParser::parsePlan, dynamic_cast(&pp)); + ros::ServiceServer service2 = nh.advertiseService("parse_plan_from_file", &KCL_rosplan::PlanParser::parsePlanFromFile, dynamic_cast(&pp)); + + ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str()); + ros::spin(); + + return 0; + }