[plan parser] [lama] Added plan parser for lama (more basic than simp…

…le parser)

rosplan_planning_system/CMakeLists.txt

@@ -62,6 +62,7 @@ add_executable(upm_planner_interface src/PlannerInterface/UPMPlannerInterface.cp
 add_executable(pprp_planner_interface src/PlannerInterface/PPRPPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp)
 add_executable(online_planner_interface src/PlannerInterface/OnlinePlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp)
 add_executable(pddl_simple_plan_parser src/PlanParsing/PDDLSimplePlanParser.cpp src/PlanParsing/PlanParser.cpp)
+add_executable(pddl_lama_plan_parser src/PlanParsing/PDDLLAMAPlanParser.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)
 add_executable(pddl_esterel_plan_dispatcher src/PlanDispatch/EsterelPlanDispatcher.cpp src/PlanDispatch/PlanDispatcher.cpp)
@@ -83,6 +84,7 @@ add_dependencies(upm_planner_interface ${catkin_EXPORTED_TARGETS})
 add_dependencies(pprp_planner_interface ${catkin_EXPORTED_TARGETS})
 add_dependencies(online_planner_interface ${catkin_EXPORTED_TARGETS})
 add_dependencies(pddl_simple_plan_parser ${catkin_EXPORTED_TARGETS})
+add_dependencies(pddl_lama_plan_parser ${catkin_EXPORTED_TARGETS})
 add_dependencies(pddl_esterel_plan_parser ${catkin_EXPORTED_TARGETS})
 add_dependencies(pddl_simple_plan_dispatcher ${catkin_EXPORTED_TARGETS})
 add_dependencies(pddl_esterel_plan_dispatcher ${catkin_EXPORTED_TARGETS})
@@ -104,6 +106,7 @@ target_link_libraries(upm_planner_interface ${catkin_LIBRARIES})
 target_link_libraries(pprp_planner_interface ${catkin_LIBRARIES})
 target_link_libraries(online_planner_interface ${catkin_LIBRARIES})
 target_link_libraries(pddl_simple_plan_parser ${catkin_LIBRARIES})
+target_link_libraries(pddl_lama_plan_parser ${catkin_LIBRARIES})
 target_link_libraries(pddl_esterel_plan_parser ${catkin_LIBRARIES})
 target_link_libraries(pddl_simple_plan_dispatcher ${catkin_LIBRARIES})
 target_link_libraries(pddl_esterel_plan_dispatcher ${catkin_LIBRARIES})
@@ -152,6 +155,7 @@ install(TARGETS
     upm_planner_interface
     pprp_planner_interface
     pddl_simple_plan_parser
+    pddl_lama_plan_parser
     pddl_esterel_plan_parser
     pddl_simple_plan_dispatcher
     pddl_esterel_plan_dispatcher

rosplan_planning_system/include/rosplan_planning_system/PlanParsing/PDDLLAMAPlanParser.h

@@ -0,0 +1,46 @@
+/**
+ * This file parses standard PDDL output and generates a list of ActionDispatch messages.
+ */
+#include "PlanParser.h"
+
+#include "rosplan_dispatch_msgs/ActionDispatch.h"
+#include "rosplan_dispatch_msgs/CompletePlan.h"
+#include "rosplan_knowledge_msgs/GetDomainOperatorDetailsService.h"
+#include "diagnostic_msgs/KeyValue.h"
+
+#include <string>
+#include <sstream>
+
+#ifndef KCL_pddl_lama_plan_parser
+#define KCL_pddl_lama_plan_parser
+
+namespace KCL_rosplan {
+
+	class PDDLLAMAPlanParser: public PlanParser
+	{
+	private:
+
+		ros::ServiceClient get_operator_details_client;
+
+	protected:
+
+		/* post process plan */
+		void processPDDLParameters(rosplan_dispatch_msgs::ActionDispatch &msg, std::vector<std::string> &params);
+
+		/* virtual methods */
+		void reset();
+		void preparePlan();
+		void publishPlan();
+
+	public:
+
+		PDDLLAMAPlanParser(ros::NodeHandle& nh);
+		~PDDLLAMAPlanParser();
+
+		/* ROS interface */
+		ros::Publisher plan_publisher;
+
+	};
+} // close namespace
+
+#endif

rosplan_planning_system/src/PlanParsing/PDDLLAMAPlanParser.cpp

@@ -0,0 +1,155 @@
+#include "rosplan_planning_system/PlanParsing/PDDLLAMAPlanParser.h"
+
+namespace KCL_rosplan {
+
+	PDDLLAMAPlanParser::PDDLLAMAPlanParser(ros::NodeHandle& nh)
+	{
+		node_handle = &nh;
+
+		// fetching problem info for TILs
+		std::string kb = "knowledge_base";
+		node_handle->getParam("knowledge_base", kb);
+
+		std::stringstream ss;
+		ss << "/" << kb << "/domain/operator_details";
+		get_operator_details_client = nh.serviceClient<rosplan_knowledge_msgs::GetDomainOperatorDetailsService>(ss.str().c_str());
+		ss.str("");
+
+		// publishing parsed plan
+		std::string planTopic = "complete_plan";
+		node_handle->getParam("plan_topic", planTopic);
+		plan_publisher = node_handle->advertise<rosplan_dispatch_msgs::CompletePlan>(planTopic, 1, true);
+	}
+
+	PDDLLAMAPlanParser::~PDDLLAMAPlanParser()
+	{
+
+	}
+
+	void PDDLLAMAPlanParser::reset() {
+		action_list.clear();
+	}
+
+	void PDDLLAMAPlanParser::publishPlan() {
+		rosplan_dispatch_msgs::CompletePlan msg;
+		msg.plan = action_list;
+		plan_publisher.publish(msg);
+	}
+
+	/*----------------------*/
+	/* Post processing plan */
+	/*----------------------*/
+
+	/**
+	 * parses plan generated by LAMA planner, generating a list of action messages.
+	 */
+	void PDDLLAMAPlanParser::preparePlan() {
+
+		int curr, next;
+		std::string line;
+		std::istringstream planfile(planner_output);
+
+		size_t planFreeActionID = 0;
+
+		while (std::getline(planfile, line)) {
+
+			if (line.length() < 2)
+				break;
+
+			// check to see if the line is a comment
+			if (line.find(";", 0) == 0)
+				continue;
+
+			rosplan_dispatch_msgs::ActionDispatch msg;
+
+			// action ID
+			msg.action_id = planFreeActionID;
+			planFreeActionID++;
+
+			// check for parameters
+			curr = line.find("(") + 1;
+			bool paramsExist = (line.find(" ", curr) < line.find(")", curr));
+
+			if(paramsExist) {
+
+				// name
+				next = line.find(" ", curr);
+				std::string name = line.substr(curr, next-curr).c_str();
+				msg.name = name;
+
+				// parameters
+				std::vector<std::string> params;
+				curr = next + 1;
+				next = line.find(")", curr);
+				int at = curr;
+				while(at < next) {
+					int cc = line.find(" ", curr);
+					int cc1 = line.find(")", curr);
+					curr = cc < cc1 && cc != -1 ? cc : cc1;
+					std::string param = line.substr(at, curr-at);
+					params.push_back(param);
+					++curr;
+					at = curr;
+				}
+				processPDDLParameters(msg, params);
+
+
+			} else {
+
+				// name
+				next=line.find(")",curr);
+				std::string name = line.substr(curr,next-curr).c_str();
+				msg.name = name;
+
+			}
+
+			action_list.push_back(msg);
+		}
+	}
+
+	/**
+	 * processes the parameters of a single PDDL action into an ActionDispatch message
+	 */
+	void PDDLLAMAPlanParser::processPDDLParameters(rosplan_dispatch_msgs::ActionDispatch &msg, std::vector<std::string> &params) {
+
+		rosplan_knowledge_msgs::GetDomainOperatorDetailsService srv;
+		srv.request.name = msg.name;
+		if(!get_operator_details_client.call(srv)) {
+			ROS_ERROR("KCL: (%s) could not call Knowledge Base for operator details, %s", ros::this_node::getName().c_str(), msg.name.c_str());
+		} else {
+			std::vector<diagnostic_msgs::KeyValue> opParams = srv.response.op.formula.typed_parameters;
+			for(size_t i=0; i<opParams.size(); i++) {
+				diagnostic_msgs::KeyValue pair;
+				pair.key = opParams[i].key;
+				pair.value = params[i];
+				msg.parameters.push_back(pair);
+			}
+		}
+	}
+} // close namespace
+
+	/*-------------*/
+	/* Main method */
+	/*-------------*/
+
+	int main(int argc, char **argv) {
+
+		ros::init(argc,argv,"rosplan_plan_parser");
+		ros::NodeHandle nh("~");
+
+		KCL_rosplan::PDDLLAMAPlanParser 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));
+
+		ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str());
+		ros::spin();
+
+		return 0;
+	}