diff --git a/rosplan_planning_system/CMakeLists.txt b/rosplan_planning_system/CMakeLists.txt index 741ed96ee..8ea19604b 100644 --- a/rosplan_planning_system/CMakeLists.txt +++ b/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}) diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h new file mode 100644 index 000000000..a9c8f824c --- /dev/null +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h @@ -0,0 +1,38 @@ +#include +#include +#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 diff --git a/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp b/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp index 9e7d91043..54ef7049b 100644 --- a/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp +++ b/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; +} diff --git a/rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp b/rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp index a077b976c..8f69c392d 100644 --- a/rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp +++ b/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(&pp)); - - // start the plan parsing services - ros::ServiceServer service1 = nh.advertiseService("parse_plan", &KCL_rosplan::PlanParser::parsePlan, dynamic_cast(&pp)); - ros::ServiceServer service2 = nh.advertiseService("parse_plan_from_file", &KCL_rosplan::PlanParser::parsePlanFromFile, dynamic_cast(&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(&pp)); - return 0; - } + // start the plan parsing services + ros::ServiceServer service1 = nh.advertiseService("parse_plan", &KCL_rosplan::PlanParser::parsePlan, dynamic_cast(&pp)); + ros::ServiceServer service2 = nh.advertiseService("parse_plan_from_file", &KCL_rosplan::PlanParser::parsePlanFromFile, dynamic_cast(&pp)); + + ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str()); + ros::spin(); + + return 0; +} diff --git a/rosplan_planning_system/src/PlanParsing/PlanParser.cpp b/rosplan_planning_system/src/PlanParsing/PlanParser.cpp index 0cee1c5de..fa375ff0e 100644 --- a/rosplan_planning_system/src/PlanParsing/PlanParser.cpp +++ b/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(); diff --git a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp new file mode 100644 index 000000000..66e1c44b8 --- /dev/null +++ b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp @@ -0,0 +1,278 @@ +#include "rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h" + +#include +#include +#include +#include +#include +#include + +using namespace KCL_rosplan; + +namespace { + + typedef std::vector OperatorParams; + + unsigned int split_string(const std::string &text, std::vector &tokens, char separator) { + size_t pos = text.find(separator); + unsigned int initialPos = 0; + tokens.clear(); + + while(pos != std::string::npos && pos < text.length()) { + if(text.substr(initialPos, pos - initialPos + 1) !=" ") { + std::string s = text.substr(initialPos, pos - initialPos + 1); + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + tokens.push_back(s); + } + initialPos = pos + 1; + pos = text.find(separator, initialPos); + } + + tokens.push_back(text.substr(initialPos, text.size() - initialPos)); + return tokens.size(); + } + + void extractElementsFromLine(const std::string &line, std::string &action_id, std::string &operator_name, OperatorParams &operator_params) { + + // Example of actions of Contigent-FF: + // 17||0 --- FIND_OBJECT C1 ITEM_0 --- SON: 18||0 + // 18||0 --- PICKUP_OBJECT C1 ITEM_0 --- SON: 19||0 + // + + std::vector tokens; + + split_string(line, tokens, ' '); + action_id = tokens[0].substr(0, tokens[0].find("||")); + operator_name = tokens[2]; + + int idx = 3; + while (idx < tokens.size() && tokens[idx] != "---") { + operator_params.push_back(tokens[idx]); + idx++; + } + + ROS_DEBUG("KCL: (%s) Elements in line: { %s, %s, etc. }", ros::this_node::getName().c_str(), action_id.c_str(), operator_name.c_str()); + } + + void createAction(const std::string &action_id, const std::string &operator_name, const OperatorParams &operator_params, std::string &action) { + + std::string operator_parameters; + for (OperatorParams::const_iterator it = operator_params.begin(); it != operator_params.end(); ++it) { + if(std::next(it) != operator_params.end()) { + operator_parameters = operator_parameters + (*it) + " "; + } else { + operator_parameters = operator_parameters + (*it); + } + } + + action = action_id + ": " + "(" + operator_name + " " + operator_parameters + ") [0.001]\n"; + ROS_DEBUG("KCL: (%s) Action: %s", ros::this_node::getName().c_str(), action.c_str()); + } +} + +CFFPlannerInterface::CFFPlannerInterface(ros::NodeHandle& nh) { + + node_handle = &nh; + plan_server = + new actionlib::SimpleActionServer((*node_handle), + "start_planning", + boost::bind(&PlannerInterface::runPlanningServerAction, this, _1), + false); + + std::string plannerTopic = "planner_output"; + node_handle->getParam("planner_topic", plannerTopic); + plan_publisher = node_handle->advertise(plannerTopic, 1, true); + + plan_server->start(); +} + +CFFPlannerInterface::~CFFPlannerInterface() { + delete plan_server; +} + +void CFFPlannerInterface::clearPreviousPlan() { + planner_output.clear(); +} + +void CFFPlannerInterface::saveProblemToFileIfNeeded() { + + ROS_INFO("KCL: (%s) Need to write problem file: %d", ros::this_node::getName().c_str(), + use_problem_topic && problem_instance_received); + + if(use_problem_topic && problem_instance_received) { + ROS_DEBUG("KCL: (%s) Writing problem to file: %s", ros::this_node::getName().c_str(), problem_name.c_str()); + + std::ofstream dest; + dest.open((problem_path).c_str()); + dest << problem_instance; + dest.close(); + } +} + +std::string CFFPlannerInterface::runCommand(std::string cmd) { + + ROS_INFO("KCL: (%s) Command: %s", ros::this_node::getName().c_str(), cmd.c_str()); + + 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 CFFPlannerInterface::callExternalPlanner() { + + std::string command_template = planner_command; + + std::size_t dit = command_template.find("DOMAIN"); + if(dit != std::string::npos) { + command_template.replace(dit, 6, domain_path); + } + + std::size_t pit = command_template.find("PROBLEM"); + if(pit != std::string::npos) { + command_template.replace(pit, 7, problem_path); + } + + std::string command = command_template + " > " + data_path + "plan.cff"; + + ROS_INFO("KCL: (%s) (%s) Running planner: %s", ros::this_node::getName().c_str(), problem_name.c_str(), command.c_str()); + std::string plan = runCommand(command.c_str()); + ROS_INFO("KCL: (%s) (%s) Planning complete", ros::this_node::getName().c_str(), problem_name.c_str()); +} + +bool CFFPlannerInterface::isPlanSolved(std::ifstream &plan_file) { + + bool solved = false; + const std::string CFF_SOLVED_PLAN_PATTERN = "ff: found plan as follows"; + + std::string line; + while (not solved and std::getline(plan_file, line)) { + if (line.find(CFF_SOLVED_PLAN_PATTERN) != line.npos) { + solved = true; + } + } + + ROS_INFO("KCL: (%s) Is plan solved? %d", ros::this_node::getName().c_str(), solved); + return solved; +} + +void CFFPlannerInterface::savePlanInPopfFormatToFile() { + + std::string plan_file = data_path + "plan.pddl"; + ROS_INFO("KCL: (%s) Writing converted plan (pddl) to file: %s", ros::this_node::getName().c_str(), planner_output.c_str()); + + std::ofstream dest; + dest.open(plan_file.c_str()); + dest << planner_output; + dest.close(); +} + +void CFFPlannerInterface::convertPlanToPopfFormat(std::ifstream &plan_file) { + + ROS_INFO("KCL: (%s) cff plan to POPF format", ros::this_node::getName().c_str()); + + bool isParseCompleted = false; + std::string line; + + while(!plan_file.eof() && !isParseCompleted) { + + std::getline(plan_file, line); + + if (!line.empty()) { + if (!(line.compare("-------------------------------------------------") == 0)) { + + std::vector tokens; + split_string(line, tokens, ' '); + + std::string action_id; + std::string operator_name; + OperatorParams operator_params; + extractElementsFromLine(line, action_id, operator_name, operator_params); + + std::string action; + createAction(action_id, operator_name, operator_params, action); + + planner_output += action; + ROS_INFO("KCL: (%s) Action: { %s }", ros::this_node::getName().c_str(), action.c_str()); + } + } else { + isParseCompleted = true; + } + } + + if (isParseCompleted) + { + std::transform(planner_output.begin(), planner_output.end(), planner_output.begin(), ::tolower); + } + + ROS_INFO("KCL: (%s) Plan converted to popf format: %d", ros::this_node::getName().c_str(), isParseCompleted); +} + +bool CFFPlannerInterface::parsePlan() { + + bool solved = false; + + std::string file_path = data_path + "plan.cff"; + ROS_INFO("KCL: (%s) Plan to parse: %s", ros::this_node::getName().c_str(), file_path.c_str()); + + std::ifstream plan_file; + plan_file.open(file_path.c_str()); + + solved = isPlanSolved(plan_file); + if (solved) { + convertPlanToPopfFormat(plan_file); + savePlanInPopfFormatToFile(); + } + plan_file.close(); + + return solved; +} + +bool CFFPlannerInterface::runPlanner() { + + bool solved = false; + + clearPreviousPlan(); + saveProblemToFileIfNeeded(); + + callExternalPlanner(); + solved = parsePlan(); + + ROS_INFO("KCL: (%s) (%s) Was plan solved? %d", ros::this_node::getName().c_str(), problem_name.c_str(), solved); + return solved; +} + +int main(int argc, char **argv) { + + srand(static_cast (time(0))); + + ros::init(argc,argv,"rosplan_planner_interface"); + ros::NodeHandle nh("~"); + KCL_rosplan::CFFPlannerInterface pi(nh); + + std::string problem_topic_name = "problem_instance"; + nh.getParam("problem_topic", problem_topic_name); + + ros::Subscriber problem_subcriber + = nh.subscribe(problem_topic_name, 1, &KCL_rosplan::PlannerInterface::problemCallback, + dynamic_cast(&pi)); + + ros::ServiceServer planning_service + = nh.advertiseService("planning_server", &KCL_rosplan::PlannerInterface::runPlanningServerDefault, + dynamic_cast(&pi)); + ros::ServiceServer planning_with_params_service + = nh.advertiseService("planning_server_params", &KCL_rosplan::PlannerInterface::runPlanningServerParams, + dynamic_cast(&pi)); + + ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str()); + ros::spin(); + + return 0; +} diff --git a/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp index dbeb68b89..2146a490f 100644 --- a/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp @@ -8,6 +8,7 @@ namespace KCL_rosplan { void PlannerInterface::problemCallback(const std_msgs::String& problemInstance) { ROS_INFO("KCL: (%s) Problem received.", ros::this_node::getName().c_str()); + ROS_INFO("KCL: (%s) Is problem empty? %d", ros::this_node::getName().c_str(), problemInstance.data.size() == 0); problem_instance_received = true; problem_instance_time = ros::WallTime::now().toSec(); problem_instance = problemInstance.data; @@ -68,7 +69,7 @@ namespace KCL_rosplan { * planning system; prepares planning; calls planner; parses plan. */ bool PlannerInterface::runPlanningServer(std::string domainPath, std::string problemPath, std::string dataPath, std::string plannerCommand, bool useProblemTopic) { - + // save parameters data_path = dataPath; domain_path = domainPath; @@ -76,9 +77,9 @@ namespace KCL_rosplan { planner_command = plannerCommand; use_problem_topic = useProblemTopic; - // check if data_path ends in "/" and add "/" if not - const char *last_char = &data_path.back(); - if (strcmp(last_char,"/") != 0)data_path = data_path + "/"; + // check if data_path ends in "/" and add "/" if not + const char *last_char = &data_path.back(); + if (strcmp(last_char,"/") != 0)data_path = data_path + "/"; // set problem name for ROS_INFO std::size_t lastDivide = problem_path.find_last_of("/\\"); @@ -97,6 +98,7 @@ namespace KCL_rosplan { // publish planner output if(success) { + ROS_INFO("KCL: (%s) Plan published.", ros::this_node::getName().c_str()); std_msgs::String planMsg; planMsg.data = planner_output; plan_publisher.publish(planMsg); diff --git a/rosplan_planning_system/src/ProblemGeneration/ProblemInterface.cpp b/rosplan_planning_system/src/ProblemGeneration/ProblemInterface.cpp index 75b606a2c..a4f083f98 100644 --- a/rosplan_planning_system/src/ProblemGeneration/ProblemInterface.cpp +++ b/rosplan_planning_system/src/ProblemGeneration/ProblemInterface.cpp @@ -132,6 +132,7 @@ namespace KCL_rosplan { // publish problem std::ifstream problemIn(problem_path.c_str()); if(problemIn) { + ROS_DEBUG("KCL: (%s) (%s) Publish problem.", ros::this_node::getName().c_str(), problem_name.c_str()); std_msgs::String problemMsg; problemMsg.data = std::string(std::istreambuf_iterator(problemIn), std::istreambuf_iterator()); problem_publisher.publish(problemMsg); @@ -141,25 +142,25 @@ namespace KCL_rosplan { } } // close namespace - /*-------------*/ - /* Main method */ - /*-------------*/ +/*-------------*/ +/* Main method */ +/*-------------*/ - int main(int argc, char **argv) { +int main(int argc, char **argv) { - srand (static_cast (time(0))); + srand (static_cast (time(0))); - ros::init(argc,argv,"rosplan_problem_interface"); - ros::NodeHandle nh("~"); + ros::init(argc,argv,"rosplan_problem_interface"); + ros::NodeHandle nh("~"); - KCL_rosplan::ProblemInterface ProblemInterface(nh); + KCL_rosplan::ProblemInterface ProblemInterface(nh); - // start the planning services - ros::ServiceServer service1 = nh.advertiseService("problem_generation_server", &KCL_rosplan::ProblemInterface::runProblemServerDefault, &ProblemInterface); - ros::ServiceServer service2 = nh.advertiseService("problem_generation_server_params", &KCL_rosplan::ProblemInterface::runProblemServerParams, &ProblemInterface); + // start the planning services + ros::ServiceServer service1 = nh.advertiseService("problem_generation_server", &KCL_rosplan::ProblemInterface::runProblemServerDefault, &ProblemInterface); + ros::ServiceServer service2 = nh.advertiseService("problem_generation_server_params", &KCL_rosplan::ProblemInterface::runProblemServerParams, &ProblemInterface); - 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; +} diff --git a/rosplan_planning_system/test/launch/cff_planner_interface.test b/rosplan_planning_system/test/launch/cff_planner_interface.test new file mode 100644 index 000000000..5445ee78a --- /dev/null +++ b/rosplan_planning_system/test/launch/cff_planner_interface.test @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rosplan_planning_system/test/launch/planner_interface.test b/rosplan_planning_system/test/launch/popf_planner_interface.test similarity index 93% rename from rosplan_planning_system/test/launch/planner_interface.test rename to rosplan_planning_system/test/launch/popf_planner_interface.test index 17cce96e8..8ac0a86db 100644 --- a/rosplan_planning_system/test/launch/planner_interface.test +++ b/rosplan_planning_system/test/launch/popf_planner_interface.test @@ -35,6 +35,6 @@ - + diff --git a/rosplan_planning_system/test/src/CffPlannerInterfaceTests.cpp b/rosplan_planning_system/test/src/CffPlannerInterfaceTests.cpp new file mode 100644 index 000000000..c22493aaa --- /dev/null +++ b/rosplan_planning_system/test/src/CffPlannerInterfaceTests.cpp @@ -0,0 +1,153 @@ +/** + * + * Copyright [2019] + * + * Author: Martin Koling (martinh.koling@kcl.ac.uk) + * Author: Oscar Lima (oscar.lima@dfki.de) + * + * Unit tests for ROSPlan planner interface + */ + +#include +#include +#include +#include + +#include "rosplan_planning_system/PlannerInterface/PlannerInterface.h" +#include "rosplan_dispatch_msgs/PlanningService.h" + +bool plan_received; +std::string last_plan; + +void testCallback(const std_msgs::String::ConstPtr &plan) { + ROS_INFO("I heard: [%s]", plan->data.c_str()); + last_plan = plan->data; + plan_received = true; +} + +GTEST_TEST(PlannerInterfaceTests, Test1_plan_found) { + + ros::NodeHandle nh("~"); + + std::string srv_name = "/rosplan_planner_interface/planning_server_params"; + ros::ServiceClient client = nh.serviceClient(srv_name); + rosplan_dispatch_msgs::PlanningService srv; + + std::string rosplan_planning_system_path = ros::package::getPath("rosplan_planning_system"); + + srv.request.use_problem_topic = false; + srv.request.data_path = rosplan_planning_system_path + "/test/pddl/test_domain"; + srv.request.domain_path = rosplan_planning_system_path + "/test/pddl/test_domain/domain.pddl"; + srv.request.problem_path = rosplan_planning_system_path + "/test/pddl/test_domain/test_problem.pddl"; + srv.request.planner_command = "timeout 10 " + rosplan_planning_system_path + "/common/bin/Contingent-FF -o DOMAIN -f PROBLEM"; + + ros::service::waitForService(srv_name, ros::Duration(3)); + bool is_srv_call_successful = false; + if(client.call(srv)) + is_srv_call_successful = true; + + EXPECT_TRUE(is_srv_call_successful); + EXPECT_TRUE(srv.response.plan_found); +} + +GTEST_TEST(PlannerInterfaceTests, Test2_format_published_on_planner_output) { + + ros::NodeHandle nh("~"); + + ros::Subscriber sub = nh.subscribe("/rosplan_planner_interface/planner_output", 1000, &testCallback); + ros::Publisher pub = nh.advertise("/rosplan_problem_interface/problem_instance", 1000); + + std::string srv_name = "/rosplan_planner_interface/planning_server_params"; + ros::ServiceClient client1 = nh.serviceClient(srv_name); + rosplan_dispatch_msgs::PlanningService srv; + + std::string rosplan_planning_system_path = ros::package::getPath("rosplan_planning_system"); + + srv.request.use_problem_topic = false; + srv.request.data_path = rosplan_planning_system_path + "/test/pddl/test_domain/"; + srv.request.domain_path = rosplan_planning_system_path + "/test/pddl/test_domain/domain.pddl"; + srv.request.problem_path = rosplan_planning_system_path + "/test/pddl/test_domain/test_problem.pddl"; + srv.request.planner_command = "timeout 10 " + rosplan_planning_system_path + "/common/bin/Contingent-FF -o DOMAIN -f PROBLEM"; + + std::ifstream t(rosplan_planning_system_path + "/test/pddl/test_domain/test_problem.pddl"); + std::stringstream ss; + ss << t.rdbuf(); + + std_msgs::String msg; + msg.data = ss.str(); + pub.publish(msg); + + ros::spinOnce(); + + ros::Rate loop_rate = 10; + loop_rate.sleep(); + + ros::service::waitForService(srv_name, ros::Duration(3)); + client1.call(srv); + + plan_received = false; + while (!plan_received && ros::ok()) { + loop_rate.sleep(); + ros::spinOnce(); + } + + std::string known_plan = "0: (movetob ball) [0.001]\n"; + + EXPECT_TRUE(srv.response.plan_found); + ASSERT_EQ(last_plan, known_plan); +} + +GTEST_TEST(PlannerInterfaceTests, Test3_problem_without_solution) { + + ros::NodeHandle nh("~"); + + std::string srv_name = "/rosplan_planner_interface/planning_server_params"; + ros::ServiceClient client = nh.serviceClient(srv_name); + rosplan_dispatch_msgs::PlanningService srv; + + std::string rosplan_planning_system_path = ros::package::getPath("rosplan_planning_system"); + + srv.request.use_problem_topic = false; + srv.request.data_path = rosplan_planning_system_path + "/test/pddl/test_domain/"; + srv.request.domain_path = rosplan_planning_system_path + "/test/pddl/test_domain/domain.pddl"; + srv.request.problem_path = rosplan_planning_system_path + "/test/pddl/test_domain/test_problem_no_solution.pddl"; + srv.request.planner_command = "timeout 10 " + rosplan_planning_system_path + "/common/bin/Contingent-FF -o DOMAIN -f PROBLEM"; + + ros::service::waitForService(srv_name, ros::Duration(3)); + bool is_srv_call_successful = false; + if(client.call(srv)) + is_srv_call_successful = true; + + EXPECT_TRUE(is_srv_call_successful); + EXPECT_FALSE(srv.response.plan_found); +} + +GTEST_TEST(PlannerInterfaceTests, Test4_invalid_pddl_syntax) { + + ros::NodeHandle nh("~"); + + std::string srv_name = "/rosplan_planner_interface/planning_server_params"; + ros::ServiceClient client1 = nh.serviceClient(srv_name); + rosplan_dispatch_msgs::PlanningService srv; + + std::string rosplan_planning_system_path = ros::package::getPath("rosplan_planning_system"); + + srv.request.use_problem_topic = false; + srv.request.data_path = rosplan_planning_system_path + "/test/pddl/test_domain/"; + srv.request.domain_path = rosplan_planning_system_path + "/test/pddl/test_domain/domain.pddl"; + srv.request.problem_path = rosplan_planning_system_path + "/test/pddl/test_domain/test_problem_invalid_syntax.pddl"; + srv.request.planner_command = "timeout 10 " + rosplan_planning_system_path + "/common/bin/Contingent-FF -o DOMAIN -f PROBLEM"; + + ros::service::waitForService(srv_name, ros::Duration(3)); + EXPECT_TRUE(client1.call(srv)); + EXPECT_FALSE(srv.response.plan_found); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "CffPlannerInterfaceTests"); + + return RUN_ALL_TESTS(); +} diff --git a/rosplan_planning_system/test/src/PlannerInterfaceTests.cpp b/rosplan_planning_system/test/src/PopfPlannerInterfaceTests.cpp similarity index 97% rename from rosplan_planning_system/test/src/PlannerInterfaceTests.cpp rename to rosplan_planning_system/test/src/PopfPlannerInterfaceTests.cpp index 19a309648..b60c23f24 100644 --- a/rosplan_planning_system/test/src/PlannerInterfaceTests.cpp +++ b/rosplan_planning_system/test/src/PopfPlannerInterfaceTests.cpp @@ -8,11 +8,11 @@ * Unit tests for ROSPlan planner interface */ +#include #include #include #include -#include "rosplan_planning_system/PlannerInterface/POPFPlannerInterface.h" #include "rosplan_planning_system/PlannerInterface/PlannerInterface.h" #include "rosplan_dispatch_msgs/PlanningService.h" @@ -147,7 +147,7 @@ GTEST_TEST(PlannerInterfaceTests, Test4_invalid_pddl_syntax) { int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "PlannerInterfaceTests"); + ros::init(argc, argv, "PopfPlannerInterfaceTests"); return RUN_ALL_TESTS(); }