Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

implemented abstraction to planner interface code #328

Closed
wants to merge 1 commit into from
Closed

implemented abstraction to planner interface code #328

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 16 additions & 39 deletions rosplan_planning_system/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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})
Expand All @@ -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})
Expand Down
8 changes: 0 additions & 8 deletions ..._planning_system/include/rosplan_planning_system/PlannerInterface/CHIMPPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
3 changes: 0 additions & 3 deletions ...lan_planning_system/include/rosplan_planning_system/PlannerInterface/FDPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,6 @@ namespace KCL_rosplan {
{
private:

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

bool runPlanner();
Expand Down
3 changes: 0 additions & 3 deletions ...lan_planning_system/include/rosplan_planning_system/PlannerInterface/FFPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,6 @@ namespace KCL_rosplan {

bool use_ffha;

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

bool runPlanner();
Expand Down
3 changes: 0 additions & 3 deletions ...an_planning_system/include/rosplan_planning_system/PlannerInterface/LPGPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,6 @@ namespace KCL_rosplan {
{
private:

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

bool runPlanner();
Expand Down
2 changes: 1 addition & 1 deletion ...planning_system/include/rosplan_planning_system/PlannerInterface/OnlinePlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
3 changes: 0 additions & 3 deletions ..._planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,6 @@ namespace KCL_rosplan {
{
private:

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

bool runPlanner();
Expand Down
3 changes: 0 additions & 3 deletions ...n_planning_system/include/rosplan_planning_system/PlannerInterface/POPFPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,6 @@ namespace KCL_rosplan {
{
private:

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

bool runPlanner();
Expand Down
3 changes: 0 additions & 3 deletions ...n_planning_system/include/rosplan_planning_system/PlannerInterface/PPRPPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,6 @@ namespace KCL_rosplan {
{
private:

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

bool runPlanner();
Expand Down
24 changes: 24 additions & 0 deletions rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*/
Expand Down Expand Up @@ -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);
Expand Down
3 changes: 0 additions & 3 deletions ...lanning_system/include/rosplan_planning_system/PlannerInterface/RDDLSIMPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,6 @@ namespace KCL_rosplan {
{
private:

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

bool runPlanner();
Expand Down
3 changes: 0 additions & 3 deletions ...an_planning_system/include/rosplan_planning_system/PlannerInterface/SMTPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,6 @@ namespace KCL_rosplan {
{
private:

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

bool runPlanner();
Expand Down
3 changes: 0 additions & 3 deletions ...an_planning_system/include/rosplan_planning_system/PlannerInterface/TFDPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,6 @@ namespace KCL_rosplan {
{
private:

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

bool runPlanner();
Expand Down
3 changes: 0 additions & 3 deletions ...an_planning_system/include/rosplan_planning_system/PlannerInterface/UPMPlannerInterface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,6 @@ namespace KCL_rosplan {
{
private:

/* runs external commands */
std::string runCommand(std::string cmd);

protected:

std::string input_problem_path;
Expand Down
100 changes: 11 additions & 89 deletions rosplan_planning_system/src/PlannerInterface/CHIMPPlannerInterface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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;
}
Loading
Loading