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.
[planning_system] Added a PANDA interface and problem generator
- Loading branch information
1 parent
90b3c45
commit 376d04a
Showing
11 changed files
with
256 additions
and
9 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
36 changes: 36 additions & 0 deletions
36
..._planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.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,36 @@ | ||
| #include "PlannerInterface.h" | ||
| #include <fstream> | ||
| #include <sstream> | ||
| #include <string> | ||
| #include <ctime> | ||
| #include <string> | ||
| #include <streambuf> | ||
|
|
||
| #ifndef KCL_PANDA_planner_interface | ||
| #define KCL_PANDA_planner_interface | ||
|
|
||
| /** | ||
| * This file contains an interface to the planner. | ||
| */ | ||
| namespace KCL_rosplan { | ||
|
|
||
| class PANDAPlannerInterface: public PlannerInterface | ||
| { | ||
| private: | ||
|
|
||
| /* runs external commands */ | ||
| std::string runCommand(std::string cmd); | ||
|
|
||
| protected: | ||
|
|
||
| bool runPlanner(); | ||
|
|
||
| public: | ||
|
|
||
| PANDAPlannerInterface(ros::NodeHandle& nh); | ||
| virtual ~PANDAPlannerInterface(); | ||
| }; | ||
|
|
||
| } // close namespace | ||
|
|
||
| #endif |
17 changes: 17 additions & 0 deletions
17
..._planning_system/include/rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.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,17 @@ | ||
| #ifndef ROSPLAN_HDDL_PROBLEM_GENERATOR_H | ||
| #define ROSPLAN_HDDL_PROBLEM_GENERATOR_H | ||
|
|
||
| #include "PDDLProblemGenerator.h" | ||
|
|
||
| namespace KCL_rosplan { | ||
|
|
||
| class HDDLProblemGenerator : public PDDLProblemGenerator { | ||
| protected: | ||
| virtual void makeProblem(std::ofstream &pFile); | ||
| void makeTasks(std::ofstream &pFile); | ||
| public: | ||
| HDDLProblemGenerator(const std::string& kb): PDDLProblemGenerator(kb) {}; | ||
| }; | ||
| } // close namespace | ||
|
|
||
| #endif |
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
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
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
139 changes: 139 additions & 0 deletions
139
rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.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,139 @@ | ||
| #include "rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h" | ||
|
|
||
| namespace KCL_rosplan { | ||
|
|
||
| /*-------------*/ | ||
| /* constructor */ | ||
| /*-------------*/ | ||
|
|
||
| PANDAPlannerInterface::PANDAPlannerInterface(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(); | ||
| } | ||
|
|
||
| PANDAPlannerInterface::~PANDAPlannerInterface() | ||
| { | ||
| 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 */ | ||
| /*------------------*/ | ||
|
|
||
| /** | ||
| * passes the problem to the Planner; the plan to post-processing. | ||
| */ | ||
| 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(); | ||
| } | ||
|
|
||
| // 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.hddl"; | ||
|
|
||
| // 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()); | ||
|
|
||
| // check the planner solved the problem | ||
| std::ifstream planfile; | ||
| planfile.open((data_path + "plan.hddl").c_str()); | ||
| std::string line; | ||
| std::stringstream ss; | ||
|
|
||
| int curr, next; | ||
| bool solved = false; | ||
| double planDuration; | ||
|
|
||
| while (std::getline(planfile, line)) { | ||
| if (line.find("SOLUTION SEQUENCE", 0) != std::string::npos) { | ||
| solved = true; | ||
| } else { | ||
| // if a solution has been found, we read the plan | ||
| if (solved) | ||
| { | ||
| // read a plan (might not be the last plan) | ||
| planDuration = 0; | ||
| ss.str(""); | ||
| while (std::getline(planfile, line)) { | ||
| if (line.length()<2) | ||
| break; | ||
| ss << line << std::endl; | ||
| } | ||
| planner_output = ss.str(); | ||
| } | ||
| } | ||
| } | ||
| 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()); | ||
|
|
||
| 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::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<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; | ||
| } |
39 changes: 39 additions & 0 deletions
39
rosplan_planning_system/src/ProblemGeneration/HDDLProblemGenerator.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,39 @@ | ||
| #include "rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h" | ||
|
|
||
| namespace KCL_rosplan { | ||
| void HDDLProblemGenerator::makeTasks(std::ofstream &pFile) { | ||
| ros::NodeHandle nh; | ||
| ros::ServiceClient getCurrentTasksClient = nh.serviceClient<rosplan_knowledge_msgs::GetTaskService>(task_service); | ||
|
|
||
| pFile << "(:htn\n :tasks (and" << std::endl; | ||
|
|
||
| // get current tasks | ||
| rosplan_knowledge_msgs::GetTaskService currentTaskSrv; | ||
| if (!getCurrentTasksClient.call(currentTaskSrv)) { | ||
| ROS_ERROR("KCL: (HDDLProblemGenerator) Failed to call service %s", task_service.c_str()); | ||
| } else { | ||
| for(size_t i=0;i<currentTaskSrv.response.attributes.size();i++) { | ||
| rosplan_knowledge_msgs::KnowledgeItem attr = currentTaskSrv.response.attributes[i]; | ||
| if(attr.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::TASK) { | ||
| pFile << " (" + attr.attribute_name; | ||
| for(size_t j=0; j<attr.values.size(); j++) { | ||
| pFile << " " << attr.values[j].value; | ||
| } | ||
| pFile << ")" << std::endl; | ||
| } | ||
| } | ||
| } | ||
| pFile << "))" << std::endl; | ||
| } | ||
|
|
||
| void HDDLProblemGenerator::makeProblem(std::ofstream &pFile) { | ||
| makeHeader(pFile); | ||
| makeInitialState(pFile); | ||
| makeTasks(pFile); | ||
| makeGoals(pFile); | ||
| makeMetric(pFile); | ||
|
|
||
| // add end of problem file | ||
| pFile << ")" << std::endl; | ||
| }; | ||
| } |
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
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
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