Add Contingent-FF planner node

rosplan_planning_system/CMakeLists.txt

@@ -51,8 +51,9 @@ add_executable(problemInterface src/ProblemGeneration/ProblemInterface.cpp src/P
                                                                            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(cff_planner_interface src/PlannerInterface/CFFPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp)
+add_executable(rddlsim_planner_interface src/PlannerInterface/RDDLSIMPlannerInterface.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)
@@ -71,6 +72,7 @@ add_executable(simulatedAction src/ActionInterface/RPSimulatedActionInterface.cp
 add_dependencies(problemInterface ${catkin_EXPORTED_TARGETS})
 add_dependencies(popf_planner_interface ${catkin_EXPORTED_TARGETS})
 add_dependencies(ff_planner_interface ${catkin_EXPORTED_TARGETS})
+add_dependencies(cff_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})
@@ -91,8 +93,9 @@ add_dependencies(simulatedAction ${catkin_EXPORTED_TARGETS})
 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(cff_planner_interface ${catkin_LIBRARIES})
+target_link_libraries(rddlsim_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})
@@ -164,9 +167,12 @@ install(DIRECTORY launch/
 # to run the test do e.g.: "rostest rosplan_planning_system foo.test --text"
 # where "--text" is optional, if you want to see the console output of your test
 if (CATKIN_ENABLE_TESTING)
-    # Planner Interface
-    add_rostest_gtest(PlannerInterfaceTests test/launch/planner_interface.test test/src/PlannerInterfaceTests.cpp)
-    target_link_libraries(PlannerInterfaceTests ${catkin_LIBRARIES})
+    # POPF Planner Interface
+    add_rostest_gtest(PopfPlannerInterfaceTests test/launch/popf_planner_interface.test test/src/PopfPlannerInterfaceTests.cpp)
+    target_link_libraries(PopfPlannerInterfaceTests ${catkin_LIBRARIES})
+    # Contingent-FF Planner Interface
+    add_rostest_gtest(CffPlannerInterfaceTests test/launch/cff_planner_interface.test test/src/CffPlannerInterfaceTests.cpp)
+    target_link_libraries(CffPlannerInterfaceTests ${catkin_LIBRARIES})    
     # Problem Interface
     add_rostest_gtest(ProblemInterfaceTests test/launch/problem_interface.test test/src/ProblemInterfaceTests.cpp)
     target_link_libraries(ProblemInterfaceTests ${catkin_LIBRARIES})

rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h

@@ -0,0 +1,38 @@
+#include <string>
+#include <fstream>
+#include "PlannerInterface.h"
+
+#ifndef KCL_FF_planner_interface
+#define KCL_FF_planner_interface
+
+/**
+ * This file contains an interface to the planner.
+ */
+namespace KCL_rosplan {
+
+    class CFFPlannerInterface: public PlannerInterface
+    {
+    private:
+
+        void clearPreviousPlan();
+        void saveProblemToFileIfNeeded();        
+        void callExternalPlanner();
+        std::string runCommand(std::string cmd);        
+        bool isPlanSolved(std::ifstream &plan_file);
+        void convertPlanToPopfFormat(std::ifstream &plan_file);
+        void savePlanInPopfFormatToFile();
+        bool parsePlan();
+
+    protected:
+
+        bool runPlanner();
+
+    public:
+
+        CFFPlannerInterface(ros::NodeHandle& nh);
+        virtual ~CFFPlannerInterface();
+    };
+
+}
+
+#endif

rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp

@@ -37,7 +37,10 @@ namespace KCL_rosplan {
 	/*-------------------*/
 
 	void SimplePlanDispatcher::planCallback(const rosplan_dispatch_msgs::CompletePlan plan) {
+
 		ROS_INFO("KCL: (%s) Plan received.", ros::this_node::getName().c_str());
+		ROS_INFO("KCL: (%s) Is plan empty?: %d", ros::this_node::getName().c_str(), plan.plan.size() == 0);
+
 		plan_received = true;
 		mission_start_time = ros::WallTime::now().toSec();
 		current_plan = plan;
@@ -53,6 +56,8 @@ namespace KCL_rosplan {
 	bool SimplePlanDispatcher::dispatchPlan(double missionStartTime, double planStartTime) {
 
 		ROS_INFO("KCL: (%s) Dispatching plan", ros::this_node::getName().c_str());
+		ROS_DEBUG("KCL: (%s) Num actions: %zu", ros::this_node::getName().c_str(), current_plan.plan.size());
+		ROS_DEBUG("KCL: (%s) Current action: %d", ros::this_node::getName().c_str(), current_action);
 
 		ros::Rate loop_rate(10);
 		replan_requested = false;
@@ -169,28 +174,28 @@ namespace KCL_rosplan {
 
 } // close namespace
 
-	/*-------------*/
-	/* Main method */
-	/*-------------*/
+/*-------------*/
+/* Main method */
+/*-------------*/
 
-	int main(int argc, char **argv) {
+int main(int argc, char **argv) {
 
-		ros::init(argc,argv,"rosplan_simple_plan_dispatcher");
-		ros::NodeHandle nh("~");
+    ros::init(argc,argv,"rosplan_simple_plan_dispatcher");
+    ros::NodeHandle nh("~");
 
-		KCL_rosplan::SimplePlanDispatcher spd(nh);
+    KCL_rosplan::SimplePlanDispatcher spd(nh);
 
-		// subscribe to planner output
-		std::string planTopic = "complete_plan";
-		nh.getParam("plan_topic", planTopic);
-		ros::Subscriber plan_sub = nh.subscribe(planTopic, 1, &KCL_rosplan::SimplePlanDispatcher::planCallback, &spd);
+    // subscribe to planner output
+    std::string planTopic = "complete_plan";
+    nh.getParam("plan_topic", planTopic);
+    ros::Subscriber plan_sub = nh.subscribe(planTopic, 1, &KCL_rosplan::SimplePlanDispatcher::planCallback, &spd);
 
-		std::string feedbackTopic = "action_feedback";
-		nh.getParam("action_feedback_topic", feedbackTopic);
-		ros::Subscriber feedback_sub = nh.subscribe(feedbackTopic, 1000, &KCL_rosplan::SimplePlanDispatcher::feedbackCallback, &spd);
+    std::string feedbackTopic = "action_feedback";
+    nh.getParam("action_feedback_topic", feedbackTopic);
+    ros::Subscriber feedback_sub = nh.subscribe(feedbackTopic, 1000, &KCL_rosplan::SimplePlanDispatcher::feedbackCallback, &spd);
 
-		ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str());
-		ros::spin();
+    ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str());
+    ros::spin();
 
-		return 0;
-	}
+    return 0;
+}

rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp

@@ -23,14 +23,19 @@ namespace KCL_rosplan {
 
 	PDDLSimplePlanParser::~PDDLSimplePlanParser()
 	{
-
 	}
 
 	void PDDLSimplePlanParser::reset() {
+
 		action_list.clear();
 	}
 
 	void PDDLSimplePlanParser::publishPlan() {
+
+		ROS_INFO("KCL: (%s) Plan published.", ros::this_node::getName().c_str());    
+		ROS_INFO("KCL: (%s) Is plan empty?: %d", ros::this_node::getName().c_str(), action_list.size() == 0);
+ 		ROS_DEBUG("KCL: (%s) Num actions: %d", ros::this_node::getName().c_str(), action_list.size());
+
 		rosplan_dispatch_msgs::CompletePlan msg;
 		msg.plan = action_list;
 		plan_publisher.publish(msg);
@@ -142,28 +147,28 @@ namespace KCL_rosplan {
 	}
 } // close namespace
 
-	/*-------------*/
-	/* Main method */
-	/*-------------*/
+/*-------------*/
+/* Main method */
+/*-------------*/
 
-	int main(int argc, char **argv) {
+int main(int argc, char **argv) {
 
-		ros::init(argc,argv,"rosplan_plan_parser");
-		ros::NodeHandle nh("~");
+    ros::init(argc,argv,"rosplan_plan_parser");
+    ros::NodeHandle nh("~");
 
-		KCL_rosplan::PDDLSimplePlanParser pp(nh);
-
-		// subscribe to planner output
-		std::string planTopic = "planner_output";
-		nh.getParam("planner_topic", planTopic);
-		ros::Subscriber plan_sub = nh.subscribe(planTopic, 1, &KCL_rosplan::PlanParser::plannerCallback, dynamic_cast<KCL_rosplan::PlanParser*>(&pp));
-
-		// start the plan parsing services
-		ros::ServiceServer service1 = nh.advertiseService("parse_plan", &KCL_rosplan::PlanParser::parsePlan, dynamic_cast<KCL_rosplan::PlanParser*>(&pp));
-		ros::ServiceServer service2 = nh.advertiseService("parse_plan_from_file", &KCL_rosplan::PlanParser::parsePlanFromFile, dynamic_cast<KCL_rosplan::PlanParser*>(&pp));
+    KCL_rosplan::PDDLSimplePlanParser pp(nh);
 
-		ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str());
-		ros::spin();
+    // subscribe to planner output
+    std::string planTopic = "planner_output";
+    nh.getParam("planner_topic", planTopic);
+    ros::Subscriber plan_sub = nh.subscribe(planTopic, 1, &KCL_rosplan::PlanParser::plannerCallback, dynamic_cast<KCL_rosplan::PlanParser*>(&pp));
 
-		return 0;
-	}
+    // start the plan parsing services
+    ros::ServiceServer service1 = nh.advertiseService("parse_plan", &KCL_rosplan::PlanParser::parsePlan, dynamic_cast<KCL_rosplan::PlanParser*>(&pp));
+    ros::ServiceServer service2 = nh.advertiseService("parse_plan_from_file", &KCL_rosplan::PlanParser::parsePlanFromFile, dynamic_cast<KCL_rosplan::PlanParser*>(&pp));
+
+    ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str());
+    ros::spin();
+
+    return 0;
+}

rosplan_planning_system/src/PlanParsing/PlanParser.cpp

@@ -7,7 +7,10 @@ namespace KCL_rosplan {
 	/*----------------------*/
 
 	void PlanParser::plannerCallback(const std_msgs::String& plannerOutput) {
-		ROS_INFO("KCL: (%s) Planner output received.", ros::this_node::getName().c_str());
+
+		ROS_INFO("KCL: (%s) Planner output received.", ros::this_node::getName().c_str());        
+		ROS_INFO("KCL: (%s) Is plan empty?: %d", ros::this_node::getName().c_str(), plannerOutput.data.size() == 0);
+
 		planner_output_received = true;
 		planner_output_time = ros::Time::now().toSec();
 		planner_output = plannerOutput.data;
@@ -53,7 +56,7 @@ namespace KCL_rosplan {
 	bool PlanParser::parsePlan(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
 
 		if(planner_output_received) {
-			ROS_INFO("KCL: (%s) Parsing planner output.", ros::this_node::getName().c_str());
+			ROS_INFO("KCL: (%s) Parsing planner output.", ros::this_node::getName().c_str());   
 			reset();
 			preparePlan();
 			publishPlan();