implemented abstraction to planner interface code

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})

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();

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();

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();

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();

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;
 

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();

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();

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();

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 <fstream>
+#include <sstream>
+#include <string>
+#include <ctime>
+#include <string>
+#include <streambuf>
+
 /**
  * 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);

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();

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();

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();

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;

rosplan_planning_system/src/PlannerInterface/CHIMPPlannerInterface.cpp

@@ -7,64 +7,14 @@ namespace KCL_rosplan {
 	/* constructor */
 	/*-------------*/
 
-	CHIMPPlannerInterface::CHIMPPlannerInterface(ros::NodeHandle& nh)
-	{
-		node_handle = &nh;
-
-		plan_server = new actionlib::SimpleActionServer<rosplan_dispatch_msgs::PlanAction>((*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<std_msgs::String>(plannerTopic, 1, true);
-
-		// start planning action server
-		plan_server->start();
+	CHIMPPlannerInterface::CHIMPPlannerInterface(ros::NodeHandle& nh): PlannerInterface(nh) {
 	}
 
 	CHIMPPlannerInterface::~CHIMPPlannerInterface()
 	{
 		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 <unsigned> (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<KCL_rosplan::PlannerInterface*>(&pi));
-
-		// start the planning services
-		ros::ServiceServer service1 = nh.advertiseService("planning_server", &KCL_rosplan::PlannerInterface::runPlanningServerDefault, dynamic_cast<KCL_rosplan::PlannerInterface*>(&pi));
-		ros::ServiceServer service2 = nh.advertiseService("planning_server_params", &KCL_rosplan::PlannerInterface::runPlanningServerParams, dynamic_cast<KCL_rosplan::PlannerInterface*>(&pi));
-
-		ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str());
-		ros::spin();
-
-		return 0;
-	}