diff --git a/rosplan_planning_system/CMakeLists.txt b/rosplan_planning_system/CMakeLists.txt index 884305b2e..9f603b35a 100644 --- a/rosplan_planning_system/CMakeLists.txt +++ b/rosplan_planning_system/CMakeLists.txt @@ -48,9 +48,11 @@ add_definitions(-Wno-deprecated-declarations) add_executable(problemInterface src/ProblemGeneration/ProblemInterface.cpp src/ProblemGeneration/PDDLProblemGenerator.cpp src/ProblemGeneration/ProblemGenerator.cpp src/ProblemGeneration/RDDLProblemGenerator.cpp + src/ProblemGeneration/HDDLProblemGenerator.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(panda_planner_interface src/PlannerInterface/PANDAPlannerInterface.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) @@ -73,6 +75,7 @@ add_executable(simulatedAction src/ActionInterface/RPSimulatedActionInterface.cp add_dependencies(problemInterface ${catkin_EXPORTED_TARGETS}) add_dependencies(popf_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(lama_planner_interface ${catkin_EXPORTED_TARGETS}) +add_dependencies(panda_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}) @@ -95,6 +98,7 @@ add_dependencies(simulatedAction ${catkin_EXPORTED_TARGETS}) 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(panda_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}) @@ -146,6 +150,7 @@ install(TARGETS problemInterface popf_planner_interface lama_planner_interface + panda_planner_interface ff_planner_interface metricff_planner_interface smt_planner_interface diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h new file mode 100644 index 000000000..d49a9db0f --- /dev/null +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h @@ -0,0 +1,36 @@ +#include "PlannerInterface.h" +#include +#include +#include +#include +#include +#include + +#ifndef KCL_PANDA_planner_interface +#define KCL_PANDA_planner_interface + +/** + * This file contains an interface to the planner. + */ +namespace KCL_rosplan { + + class PANDAPlannerInterface: public PlannerInterface + { + private: + + /* runs external commands */ + std::string runCommand(std::string cmd); + + protected: + + bool runPlanner(); + + public: + + PANDAPlannerInterface(ros::NodeHandle& nh); + virtual ~PANDAPlannerInterface(); + }; + +} // close namespace + +#endif diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h new file mode 100644 index 000000000..34ba6abe4 --- /dev/null +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h @@ -0,0 +1,17 @@ +#ifndef ROSPLAN_HDDL_PROBLEM_GENERATOR_H +#define ROSPLAN_HDDL_PROBLEM_GENERATOR_H + +#include "PDDLProblemGenerator.h" + +namespace KCL_rosplan { + + class HDDLProblemGenerator : public PDDLProblemGenerator { + protected: + virtual void makeProblem(std::ofstream &pFile); + void makeTasks(std::ofstream &pFile); + public: + HDDLProblemGenerator(const std::string& kb): PDDLProblemGenerator(kb) {}; + }; +} // close namespace + +#endif diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/PDDLProblemGenerator.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/PDDLProblemGenerator.h index 49cb614f5..3e600a8cc 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/PDDLProblemGenerator.h +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/PDDLProblemGenerator.h @@ -11,14 +11,14 @@ namespace KCL_rosplan { class PDDLProblemGenerator : public ProblemGenerator { - private: - void makeHeader(std::ofstream &pFile); - void makeInitialState(std::ofstream &pFile); - void makeGoals(std::ofstream &pFile); - void makeMetric(std::ofstream &pFile); - void printExpression(std::ofstream &pFile, rosplan_knowledge_msgs::ExprComposite &e); + protected: + virtual void makeHeader(std::ofstream &pFile); + virtual void makeInitialState(std::ofstream &pFile); + virtual void makeGoals(std::ofstream &pFile); + virtual void makeMetric(std::ofstream &pFile); + virtual void printExpression(std::ofstream &pFile, rosplan_knowledge_msgs::ExprComposite &e); - void makeProblem(std::ofstream &pFile); + virtual void makeProblem(std::ofstream &pFile); public: PDDLProblemGenerator(const std::string& kb): ProblemGenerator(kb) {}; }; diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGenerator.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGenerator.h index 83895abaf..eb3d0e99f 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGenerator.h +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGenerator.h @@ -16,6 +16,7 @@ #include "rosplan_knowledge_msgs/GetDomainAttributeService.h" #include "rosplan_knowledge_msgs/GetInstanceService.h" #include "rosplan_knowledge_msgs/GetAttributeService.h" +#include "rosplan_knowledge_msgs/GetTaskService.h" #include "rosplan_knowledge_msgs/GetMetricService.h" #ifndef KCL_ProblemGenerator @@ -39,6 +40,7 @@ namespace KCL_rosplan { std::string state_function_service; std::string state_timed_knowledge_service; std::string state_goal_service; + std::string task_service; std::string state_metric_service; virtual void makeProblem(std::ofstream &pFile) =0; diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGeneratorFactory.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGeneratorFactory.h index fc86193b1..a870138f2 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGeneratorFactory.h +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGeneratorFactory.h @@ -8,6 +8,7 @@ #include "ProblemGenerator.h" #include "PDDLProblemGenerator.h" #include "RDDLProblemGenerator.h" +#include "HDDLProblemGenerator.h" namespace KCL_rosplan { typedef std::unique_ptr ProblemGeneratorPtr; @@ -16,7 +17,8 @@ namespace KCL_rosplan { public: enum ProbGen { PDDL, - RDDL + RDDL, + HDDL }; static ProblemGeneratorPtr createProblemGenerator(ProbGen pg_type, const std::string& kb); diff --git a/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp new file mode 100644 index 000000000..a2f3f32a7 --- /dev/null +++ b/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp @@ -0,0 +1,139 @@ +#include "rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h" + +namespace KCL_rosplan { + + /*-------------*/ + /* constructor */ + /*-------------*/ + + PANDAPlannerInterface::PANDAPlannerInterface(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(); + } + + PANDAPlannerInterface::~PANDAPlannerInterface() + { + delete plan_server; + } + + /** + * Runs external commands + */ + std::string PANDAPlannerInterface::runCommand(std::string cmd) { + 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 PANDAPlannerInterface::runPlanner() { + + // save problem to file for PANDA + 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(); + } + + // 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); + std::string commandString = str + " > " + data_path + "plan.hddl"; + + // call the planer + 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()); + ROS_INFO("KCL: (%s) (%s) Planning complete", ros::this_node::getName().c_str(), problem_name.c_str()); + + // check the planner solved the problem + std::ifstream planfile; + planfile.open((data_path + "plan.hddl").c_str()); + std::string line; + std::stringstream ss; + + int curr, next; + bool solved = false; + double planDuration; + + while (std::getline(planfile, line)) { + if (line.find("SOLUTION SEQUENCE", 0) != std::string::npos) { + solved = true; + } else { + // if a solution has been found, we read the plan + if (solved) + { + // read a plan (might not be the last plan) + planDuration = 0; + ss.str(""); + while (std::getline(planfile, line)) { + if (line.length()<2) + break; + ss << line << std::endl; + } + planner_output = ss.str(); + } + } + } + planfile.close(); + + 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; + } + +} // 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::PANDAPlannerInterface 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; + } diff --git a/rosplan_planning_system/src/ProblemGeneration/HDDLProblemGenerator.cpp b/rosplan_planning_system/src/ProblemGeneration/HDDLProblemGenerator.cpp new file mode 100644 index 000000000..64b23de97 --- /dev/null +++ b/rosplan_planning_system/src/ProblemGeneration/HDDLProblemGenerator.cpp @@ -0,0 +1,39 @@ +#include "rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h" + +namespace KCL_rosplan { + void HDDLProblemGenerator::makeTasks(std::ofstream &pFile) { + ros::NodeHandle nh; + ros::ServiceClient getCurrentTasksClient = nh.serviceClient(task_service); + + pFile << "(:htn\n :tasks (and" << std::endl; + + // get current tasks + rosplan_knowledge_msgs::GetTaskService currentTaskSrv; + if (!getCurrentTasksClient.call(currentTaskSrv)) { + ROS_ERROR("KCL: (HDDLProblemGenerator) Failed to call service %s", task_service.c_str()); + } else { + for(size_t i=0;i 5) ? problem_path.substr(problem_path.find_last_of('.')) : ""; if (extension == ".pddl" or extension == ".ppddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::PDDL; else if (extension == ".rddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::RDDL; + else if (extension == ".hddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::HDDL; else { ROS_ERROR("KCL: (%s) Unexpected problem file extension %s. Please provide a problem file written in PDDL (.pddl extension) or RDDL (.rddl extension).", ros::this_node::getName().c_str(), extension.c_str());