From 667178f7a73f0f5686029c07a8ea251f00691674 Mon Sep 17 00:00:00 2001 From: Aaron Ram Date: Tue, 19 Sep 2023 12:00:18 +0100 Subject: [PATCH] implemented abstraction to planner interface code --- rosplan_planning_system/CMakeLists.txt | 55 +++----- .../PlannerInterface/CHIMPPlannerInterface.h | 8 -- .../PlannerInterface/FDPlannerInterface.h | 3 - .../PlannerInterface/FFPlannerInterface.h | 3 - .../PlannerInterface/LPGPlannerInterface.h | 3 - .../PlannerInterface/OnlinePlannerInterface.h | 2 +- .../PlannerInterface/PANDAPlannerInterface.h | 3 - .../PlannerInterface/POPFPlannerInterface.h | 3 - .../PlannerInterface/PPRPPlannerInterface.h | 3 - .../PlannerInterface/PlannerInterface.h | 24 ++++ .../RDDLSIMPlannerInterface.h | 3 - .../PlannerInterface/SMTPlannerInterface.h | 3 - .../PlannerInterface/TFDPlannerInterface.h | 3 - .../PlannerInterface/UPMPlannerInterface.h | 3 - .../CHIMPPlannerInterface.cpp | 100 ++----------- .../PlannerInterface/FDPlannerInterface.cpp | 77 +--------- .../PlannerInterface/FFPlannerInterface.cpp | 79 +---------- .../PlannerInterface/LPGPlannerInterface.cpp | 81 +---------- .../OnlinePlannerInterface.cpp | 64 +-------- .../PANDAPlannerInterface.cpp | 78 +---------- .../PlannerInterface/POPFPlannerInterface.cpp | 78 +---------- .../PlannerInterface/PPRPPlannerInterface.cpp | 79 +---------- .../src/PlannerInterface/PlannerInterface.cpp | 75 +++++++++- .../RDDLSIMPlannerInterface.cpp | 79 +---------- .../PlannerInterface/SMTPlannerInterface.cpp | 79 +---------- .../PlannerInterface/TFDPlannerInterface.cpp | 77 +--------- .../PlannerInterface/UPMPlannerInterface.cpp | 77 +--------- .../src/PlannerInterface/main.cpp | 132 ++++++++++++++++++ 28 files changed, 318 insertions(+), 956 deletions(-) create mode 100644 rosplan_planning_system/src/PlannerInterface/main.cpp diff --git a/rosplan_planning_system/CMakeLists.txt b/rosplan_planning_system/CMakeLists.txt index ff0da33ff..6495c2369 100644 --- a/rosplan_planning_system/CMakeLists.txt +++ b/rosplan_planning_system/CMakeLists.txt @@ -52,19 +52,20 @@ add_executable(problemInterface src/ProblemGeneration/ProblemInterface.cpp src/P src/ProblemGeneration/CHIMPProblem.cpp src/ProblemGeneration/CHIMPProblemGenerator.cpp src/ProblemGeneration/ProblemGeneratorFactory.cpp) -add_executable(popf_planner_interface src/PlannerInterface/POPFPlannerInterface.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) -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(chimp_planner_interface src/PlannerInterface/CHIMPPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) -add_executable(online_planner_interface src/PlannerInterface/OnlinePlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) +add_executable(main src/PlannerInterface/main.cpp src/PlannerInterface/POPFPlannerInterface.cpp + src/PlannerInterface/PANDAPlannerInterface.cpp + src/PlannerInterface/FFPlannerInterface.cpp + src/PlannerInterface/RDDLSIMPlannerInterface.cpp + src/PlannerInterface/FFPlannerInterface.cpp + src/PlannerInterface/SMTPlannerInterface.cpp + src/PlannerInterface/FDPlannerInterface.cpp + src/PlannerInterface/TFDPlannerInterface.cpp + src/PlannerInterface/LPGPlannerInterface.cpp + src/PlannerInterface/UPMPlannerInterface.cpp + src/PlannerInterface/PPRPPlannerInterface.cpp + src/PlannerInterface/CHIMPPlannerInterface.cpp + 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) @@ -74,19 +75,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(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}) -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(chimp_planner_interface ${catkin_EXPORTED_TARGETS}) +add_dependencies(main ${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}) @@ -96,19 +85,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(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}) -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(chimp_planner_interface ${catkin_LIBRARIES}) +target_link_libraries(main ${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/CHIMPPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CHIMPPlannerInterface.h index cb9ede8f0..3f9b7a714 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CHIMPPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CHIMPPlannerInterface.h @@ -20,14 +20,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - - /* create command line for the planner */ - std::string prepareCommand(); - /* write the problem instance to problem_path file */ - void writeProblemToFile(); - protected: bool runPlanner(); diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/FDPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/FDPlannerInterface.h index ac0c2b19c..f424f77fd 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/FDPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/FDPlannerInterface.h @@ -19,9 +19,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: bool runPlanner(); diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/FFPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/FFPlannerInterface.h index fd28ce029..15db838c4 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/FFPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/FFPlannerInterface.h @@ -20,9 +20,6 @@ namespace KCL_rosplan { bool use_ffha; - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: bool runPlanner(); diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/LPGPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/LPGPlannerInterface.h index 946b79ebb..4ebb18a72 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/LPGPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/LPGPlannerInterface.h @@ -18,9 +18,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: bool runPlanner(); diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/OnlinePlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/OnlinePlannerInterface.h index a2b193b89..6923c0a72 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/OnlinePlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/OnlinePlannerInterface.h @@ -16,7 +16,7 @@ namespace KCL_rosplan { private: /* runs external commands */ - std::string runCommand(std::string cmd); + std::string runCommand(std::string cmd); std::thread planner; bool planner_running; diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h index 1b04b1715..db2f5e6c8 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h @@ -18,9 +18,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: bool runPlanner(); diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/POPFPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/POPFPlannerInterface.h index 92ed29af3..653ee0ea7 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/POPFPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/POPFPlannerInterface.h @@ -18,9 +18,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: bool runPlanner(); diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PPRPPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PPRPPlannerInterface.h index b633e89cd..db980feae 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PPRPPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PPRPPlannerInterface.h @@ -18,9 +18,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: bool runPlanner(); 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..8f93e5efd 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PlannerInterface.h @@ -10,6 +10,13 @@ #include "rosplan_dispatch_msgs/PlanningService.h" #include "rosplan_dispatch_msgs/PlanAction.h" +#include +#include +#include +#include +#include +#include + /** * This file contains an interface to the planner. */ @@ -43,8 +50,25 @@ namespace KCL_rosplan { /* planning */ virtual bool runPlanner() =0; + /* runs external commands */ + std::string runCommand(std::string cmd); + + /* saves problem to file */ + void saveProblem(); + + /* prepares the planner command line */ + std::string prepareCommand(); + + /* calls the planner */ + void callPlanner(std::string & commandString); + + void solvedMessages(bool & solved); + public: + PlannerInterface(ros::NodeHandle& nh); + virtual ~PlannerInterface(); + void problemCallback(const std_msgs::String& problemInstance); bool runPlanningServerDefault(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/RDDLSIMPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/RDDLSIMPlannerInterface.h index 2a01e224b..357b7fb42 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/RDDLSIMPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/RDDLSIMPlannerInterface.h @@ -22,9 +22,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: bool runPlanner(); diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/SMTPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/SMTPlannerInterface.h index 9f0d3d521..4a57dbd00 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/SMTPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/SMTPlannerInterface.h @@ -19,9 +19,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: bool runPlanner(); 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..08de2fbfd 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/TFDPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/TFDPlannerInterface.h @@ -19,9 +19,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: bool runPlanner(); diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/UPMPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/UPMPlannerInterface.h index 59a2d0c8b..7341e9752 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/UPMPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/UPMPlannerInterface.h @@ -19,9 +19,6 @@ namespace KCL_rosplan { { private: - /* runs external commands */ - std::string runCommand(std::string cmd); - protected: std::string input_problem_path; diff --git a/rosplan_planning_system/src/PlannerInterface/CHIMPPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/CHIMPPlannerInterface.cpp index 1eef86a88..79b5dc316 100644 --- a/rosplan_planning_system/src/PlannerInterface/CHIMPPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/CHIMPPlannerInterface.cpp @@ -7,19 +7,7 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - CHIMPPlannerInterface::CHIMPPlannerInterface(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(); + CHIMPPlannerInterface::CHIMPPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { } CHIMPPlannerInterface::~CHIMPPlannerInterface() @@ -27,44 +15,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string CHIMPPlannerInterface::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; - } - - void CHIMPPlannerInterface::writeProblemToFile() { - std::ofstream dest; - dest.open((problem_path).c_str()); - dest << problem_instance; - dest.close(); - } - - /** - * Creates command line string by setting domain and problem path and output. - */ - std::string CHIMPPlannerInterface::prepareCommand() { - 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::size_t oit = str.find("OUTPUT"); - if(oit!=std::string::npos) str.replace(oit,6,data_path + "plan.txt"); - std::size_t eit = str.find("ESTEREL"); - if(eit!=std::string::npos) str.replace(eit,7,data_path + "esterel.yaml"); - - return str; - } - bool containsEsterel(const std::string& commandStr) { std::size_t eit = commandStr.find("ESTEREL"); return eit!=std::string::npos; @@ -89,16 +39,17 @@ namespace KCL_rosplan { bool CHIMPPlannerInterface::runPlanner() { // save problem to file for CHIMP - if(use_problem_topic && problem_instance_received) { - ROS_INFO("ROSPlan: (%s) (%s) Writing problem to file.", ros::this_node::getName().c_str(), problem_name.c_str()); - writeProblemToFile(); - } + saveProblem(); - // call the planer + // prepare the planner command line std::string commandString = prepareCommand(); - ROS_INFO("ROSPlan: (%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("ROSPlan: (%s) (%s) Planning complete", ros::this_node::getName().c_str(), problem_name.c_str()); + std::size_t oit = commandString.find("OUTPUT"); + if(oit!=std::string::npos) commandString.replace(oit,6,data_path + "plan.txt"); + std::size_t eit = commandString.find("ESTEREL"); + if(eit!=std::string::npos) commandString.replace(eit,7,data_path + "esterel.yaml"); + + // call the planer + callPlanner(commandString); // check if the planner solved the problem std::ifstream planfile; @@ -139,38 +90,9 @@ namespace KCL_rosplan { } - if(!solved) ROS_INFO("ROSPlan: (%s) (%s) Plan was unsolvable.", ros::this_node::getName().c_str(), problem_name.c_str()); - else ROS_INFO("ROSPlan: (%s) (%s) Plan was solved.", ros::this_node::getName().c_str(), problem_name.c_str()); + solvedMessages(solved); 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::CHIMPPlannerInterface 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/PlannerInterface/FDPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/FDPlannerInterface.cpp index 12c030a6c..3ea9504a2 100644 --- a/rosplan_planning_system/src/PlannerInterface/FDPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/FDPlannerInterface.cpp @@ -7,19 +7,7 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - FDPlannerInterface::FDPlannerInterface(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(); + FDPlannerInterface::FDPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { } FDPlannerInterface::~FDPlannerInterface() @@ -27,20 +15,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string FDPlannerInterface::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 */ /*------------------*/ @@ -51,28 +25,16 @@ namespace KCL_rosplan { bool FDPlannerInterface::runPlanner() { // save problem to file for FD - 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(); - } + saveProblem(); // 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 = prepareCommand(); // path is based on the default installation of the Fast Downward planner std::string updatePlan = "cp "+data_path+"../../rosplan_planning_system/common/bin/downward/fdplan.1"+" "+data_path+"plan.pddl"; // call the planer - 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()); + callPlanner(commandString); // move plan to correct path runCommand(updatePlan.c_str()); @@ -106,38 +68,9 @@ namespace KCL_rosplan { } 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()); + solvedMessages(solved); 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::FDPlannerInterface 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/PlannerInterface/FFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/FFPlannerInterface.cpp index 3d8873046..75eda0417 100644 --- a/rosplan_planning_system/src/PlannerInterface/FFPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/FFPlannerInterface.cpp @@ -6,21 +6,8 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - FFPlannerInterface::FFPlannerInterface(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); - + FFPlannerInterface::FFPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { node_handle->param("use_ffha", this->use_ffha, false); - - // start planning action server - plan_server->start(); } FFPlannerInterface::~FFPlannerInterface() @@ -28,19 +15,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string FFPlannerInterface::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 */ @@ -52,26 +26,13 @@ namespace KCL_rosplan { bool FFPlannerInterface::runPlanner() { planner_output.clear(); // Clear previous plan // save problem to file for planner - 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(); - } + saveProblem(); // 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.pddl"; + std::string commandString = prepareCommand() + " > " + data_path + "plan.pddl"; // 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()); + callPlanner(commandString); // check the planner solved the problem std::ifstream planfile; @@ -81,6 +42,7 @@ namespace KCL_rosplan { bool solved = false; while (not solved and std::getline(planfile, line)) { // skip lines until there is a plan + std::cout << line << std::endl; if (line.find("ff: found legal plan") != line.npos) { solved = true; } @@ -125,38 +87,9 @@ namespace KCL_rosplan { } 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()); + solvedMessages(solved); 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::FFPlannerInterface 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/PlannerInterface/LPGPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/LPGPlannerInterface.cpp index 277e219ba..47c871d53 100644 --- a/rosplan_planning_system/src/PlannerInterface/LPGPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/LPGPlannerInterface.cpp @@ -6,19 +6,7 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - LPGPlannerInterface::LPGPlannerInterface(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(); + LPGPlannerInterface::LPGPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { } LPGPlannerInterface::~LPGPlannerInterface() @@ -26,20 +14,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string LPGPlannerInterface::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 */ /*------------------*/ @@ -50,33 +24,21 @@ namespace KCL_rosplan { bool LPGPlannerInterface::runPlanner() { // save problem to file for LPG - 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(); - } + saveProblem(); // 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::size_t oit = str.find("-out "); + std::string commandString = prepareCommand(); + std::size_t oit = commandString.find("-out "); //change output path of the planner to data_path/lpgplan ROS_INFO("KCL: (%s) (%s) Changing '-out' to 'data_path/lpgplan'. !!! '-out' must be the last parameter in the planner_command !!!", ros::this_node::getName().c_str(), problem_name.c_str()); - if(oit!=std::string::npos) str.replace(oit,500,"-out "+data_path+"lpgplan"); + if(oit!=std::string::npos) commandString.replace(oit,500,"-out "+data_path+"lpgplan"); //copy the .SOL output plan to data_path/plan.pddl std::string updatePlan = "cp "+data_path+"lpgplan.SOL"+" "+data_path+"plan.pddl"; // call the planer - 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()); + callPlanner(commandString); // move plan to correct path runCommand(updatePlan.c_str()); @@ -115,38 +77,9 @@ namespace KCL_rosplan { } 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()); + solvedMessages(solved); 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::LPGPlannerInterface 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/PlannerInterface/OnlinePlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/OnlinePlannerInterface.cpp index 5ad5ee0b6..a90a11d8a 100644 --- a/rosplan_planning_system/src/PlannerInterface/OnlinePlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/OnlinePlannerInterface.cpp @@ -4,22 +4,10 @@ #include -#include "rosplan_planning_system/PlannerInterface/OnlinePlannerInterface.h" - namespace KCL_rosplan { - OnlinePlannerInterface::OnlinePlannerInterface(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); - + OnlinePlannerInterface::OnlinePlannerInterface(ros::NodeHandle &nh): PlannerInterface(nh) { // start planning action server - plan_server->start(); planner_running = false; get_planner_params = nh.advertiseService("get_planning_params", &KCL_rosplan::OnlinePlannerInterface::getPlannerParams, this); } @@ -46,31 +34,20 @@ namespace KCL_rosplan { bool OnlinePlannerInterface::runPlanner() { if (planner_running) { - ROS_INFO("KCL: (%s) A plan is already being computed.", ros::this_node::getName().c_str()); + ROS_INFO("ROSPlan: (%s) A plan is already being computed.", ros::this_node::getName().c_str()); return false; } else if (planner.joinable()) planner.join(); // save problem to file for planner - 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(); - } + saveProblem(); // 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.pddl"; + std::string commandString = prepareCommand(); + // std::string commandString = str + " > " + data_path + "plan.pddl"; // call the planer - ROS_INFO("KCL: (%s) (%s) Running: %s in a new thread", ros::this_node::getName().c_str(), problem_name.c_str(), + ROS_INFO("ROSPlan: (%s) (%s) Running: %s in a new thread", ros::this_node::getName().c_str(), problem_name.c_str(), commandString.c_str()); // Launch thread and return, as the dispatcher will take care of the planning @@ -86,32 +63,3 @@ namespace KCL_rosplan { return true; } } - - -/*-------------*/ -/* 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::OnlinePlannerInterface 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; -} \ No newline at end of file diff --git a/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp index 46c6d0f5e..a99dfd452 100644 --- a/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp @@ -6,19 +6,7 @@ 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(ros::NodeHandle& nh): PlannerInterface(nh) { } PANDAPlannerInterface::~PANDAPlannerInterface() @@ -26,20 +14,6 @@ namespace KCL_rosplan { 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 */ /*------------------*/ @@ -50,26 +24,13 @@ namespace KCL_rosplan { 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(); - } + saveProblem(); // 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.pddl"; + std::string commandString = prepareCommand() + " > " + data_path + "plan.pddl"; // 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()); + callPlanner(commandString); // check the planner solved the problem std::ifstream planfile; @@ -101,38 +62,9 @@ namespace KCL_rosplan { } 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()); + solvedMessages(solved); 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/PlannerInterface/POPFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp index c8ad1ce2f..6c1b308c4 100644 --- a/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp @@ -6,19 +6,7 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - POPFPlannerInterface::POPFPlannerInterface(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(); + POPFPlannerInterface::POPFPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { } POPFPlannerInterface::~POPFPlannerInterface() @@ -26,20 +14,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string POPFPlannerInterface::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 */ /*------------------*/ @@ -50,26 +24,13 @@ namespace KCL_rosplan { bool POPFPlannerInterface::runPlanner() { // save problem to file for POPF - 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(); - } + saveProblem(); // 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.pddl"; + std::string commandString = prepareCommand() + " > " + data_path + "plan.pddl"; // 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()); + callPlanner(commandString); // check the planner solved the problem std::ifstream planfile; @@ -101,38 +62,9 @@ namespace KCL_rosplan { } 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()); + solvedMessages(solved); 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::POPFPlannerInterface 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/PlannerInterface/PPRPPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/PPRPPlannerInterface.cpp index 4666811eb..3e95c7efc 100644 --- a/rosplan_planning_system/src/PlannerInterface/PPRPPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/PPRPPlannerInterface.cpp @@ -7,19 +7,7 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - PPRPPlannerInterface::PPRPPlannerInterface(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(); + PPRPPlannerInterface::PPRPPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { } PPRPPlannerInterface::~PPRPPlannerInterface() @@ -27,20 +15,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string PPRPPlannerInterface::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 */ /*------------------*/ @@ -51,26 +25,12 @@ namespace KCL_rosplan { bool PPRPPlannerInterface::runPlanner() { // save problem to file for planner - 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(); - } - + saveProblem(); // 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.pddl"; + std::string commandString = prepareCommand() + " > " + data_path + "plan.pddl"; // 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()); + callPlanner(commandString); // check the planner solved the problem std::ifstream planfile; @@ -115,38 +75,9 @@ namespace KCL_rosplan { planfile.close(); - if (!solved) ROS_WARN("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()); + solvedMessages(solved); 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::PPRPPlannerInterface 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/PlannerInterface/PlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp index dbeb68b89..64affa61f 100644 --- a/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp @@ -2,12 +2,83 @@ namespace KCL_rosplan { + /*-------------*/ + /* constructor */ + /*-------------*/ + + PlannerInterface::PlannerInterface(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(); + } + + PlannerInterface::~PlannerInterface(){ + delete plan_server; + } + + /** + * Runs external commands + */ + std::string PlannerInterface::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 */ + /*------------------*/ + + void PlannerInterface::saveProblem(){ + if(use_problem_topic && problem_instance_received) { + ROS_INFO("ROSPlan: (%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(); + } + } + + std::string PlannerInterface::prepareCommand() { + 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); + + return str; + } + + void PlannerInterface::callPlanner(std::string & commandString){ + ROS_INFO("ROSPlan: (%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("ROSPlan: (%s) (%s) Planning complete", ros::this_node::getName().c_str(), problem_name.c_str()); + } + + void PlannerInterface::solvedMessages(bool & solved){ + if(!solved) ROS_INFO("ROSPlan: (%s) (%s) Plan was unsolvable.", ros::this_node::getName().c_str(), problem_name.c_str()); + else ROS_INFO("ROSPlan: (%s) (%s) Plan was solved.", ros::this_node::getName().c_str(), problem_name.c_str()); + } + /*----------------------*/ /* Problem subscription */ /*----------------------*/ void PlannerInterface::problemCallback(const std_msgs::String& problemInstance) { - ROS_INFO("KCL: (%s) Problem received.", ros::this_node::getName().c_str()); + ROS_INFO("ROSPlan: (%s) Problem received.", ros::this_node::getName().c_str()); problem_instance_received = true; problem_instance_time = ros::WallTime::now().toSec(); problem_instance = problemInstance.data; @@ -89,7 +160,7 @@ namespace KCL_rosplan { } if(use_problem_topic && !problem_instance_received) { - ROS_INFO("KCL: (%s) (%s) Problem was not published yet.", ros::this_node::getName().c_str(), problem_name.c_str()); + ROS_INFO("ROSPlan: (%s) (%s) Problem was not published yet.", ros::this_node::getName().c_str(), problem_name.c_str()); return false; } diff --git a/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp index 671468887..ef4f1be1d 100644 --- a/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp @@ -10,19 +10,7 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - RDDLSIMPlannerInterface::RDDLSIMPlannerInterface(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(); + RDDLSIMPlannerInterface::RDDLSIMPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { } RDDLSIMPlannerInterface::~RDDLSIMPlannerInterface() @@ -30,20 +18,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string RDDLSIMPlannerInterface::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 */ /*------------------*/ @@ -54,26 +28,13 @@ namespace KCL_rosplan { bool RDDLSIMPlannerInterface::runPlanner() { // save problem to file for planner - 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(); - } + saveProblem(); // 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.pddl"; + std::string commandString = prepareCommand() + " > " + data_path + "plan.pddl"; // 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()); + callPlanner(commandString); // CHECK PLANNER OUTPUT // Planner output example: @@ -92,6 +53,7 @@ namespace KCL_rosplan { std::smatch match; int action_idx = 0; while (std::getline(planfile, line)) { + std::cout << "test: " << line << std::endl; // Check any action was found if (std::regex_search(line, match, actionline_rgx)) { std::istringstream action_list(match[1].str()); // Get all the actions in a sstream @@ -115,38 +77,9 @@ namespace KCL_rosplan { } planfile.close(); bool solved = !planner_output.empty(); - 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()); + solvedMessages(solved); 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::RDDLSIMPlannerInterface 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/PlannerInterface/SMTPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/SMTPlannerInterface.cpp index c07f27005..aaeb08009 100644 --- a/rosplan_planning_system/src/PlannerInterface/SMTPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/SMTPlannerInterface.cpp @@ -6,19 +6,7 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - SMTPlannerInterface::SMTPlannerInterface(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(); + SMTPlannerInterface::SMTPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { } SMTPlannerInterface::~SMTPlannerInterface() @@ -26,20 +14,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string SMTPlannerInterface::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 */ /*------------------*/ @@ -50,26 +24,14 @@ namespace KCL_rosplan { bool SMTPlannerInterface::runPlanner() { // save problem to file for SMT - 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(); - } + saveProblem(); // 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.pddl"; + std::string commandString = prepareCommand(); + commandString = commandString + " > " + data_path + "plan.pddl"; // 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()); + callPlanner(commandString); // check the planner solved the problem std::ifstream planfile; @@ -98,38 +60,9 @@ namespace KCL_rosplan { } 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()); + solvedMessages(solved); 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::SMTPlannerInterface 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/PlannerInterface/TFDPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp index 71bd4f9ac..e9a85fa0b 100644 --- a/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp @@ -6,19 +6,7 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - TFDPlannerInterface::TFDPlannerInterface(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(); + TFDPlannerInterface::TFDPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { } TFDPlannerInterface::~TFDPlannerInterface() @@ -26,20 +14,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string TFDPlannerInterface::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 */ /*------------------*/ @@ -53,20 +27,10 @@ namespace KCL_rosplan { std::string tfdOutputName = "tfdplan"; // save problem to file for TFD - 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(); - } + saveProblem(); // 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 = prepareCommand(); // delete old plans before running the planner ROS_INFO("KCL: (%s) Removing old TFD plans with (%s) name.", ros::this_node::getName().c_str(), tfdOutputName.c_str()); @@ -74,9 +38,7 @@ namespace KCL_rosplan { runCommand(removeOldPlans); // call the planer - 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()); + callPlanner(commandString); // get the most optimal plan in case there are many (tfdplan.[the highest number]) std::string bestPlanCommand = "cd " + plannerPlanPath + " && ls | grep " + tfdOutputName + " | tail -1"; @@ -118,38 +80,9 @@ namespace KCL_rosplan { } 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()); + solvedMessages(solved); 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::TFDPlannerInterface 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/PlannerInterface/UPMPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/UPMPlannerInterface.cpp index 4be6a4099..778380a4e 100644 --- a/rosplan_planning_system/src/PlannerInterface/UPMPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/UPMPlannerInterface.cpp @@ -6,19 +6,7 @@ namespace KCL_rosplan { /* constructor */ /*-------------*/ - UPMPlannerInterface::UPMPlannerInterface(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(); + UPMPlannerInterface::UPMPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) { } UPMPlannerInterface::~UPMPlannerInterface() @@ -26,20 +14,6 @@ namespace KCL_rosplan { delete plan_server; } - /** - * Runs external commands - */ - std::string UPMPlannerInterface::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 */ /*------------------*/ @@ -50,20 +24,10 @@ namespace KCL_rosplan { bool UPMPlannerInterface::runPlanner() { // save problem to file for UPM - 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(); - } + saveProblem(); // 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 = prepareCommand(); // create the plan path from domain_path (problem file must be named problem.pddl) @@ -78,9 +42,7 @@ namespace KCL_rosplan { // call the planer - 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()); + callPlanner(commandString); // move plan to correct path runCommand(updatePlan.c_str()); @@ -108,38 +70,9 @@ namespace KCL_rosplan { } 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()); + solvedMessages(solved); 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::UPMPlannerInterface 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/PlannerInterface/main.cpp b/rosplan_planning_system/src/PlannerInterface/main.cpp new file mode 100644 index 000000000..e7b698a3c --- /dev/null +++ b/rosplan_planning_system/src/PlannerInterface/main.cpp @@ -0,0 +1,132 @@ +#include +#include +#include +#include +#include + +#include "rosplan_planning_system/PlannerInterface/PlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/UPMPlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/POPFPlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/FFPlannerInterface.h" +#include +#include "rosplan_planning_system/PlannerInterface/TFDPlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/CHIMPPlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/SMTPlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/RDDLSIMPlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/PPRPPlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/LPGPlannerInterface.h" +#include "rosplan_planning_system/PlannerInterface/FDPlannerInterface.h" + +template +void runPlanner(T & pi, ros::NodeHandle & 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(); +} + +/*-------------*/ +/* Main method */ +/*-------------*/ + +int main(int argc, char **argv) { + + srand (static_cast (time(0))); + + ros::init(argc,argv,"rosplan_planner_interface"); + ros::NodeHandle nh("~"); + + + // Read from the text file + std::string plannerChoice; + std::ifstream myPlannerFile("/home/crackoverflow/catkin_ws/ROSPlan/src/rosplan/rosplan_planning_system/src/planner.txt"); + getline(myPlannerFile, plannerChoice); + myPlannerFile.close(); + + switch(stoi(plannerChoice)) + { + case 1: + { + KCL_rosplan::UPMPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 2: + { + KCL_rosplan::POPFPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 3: + case 4: + { + KCL_rosplan::FFPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 5: + { + KCL_rosplan::OnlinePlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 6: + { + KCL_rosplan::TFDPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 7: + { + KCL_rosplan::CHIMPPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 8: + { + KCL_rosplan::SMTPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 9: + { + KCL_rosplan::RDDLSIMPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 10: + { + KCL_rosplan::PPRPPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 11: + { + KCL_rosplan::PANDAPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 12: + { + KCL_rosplan::LPGPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + case 13: + { + KCL_rosplan::FDPlannerInterface pi(nh); + runPlanner(pi, nh); + break; + } + } + + return 0; +}