forked from KCL-Planning/ROSPlan
-
Notifications
You must be signed in to change notification settings - Fork 4
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'master' of github.com:b-it-bots/ROSPlan
- Loading branch information
Showing
5 changed files
with
414 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
46 changes: 46 additions & 0 deletions
46
rosplan_planning_system/include/rosplan_planning_system/PlanParsing/PDDLLAMAPlanParser.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> ¶ms); | ||
|
|
||
| /* virtual methods */ | ||
| void reset(); | ||
| void preparePlan(); | ||
| void publishPlan(); | ||
|
|
||
| public: | ||
|
|
||
| PDDLLAMAPlanParser(ros::NodeHandle& nh); | ||
| ~PDDLLAMAPlanParser(); | ||
|
|
||
| /* ROS interface */ | ||
| ros::Publisher plan_publisher; | ||
|
|
||
| }; | ||
| } // close namespace | ||
|
|
||
| #endif |
39 changes: 39 additions & 0 deletions
39
...n_planning_system/include/rosplan_planning_system/PlannerInterface/LAMAPlannerInterface.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,39 @@ | ||
| #include "PlannerInterface.h" | ||
| #include <fstream> | ||
| #include <sstream> | ||
| #include <string> | ||
| #include <ctime> | ||
| #include <string> | ||
| #include <streambuf> | ||
|
|
||
| #ifndef KCL_LAMA_planner_interface | ||
| #define KCL_LAMA_planner_interface | ||
|
|
||
| /** | ||
| * This file contains an interface to the planner. | ||
| */ | ||
| namespace KCL_rosplan { | ||
|
|
||
| class LAMAPlannerInterface: public PlannerInterface | ||
| { | ||
| private: | ||
|
|
||
| /* runs external commands */ | ||
| std::string runCommand(std::string cmd); | ||
|
|
||
| /* remove all white space from a string */ | ||
| std::string strip(std::string str); | ||
|
|
||
| protected: | ||
|
|
||
| bool runPlanner(); | ||
|
|
||
| public: | ||
|
|
||
| LAMAPlannerInterface(ros::NodeHandle& nh); | ||
| virtual ~LAMAPlannerInterface(); | ||
| }; | ||
|
|
||
| } // close namespace | ||
|
|
||
| #endif |
155 changes: 155 additions & 0 deletions
155
rosplan_planning_system/src/PlanParsing/PDDLLAMAPlanParser.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> ¶ms) { | ||
|
|
||
| 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; | ||
| } |
Oops, something went wrong.