From 1eb9616fdf5773cb3e81a0d12aa9924b9d67929c Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Thu, 25 Jul 2019 23:56:54 +0200 Subject: [PATCH 1/2] remove harcoded path --- .../src/PlannerInterface/TFDPlannerInterface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp index d1c93986a..e7b4c701e 100644 --- a/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp @@ -66,7 +66,7 @@ namespace KCL_rosplan { if(pit!=std::string::npos) str.replace(pit,7,problem_path); // path is based on the default installation of the Temporal Fast Downward - std::string updatePlan = "cp "+data_path+"../../rosplan_planning_system/common/bin/tfd-src-0.4/downward/tfdplan.1"+" "+data_path+"plan.pddl"; + std::string updatePlan = "cp "+data_path+"tfdplan.1"+" "+data_path+"plan.pddl"; // call the planer From 1dd03f907f00fb91b99e3de2c807490c3b0ff394 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Fri, 9 Aug 2019 17:17:53 +0200 Subject: [PATCH 2/2] extract planner path from planner command, cd there first, then run planner --- rosplan_planning_system/CMakeLists.txt | 60 +++++++++---------- .../PlannerInterface/PlannerInterface.h | 3 +- .../PlannerInterface/TFDPlannerInterface.h | 2 +- .../src/PlannerInterface/PlannerInterface.cpp | 38 +++++++++++- .../PlannerInterface/TFDPlannerInterface.cpp | 8 +-- 5 files changed, 72 insertions(+), 39 deletions(-) diff --git a/rosplan_planning_system/CMakeLists.txt b/rosplan_planning_system/CMakeLists.txt index 741ed96ee..bb7e9b810 100644 --- a/rosplan_planning_system/CMakeLists.txt +++ b/rosplan_planning_system/CMakeLists.txt @@ -49,17 +49,17 @@ add_executable(problemInterface src/ProblemGeneration/ProblemInterface.cpp src/P src/ProblemGeneration/ProblemGenerator.cpp src/ProblemGeneration/RDDLProblemGenerator.cpp src/ProblemGeneration/ProblemGeneratorFactory.cpp) -add_executable(popf_planner_interface src/PlannerInterface/POPFPlannerInterface.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) -add_executable(smt_planner_interface src/PlannerInterface/SMTPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) -add_executable(fd_planner_interface src/PlannerInterface/FDPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) +# add_executable(popf_planner_interface src/PlannerInterface/POPFPlannerInterface.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) +# add_executable(smt_planner_interface src/PlannerInterface/SMTPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) +# add_executable(fd_planner_interface src/PlannerInterface/FDPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(tfd_planner_interface src/PlannerInterface/TFDPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) -add_executable(lpg_planner_interface src/PlannerInterface/LPGPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) -add_executable(upm_planner_interface src/PlannerInterface/UPMPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) -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(lpg_planner_interface src/PlannerInterface/LPGPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) +# add_executable(upm_planner_interface src/PlannerInterface/UPMPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) +# 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_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) @@ -69,17 +69,17 @@ 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(ff_planner_interface ${catkin_EXPORTED_TARGETS}) -add_dependencies(rddlsim_planner_interface ${catkin_EXPORTED_TARGETS}) -add_dependencies(metricff_planner_interface ${catkin_EXPORTED_TARGETS}) -add_dependencies(smt_planner_interface ${catkin_EXPORTED_TARGETS}) -add_dependencies(fd_planner_interface ${catkin_EXPORTED_TARGETS}) +# add_dependencies(popf_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}) +# add_dependencies(smt_planner_interface ${catkin_EXPORTED_TARGETS}) +# add_dependencies(fd_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(tfd_planner_interface ${catkin_EXPORTED_TARGETS}) -add_dependencies(lpg_planner_interface ${catkin_EXPORTED_TARGETS}) -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(lpg_planner_interface ${catkin_EXPORTED_TARGETS}) +# 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_esterel_plan_parser ${catkin_EXPORTED_TARGETS}) add_dependencies(pddl_simple_plan_dispatcher ${catkin_EXPORTED_TARGETS}) @@ -89,17 +89,17 @@ 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(ff_planner_interface ${catkin_LIBRARIES}) -target_link_libraries(rddlsim_planner_interface ${catkin_LIBRARIES}) -target_link_libraries(metricff_planner_interface ${catkin_LIBRARIES}) -target_link_libraries(smt_planner_interface ${catkin_LIBRARIES}) -target_link_libraries(fd_planner_interface ${catkin_LIBRARIES}) +# target_link_libraries(popf_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}) +# target_link_libraries(smt_planner_interface ${catkin_LIBRARIES}) +# target_link_libraries(fd_planner_interface ${catkin_LIBRARIES}) target_link_libraries(tfd_planner_interface ${catkin_LIBRARIES}) -target_link_libraries(lpg_planner_interface ${catkin_LIBRARIES}) -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(lpg_planner_interface ${catkin_LIBRARIES}) +# 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_esterel_plan_parser ${catkin_LIBRARIES}) target_link_libraries(pddl_simple_plan_dispatcher ${catkin_LIBRARIES}) diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PlannerInterface.h index e0d7f3591..c8e58cb6b 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PlannerInterface.h @@ -41,7 +41,7 @@ namespace KCL_rosplan { double problem_instance_time; /* planning */ - virtual bool runPlanner() =0; + virtual bool runPlanner(std::string planner_path) =0; public: @@ -50,6 +50,7 @@ namespace KCL_rosplan { bool runPlanningServerDefault(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); bool runPlanningServerParams(rosplan_dispatch_msgs::PlanningService::Request &req, rosplan_dispatch_msgs::PlanningService::Response &res); void runPlanningServerAction(const rosplan_dispatch_msgs::PlanGoalConstPtr& goal); + bool extract_planner_path(std::string &planner_command, std::string &planner_path); bool runPlanningServer(std::string domainPath, std::string problemPath, std::string dataPath, std::string plannerCommand, bool useProblemTopic); /* ROS interface */ diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/TFDPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/TFDPlannerInterface.h index 7e50027fe..d3c6f5da2 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/TFDPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/TFDPlannerInterface.h @@ -24,7 +24,7 @@ namespace KCL_rosplan { protected: - bool runPlanner(); + bool runPlanner(std::string planner_path); public: diff --git a/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp index dbeb68b89..1d4d94184 100644 --- a/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp @@ -63,7 +63,37 @@ namespace KCL_rosplan { plan_server->setAborted(); } } - + + /** + * extract planner path from planner command + */ + bool PlannerInterface::extract_planner_path(std::string &planner_command, std::string &planner_path) { + size_t pos = 0; + + planner_path.clear(); + planner_path = planner_command; + + // remove timeout N + for(size_t i = 0; i < 2; i++) { + pos = planner_path.find(" "); + if(pos == std::string::npos) return false; + planner_path.erase(0, pos + 1); + } + + pos = planner_path.find(" "); + if(pos == std::string::npos) return false; + planner_path.erase(pos, planner_path.size()); + + pos = planner_path.find_last_of("/"); + if(pos == std::string::npos) return false; + planner_path.erase(pos, planner_path.size()); + + // add "/" at the end of the planner path + planner_path.insert(planner_path.size(), "/"); + + return true; + } + /** * planning system; prepares planning; calls planner; parses plan. */ @@ -93,7 +123,11 @@ namespace KCL_rosplan { return false; } - bool success = runPlanner(); + // get planner path + std::string planner_path; + extract_planner_path(planner_command, planner_path); + + bool success = runPlanner(planner_path); // publish planner output if(success) { diff --git a/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp index 00d3ed99b..a92409f19 100644 --- a/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp @@ -47,7 +47,7 @@ namespace KCL_rosplan { /** * passes the problem to the Planner; the plan to post-processing. */ - bool TFDPlannerInterface::runPlanner() { + bool TFDPlannerInterface::runPlanner(std::string planner_path) { // save problem to file for TFD if(use_problem_topic && problem_instance_received) { @@ -65,16 +65,14 @@ namespace KCL_rosplan { std::size_t pit = str.find("PROBLEM"); if(pit!=std::string::npos) str.replace(pit,7,problem_path); - // path is based on the default installation of the Temporal Fast Downward - std::string updatePlan = "cp "+data_path+"tfdplan.1"+" "+data_path+"plan.pddl"; - - // call the planer + str.insert(0, "cd " + planner_path + " && "); ROS_INFO("KCL: (%s) (%s) Running: %s", ros::this_node::getName().c_str(), problem_name.c_str(), str.c_str()); std::string plan = runCommand(str.c_str()); ROS_INFO("KCL: (%s) (%s) Planning complete", ros::this_node::getName().c_str(), problem_name.c_str()); // move plan to correct path + std::string updatePlan = "cp " + planner_path + "plan.pddl.1 " + data_path + "plan.pddl"; runCommand(updatePlan.c_str()); // check the planner solved the problem