From 48d42b3b1b3d281707269609ad8af3826bb763e2 Mon Sep 17 00:00:00 2001 From: Dharmin Date: Sun, 5 Jan 2020 02:04:19 +0100 Subject: [PATCH] [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; + }