From 6bbd5914685a207ae86bdddf3623e6b7f1bb6d7c Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 5 Jul 2020 09:57:37 +0200 Subject: [PATCH 01/12] Add specific PlannerInterface node for Contingent-FF. --- .idea/new_rosplan.iml | 8 + rosplan_planning_system/CMakeLists.txt | 7 +- .../PlannerInterface/CFFPlannerInterface.h | 39 ++++ .../PlannerInterface/CFFPlannerInterface.cpp | 177 ++++++++++++++++++ 4 files changed, 229 insertions(+), 2 deletions(-) create mode 100644 .idea/new_rosplan.iml create mode 100644 rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h create mode 100644 rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp diff --git a/.idea/new_rosplan.iml b/.idea/new_rosplan.iml new file mode 100644 index 000000000..d0876a78d --- /dev/null +++ b/.idea/new_rosplan.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/rosplan_planning_system/CMakeLists.txt b/rosplan_planning_system/CMakeLists.txt index 741ed96ee..488c9b342 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}) 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..a8f0c622e --- /dev/null +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h @@ -0,0 +1,39 @@ +#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: + + bool use_ffha; + + void clearPreviousPlan(); + void saveProblemToFileIfNeeded(); + void callExternalPlanner(); + std::string runCommand(std::string cmd); + bool isPlanSolved(std::ifstream &plan_file); + void convertPlanToPopfFormat(std::ifstream &plan_file); + bool parsePlan(); + + protected: + + bool runPlanner(); + + public: + + CFFPlannerInterface(ros::NodeHandle& nh); + virtual ~CFFPlannerInterface(); + }; + +} // close namespace + +#endif diff --git a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp new file mode 100644 index 000000000..1965723e0 --- /dev/null +++ b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp @@ -0,0 +1,177 @@ +#include "rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h" + +#include +#include +#include +#include +#include + +using namespace KCL_rosplan; + +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); + + node_handle->param("use_ffha", this->use_ffha, false); + + plan_server->start(); +} + +CFFPlannerInterface::~CFFPlannerInterface() +{ + delete plan_server; +} + +void CFFPlannerInterface::clearPreviousPlan() { + planner_output.clear(); +} + +void CFFPlannerInterface::saveProblemToFileIfNeeded() { + 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(); + } +} + +std::string CFFPlannerInterface::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 CFFPlannerInterface::callExternalPlanner() { + // 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.pddl"; + + // 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()); +} + +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; + } + } + + return solved; +} + +void CFFPlannerInterface::convertPlanToPopfFormat(std::ifstream &plan_file) { + std::string line; + + if (this->use_ffha) { + std::getline(plan_file, line); //go to first action line + } else { + // actions look like this: + // step 0: got_place C1 + // 1: find_object V1 C1 + // plan cost: XX + while (std::getline(plan_file, line)) { // Move to the beginning of the plan + if (line.substr(0, 4) == "step") { + line = line.substr(4); // Remove the step + break; + } + } + } + + // First iteration line will be like 0: got_place C1 + while (line.find("Total cost") == line.npos and line.find("time spend") == line.npos and line.size() > 0) { + std::stringstream ss(line); // To trim whitespaces + std::string aux; + // Read the action number X: + ss >> aux; + std::string add = (use_ffha) ? " " : " ("; + planner_output += aux + add; // Add parenthesis before the action + while (ss >> aux) { // Read the rest + planner_output += aux; + if (ss.good()) planner_output += " "; // Add a whitespace unless we have processed all the line + } + add = (use_ffha) ? " [0.001]\n" : ") [0.001]\n"; + planner_output += add; // Close parenthesis and add duration + std::getline(plan_file, line); + } + // Convert to lowercase as FF prints all the actions and parameters in uppercase + std::transform(planner_output.begin(), planner_output.end(), planner_output.begin(), ::tolower); +} + +bool CFFPlannerInterface::parsePlan() { + std::ifstream plan_file; + plan_file.open((data_path + "plan.pddl").c_str()); + + bool solved = isPlanSolved(plan_file); + if (solved) { + convertPlanToPopfFormat(plan_file); + } + + plan_file.close(); +} + +bool CFFPlannerInterface::runPlanner() { + bool success = false; + + clearPreviousPlan(); + saveProblemToFileIfNeeded(); + + callExternalPlanner(); + success = parsePlan(); + + if(!success) 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 success; +} + +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); + + // 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(&pi)); + + // start the planning services + ros::ServiceServer service1 = nh.advertiseService("planning_server", &KCL_rosplan::PlannerInterface::runPlanningServerDefault, dynamic_cast(&pi)); + ros::ServiceServer service2 = 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; +} From 6f82ec6076ceacef56ff4005e76e059c27d280b2 Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 5 Jul 2020 10:00:11 +0200 Subject: [PATCH 02/12] Add several ROS log traces. --- .../src/PlanDispatch/SimplePlanDispatcher.cpp | 8 ++++++-- .../src/PlanParsing/PDDLSimplePlanParser.cpp | 5 ++++- rosplan_planning_system/src/PlanParsing/PlanParser.cpp | 6 ++++-- .../src/PlannerInterface/PlannerInterface.cpp | 6 ++++-- .../src/ProblemGeneration/ProblemInterface.cpp | 1 + 5 files changed, 19 insertions(+), 7 deletions(-) diff --git a/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp b/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp index 9e7d91043..71db791b8 100644 --- a/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp +++ b/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp @@ -38,7 +38,9 @@ namespace KCL_rosplan { void SimplePlanDispatcher::planCallback(const rosplan_dispatch_msgs::CompletePlan plan) { ROS_INFO("KCL: (%s) Plan received.", ros::this_node::getName().c_str()); - plan_received = true; + 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 +55,8 @@ namespace KCL_rosplan { bool SimplePlanDispatcher::dispatchPlan(double missionStartTime, double planStartTime) { ROS_INFO("KCL: (%s) Dispatching plan", ros::this_node::getName().c_str()); + ROS_INFO("KCL: (%s) Num actions: %d", ros::this_node::getName().c_str(), current_plan.plan.size()); + ROS_INFO("KCL: (%s) Current action: %d", ros::this_node::getName().c_str(), current_action); ros::Rate loop_rate(10); replan_requested = false; @@ -131,7 +135,7 @@ namespace KCL_rosplan { if(replan_requested) return false; } - ROS_INFO("KCL: (%s) Dispatch complete.", ros::this_node::getName().c_str()); + ROS_INFO("KCL: (%s) Dispatch complete.", ros::this_node::getName().c_str()); return true; } diff --git a/rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp b/rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp index a077b976c..be6850dbe 100644 --- a/rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp +++ b/rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp @@ -23,7 +23,6 @@ namespace KCL_rosplan { PDDLSimplePlanParser::~PDDLSimplePlanParser() { - } void PDDLSimplePlanParser::reset() { @@ -31,6 +30,10 @@ namespace KCL_rosplan { } 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_INFO("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); diff --git a/rosplan_planning_system/src/PlanParsing/PlanParser.cpp b/rosplan_planning_system/src/PlanParsing/PlanParser.cpp index 0cee1c5de..2bdd49e97 100644 --- a/rosplan_planning_system/src/PlanParsing/PlanParser.cpp +++ b/rosplan_planning_system/src/PlanParsing/PlanParser.cpp @@ -7,7 +7,9 @@ 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 +55,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/PlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp index dbeb68b89..d5759fb5c 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,8 +69,8 @@ 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 + + // save parameters data_path = dataPath; domain_path = domainPath; problem_path = problemPath; @@ -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..fa8b849f8 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_INFO("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); From cb1fe8251c4033d7c1a2d43c603d673d5ef1422d Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Mon, 6 Jul 2020 09:33:00 +0200 Subject: [PATCH 03/12] Update CFFPlannerInterface to export the Contiget-FF plan following POPF format, so it could be read by PDDLSimplePlanParser. --- .../PlannerInterface/CFFPlannerInterface.h | 3 +- .../PlannerInterface/CFFPlannerInterface.cpp | 210 ++++++++++++------ 2 files changed, 149 insertions(+), 64 deletions(-) diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h index a8f0c622e..ed84d6735 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h @@ -14,14 +14,13 @@ namespace KCL_rosplan { { private: - bool use_ffha; - 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: diff --git a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp index 1965723e0..82c0b17c6 100644 --- a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp @@ -1,30 +1,53 @@ #include "rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h" -#include -#include #include #include +#include +#include #include +#include using namespace KCL_rosplan; -CFFPlannerInterface::CFFPlannerInterface(ros::NodeHandle& nh) -{ - node_handle = &nh; +namespace { + + unsigned int split_string(const std::string &txt, std::vector &strs, char ch) { + size_t pos = txt.find( ch ); + unsigned int initialPos = 0; + strs.clear(); + + while( pos != std::string::npos && pos < txt.length()) { + if(txt.substr( initialPos, pos - initialPos + 1 ) !=" ") { + std::string s = txt.substr( initialPos, pos - initialPos + 1 ); + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + strs.push_back(s); + } + initialPos = pos + 1; + pos = txt.find( ch, initialPos ); + } + + strs.push_back( txt.substr( initialPos, txt.size() - initialPos ) ); + return strs.size(); + } +} - plan_server = new actionlib::SimpleActionServer((*node_handle), "start_planning", boost::bind(&PlannerInterface::runPlanningServerAction, this, _1), false); +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); - node_handle->param("use_ffha", this->use_ffha, false); - plan_server->start(); } -CFFPlannerInterface::~CFFPlannerInterface() -{ +CFFPlannerInterface::~CFFPlannerInterface() { delete plan_server; } @@ -33,8 +56,12 @@ void CFFPlannerInterface::clearPreviousPlan() { } void CFFPlannerInterface::saveProblemToFileIfNeeded() { - 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()); + + 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_INFO("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()); @@ -44,10 +71,13 @@ void CFFPlannerInterface::saveProblemToFileIfNeeded() { } 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); @@ -57,17 +87,18 @@ std::string CFFPlannerInterface::runCommand(std::string cmd) { return data; } -void CFFPlannerInterface::callExternalPlanner() { +void CFFPlannerInterface::callExternalPlanner() { + // 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.pddl"; + std::string commandString = str + " > " + data_path + "plan.cff"; // call the planer - ROS_INFO("KCL: (%s) (%s) Running: %s", ros::this_node::getName().c_str(), problem_name.c_str(), commandString.c_str()); + ROS_INFO("KCL: (%s) (%s) Running planner: %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()); } @@ -84,57 +115,104 @@ bool CFFPlannerInterface::isPlanSolved(std::ifstream &plan_file) { } } + 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()); + std::string line; + bool isPlanParsed = false; + + while(!plan_file.eof() && !isPlanParsed) { + + std::getline(plan_file, line); + + if(!line.empty()) { - if (this->use_ffha) { - std::getline(plan_file, line); //go to first action line - } else { - // actions look like this: - // step 0: got_place C1 - // 1: find_object V1 C1 - // plan cost: XX - while (std::getline(plan_file, line)) { // Move to the beginning of the plan - if (line.substr(0, 4) == "step") { - line = line.substr(4); // Remove the step - break; + // actions look like this: + // 17||0 --- FIND_OBJECT C1 ITEM_0 --- SON: 18||0 + // 18||0 --- PICKUP_OBJECT C1 ITEM_0 --- SON: 19||0 + + if (!(line.compare("-------------------------------------------------") == 0)) { + + // extract elements from line + std::vector tokens; + split_string(line, tokens, ' '); + + std::string action_id = tokens[0].substr(0, tokens[0].find("||")); + std::string operator_name = tokens[2]; + + typedef std::vector OperatorParams; + OperatorParams params; + + int idx = 3; + while (idx < tokens.size() && tokens[idx] != "---") { + params.push_back(tokens[idx]); + idx++; + } + + // add action based on elements of the line + std::string operator_parameters; + for (OperatorParams::iterator it = params.begin(); it != params.end(); ++it) { + if(std::next(it) != params.end()) { + operator_parameters = operator_parameters + (*it) + " "; + } else { + operator_parameters = operator_parameters + (*it); + } + } + std::string action = action_id + ": " + + "(" + operator_name + " " + operator_parameters + ") [0.001]\n"; + + planner_output += action; + ROS_INFO("KCL: (%s) Action: { %s }", ros::this_node::getName().c_str(), action.c_str()); } } - } - - // First iteration line will be like 0: got_place C1 - while (line.find("Total cost") == line.npos and line.find("time spend") == line.npos and line.size() > 0) { - std::stringstream ss(line); // To trim whitespaces - std::string aux; - // Read the action number X: - ss >> aux; - std::string add = (use_ffha) ? " " : " ("; - planner_output += aux + add; // Add parenthesis before the action - while (ss >> aux) { // Read the rest - planner_output += aux; - if (ss.good()) planner_output += " "; // Add a whitespace unless we have processed all the line + else { + isPlanParsed = true; } - add = (use_ffha) ? " [0.001]\n" : ") [0.001]\n"; - planner_output += add; // Close parenthesis and add duration - std::getline(plan_file, line); + } + + if (isPlanParsed) + { + std::transform(planner_output.begin(), planner_output.end(), planner_output.begin(), ::tolower); } - // Convert to lowercase as FF prints all the actions and parameters in uppercase - 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(), isPlanParsed); } bool CFFPlannerInterface::parsePlan() { - std::ifstream plan_file; - plan_file.open((data_path + "plan.pddl").c_str()); - bool solved = isPlanSolved(plan_file); - if (solved) { - convertPlanToPopfFormat(plan_file); - } + 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() { @@ -146,29 +224,37 @@ bool CFFPlannerInterface::runPlanner() { callExternalPlanner(); success = parsePlan(); - if(!success) 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()); + if(!success) { + 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 success; } int main(int argc, char **argv) { - srand (static_cast (time(0))); + srand(static_cast (time(0))); ros::init(argc,argv,"rosplan_planner_interface"); ros::NodeHandle nh("~"); - KCL_rosplan::CFFPlannerInterface 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(&pi)); + 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)); - // start the planning services - ros::ServiceServer service1 = nh.advertiseService("planning_server", &KCL_rosplan::PlannerInterface::runPlanningServerDefault, dynamic_cast(&pi)); - ros::ServiceServer service2 = nh.advertiseService("planning_server_params", &KCL_rosplan::PlannerInterface::runPlanningServerParams, 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(); From 65770f7efcaa52f5e3c8e50a71b59e2423c820e3 Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Wed, 8 Jul 2020 16:54:20 +0200 Subject: [PATCH 04/12] Remove wrong commited file. --- .idea/new_rosplan.iml | 8 -------- 1 file changed, 8 deletions(-) delete mode 100644 .idea/new_rosplan.iml diff --git a/.idea/new_rosplan.iml b/.idea/new_rosplan.iml deleted file mode 100644 index d0876a78d..000000000 --- a/.idea/new_rosplan.iml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file From 5057807be912250bcdc12dee5ecc2ee0914e8801 Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 12 Jul 2020 09:08:00 +0200 Subject: [PATCH 05/12] Create functions to extract line and create action functionalities of convertPlanToPopfFormat(). --- .../PlannerInterface/CFFPlannerInterface.h | 2 +- .../PlannerInterface/CFFPlannerInterface.cpp | 120 ++++++++++-------- .../test/src/PlannerInterfaceTests.cpp | 1 - 3 files changed, 70 insertions(+), 53 deletions(-) diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h index ed84d6735..a9c8f824c 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/CFFPlannerInterface.h @@ -33,6 +33,6 @@ namespace KCL_rosplan { virtual ~CFFPlannerInterface(); }; -} // close namespace +} #endif diff --git a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp index 82c0b17c6..9dbe02864 100644 --- a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp @@ -11,24 +11,58 @@ using namespace KCL_rosplan; namespace { - unsigned int split_string(const std::string &txt, std::vector &strs, char ch) { - size_t pos = txt.find( ch ); + typedef std::vector OperatorParams; + + unsigned int split_string(const std::string &txt, std::vector &strs, char separator) { + size_t pos = txt.find(separator); unsigned int initialPos = 0; strs.clear(); - while( pos != std::string::npos && pos < txt.length()) { - if(txt.substr( initialPos, pos - initialPos + 1 ) !=" ") { - std::string s = txt.substr( initialPos, pos - initialPos + 1 ); + while(pos != std::string::npos && pos < txt.length()) { + if(txt.substr(initialPos, pos - initialPos + 1) !=" ") { + std::string s = txt.substr(initialPos, pos - initialPos + 1); s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); strs.push_back(s); } initialPos = pos + 1; - pos = txt.find( ch, initialPos ); + pos = txt.find(separator, initialPos); } - strs.push_back( txt.substr( initialPos, txt.size() - initialPos ) ); + strs.push_back(txt.substr(initialPos, txt.size() - initialPos)); return strs.size(); } + + void extractElementsFromLine(const std::string &line, std::string &action_id, std::string &operator_name, OperatorParams &operator_params) { + + 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_INFO("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_INFO("KCL: (%s) Action: %s", ros::this_node::getName().c_str(), action.c_str()); + } } CFFPlannerInterface::CFFPlannerInterface(ros::NodeHandle& nh) { @@ -47,7 +81,7 @@ CFFPlannerInterface::CFFPlannerInterface(ros::NodeHandle& nh) { plan_server->start(); } -CFFPlannerInterface::~CFFPlannerInterface() { +CFFPlannerInterface::~CFFPlannerInterface() { delete plan_server; } @@ -61,7 +95,7 @@ void CFFPlannerInterface::saveProblemToFileIfNeeded() { use_problem_topic && problem_instance_received); if(use_problem_topic && problem_instance_received) { - ROS_INFO("KCL: (%s) Writing problem to file: %s", ros::this_node::getName().c_str(), problem_name.c_str()); + 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()); @@ -89,17 +123,22 @@ std::string CFFPlannerInterface::runCommand(std::string cmd) { void CFFPlannerInterface::callExternalPlanner() { - // 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.cff"; + 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"; - // call the planer - ROS_INFO("KCL: (%s) (%s) Running planner: %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) 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()); } @@ -119,7 +158,6 @@ bool CFFPlannerInterface::isPlanSolved(std::ifstream &plan_file) { return solved; } - void CFFPlannerInterface::savePlanInPopfFormatToFile() { std::string plan_file = data_path + "plan.pddl"; @@ -147,37 +185,22 @@ void CFFPlannerInterface::convertPlanToPopfFormat(std::ifstream &plan_file) { // actions look like this: // 17||0 --- FIND_OBJECT C1 ITEM_0 --- SON: 18||0 // 18||0 --- PICKUP_OBJECT C1 ITEM_0 --- SON: 19||0 - + // + if (!(line.compare("-------------------------------------------------") == 0)) { // extract elements from line std::vector tokens; split_string(line, tokens, ' '); - - std::string action_id = tokens[0].substr(0, tokens[0].find("||")); - std::string operator_name = tokens[2]; - typedef std::vector OperatorParams; - OperatorParams params; + std::string action_id; + std::string operator_name; + OperatorParams operator_params; + extractElementsFromLine(line, action_id, operator_name, operator_params); - int idx = 3; - while (idx < tokens.size() && tokens[idx] != "---") { - params.push_back(tokens[idx]); - idx++; - } - - // add action based on elements of the line - std::string operator_parameters; - for (OperatorParams::iterator it = params.begin(); it != params.end(); ++it) { - if(std::next(it) != params.end()) { - operator_parameters = operator_parameters + (*it) + " "; - } else { - operator_parameters = operator_parameters + (*it); - } - } - std::string action = action_id + ": " - + "(" + operator_name + " " + operator_parameters + ") [0.001]\n"; - + 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()); } @@ -216,6 +239,7 @@ bool CFFPlannerInterface::parsePlan() { } bool CFFPlannerInterface::runPlanner() { + bool success = false; clearPreviousPlan(); @@ -224,13 +248,7 @@ bool CFFPlannerInterface::runPlanner() { callExternalPlanner(); success = parsePlan(); - if(!success) { - 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()); - } - + ROS_INFO("KCL: (%s) (%s) Was plan solved? %d", ros::this_node::getName().c_str(), problem_name.c_str(), success); return success; } diff --git a/rosplan_planning_system/test/src/PlannerInterfaceTests.cpp b/rosplan_planning_system/test/src/PlannerInterfaceTests.cpp index 19a309648..4600fc1eb 100644 --- a/rosplan_planning_system/test/src/PlannerInterfaceTests.cpp +++ b/rosplan_planning_system/test/src/PlannerInterfaceTests.cpp @@ -12,7 +12,6 @@ #include #include -#include "rosplan_planning_system/PlannerInterface/POPFPlannerInterface.h" #include "rosplan_planning_system/PlannerInterface/PlannerInterface.h" #include "rosplan_dispatch_msgs/PlanningService.h" From 3f522dfe4b980f7dbed247a415241deafa6b6c30 Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 12 Jul 2020 09:16:14 +0200 Subject: [PATCH 06/12] Rename arguments of split_string(). --- .../PlannerInterface/CFFPlannerInterface.cpp | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp index 9dbe02864..e32d1586d 100644 --- a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp @@ -13,23 +13,23 @@ namespace { typedef std::vector OperatorParams; - unsigned int split_string(const std::string &txt, std::vector &strs, char separator) { - size_t pos = txt.find(separator); + unsigned int split_string(const std::string &text, std::vector &tokens, char separator) { + size_t pos = text.find(separator); unsigned int initialPos = 0; - strs.clear(); + tokens.clear(); - while(pos != std::string::npos && pos < txt.length()) { - if(txt.substr(initialPos, pos - initialPos + 1) !=" ") { - std::string s = txt.substr(initialPos, pos - initialPos + 1); + 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()); - strs.push_back(s); + tokens.push_back(s); } initialPos = pos + 1; - pos = txt.find(separator, initialPos); + pos = text.find(separator, initialPos); } - strs.push_back(txt.substr(initialPos, txt.size() - initialPos)); - return strs.size(); + 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) { @@ -46,7 +46,7 @@ namespace { idx++; } - ROS_INFO("KCL: (%s) Elements in line: { %s, %s, etc. }", ros::this_node::getName().c_str(), action_id.c_str(), operator_name.c_str()); + 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) { @@ -61,7 +61,7 @@ namespace { } action = action_id + ": " + "(" + operator_name + " " + operator_parameters + ") [0.001]\n"; - ROS_INFO("KCL: (%s) Action: %s", ros::this_node::getName().c_str(), action.c_str()); + ROS_DEBUG("KCL: (%s) Action: %s", ros::this_node::getName().c_str(), action.c_str()); } } @@ -172,9 +172,9 @@ void CFFPlannerInterface::savePlanInPopfFormatToFile() { void CFFPlannerInterface::convertPlanToPopfFormat(std::ifstream &plan_file) { ROS_INFO("KCL: (%s) cff plan to popf format", ros::this_node::getName().c_str()); - - std::string line; - bool isPlanParsed = false; + + bool isPlanParsed = false; + std::string line; while(!plan_file.eof() && !isPlanParsed) { @@ -215,7 +215,7 @@ void CFFPlannerInterface::convertPlanToPopfFormat(std::ifstream &plan_file) { 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(), isPlanParsed); + ROS_INFO("KCL: (%s) Plan converted to popf format: %d", ros::this_node::getName().c_str(), isPlanParsed); } bool CFFPlannerInterface::parsePlan() { @@ -230,7 +230,7 @@ bool CFFPlannerInterface::parsePlan() { solved = isPlanSolved(plan_file); if (solved) { - convertPlanToPopfFormat(plan_file); + (void) convertPlanToPopfFormat(plan_file); savePlanInPopfFormatToFile(); } plan_file.close(); From 5724818f7a635334c63d68512e39b840c9930263 Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 12 Jul 2020 09:28:48 +0200 Subject: [PATCH 07/12] Clean some code. --- .../PlannerInterface/CFFPlannerInterface.cpp | 65 +++++++++---------- 1 file changed, 31 insertions(+), 34 deletions(-) diff --git a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp index e32d1586d..478c1ae87 100644 --- a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp @@ -33,7 +33,12 @@ namespace { } 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, ' '); @@ -171,51 +176,43 @@ void CFFPlannerInterface::savePlanInPopfFormatToFile() { void CFFPlannerInterface::convertPlanToPopfFormat(std::ifstream &plan_file) { - ROS_INFO("KCL: (%s) cff plan to popf format", ros::this_node::getName().c_str()); + ROS_INFO("KCL: (%s) cff plan to POPF format", ros::this_node::getName().c_str()); - bool isPlanParsed = false; + bool isParseCompleted = false; std::string line; - while(!plan_file.eof() && !isPlanParsed) { + while(!plan_file.eof() && !isParseCompleted) { std::getline(plan_file, line); - if(!line.empty()) { - - // actions look like this: - // 17||0 --- FIND_OBJECT C1 ITEM_0 --- SON: 18||0 - // 18||0 --- PICKUP_OBJECT C1 ITEM_0 --- SON: 19||0 - // - - if (!(line.compare("-------------------------------------------------") == 0)) { + if(!line.empty() && + !(line.compare("-------------------------------------------------") == 0)) { - // extract elements from line - std::vector tokens; - split_string(line, tokens, ' '); + 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()); - } + 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 { - isPlanParsed = true; + isParseCompleted = true; } } - if (isPlanParsed) + 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(), isPlanParsed); + ROS_INFO("KCL: (%s) Plan converted to popf format: %d", ros::this_node::getName().c_str(), isParseCompleted); } bool CFFPlannerInterface::parsePlan() { @@ -230,7 +227,7 @@ bool CFFPlannerInterface::parsePlan() { solved = isPlanSolved(plan_file); if (solved) { - (void) convertPlanToPopfFormat(plan_file); + convertPlanToPopfFormat(plan_file); savePlanInPopfFormatToFile(); } plan_file.close(); @@ -240,16 +237,16 @@ bool CFFPlannerInterface::parsePlan() { bool CFFPlannerInterface::runPlanner() { - bool success = false; + bool solved = false; clearPreviousPlan(); saveProblemToFileIfNeeded(); callExternalPlanner(); - success = parsePlan(); + solved = parsePlan(); - ROS_INFO("KCL: (%s) (%s) Was plan solved? %d", ros::this_node::getName().c_str(), problem_name.c_str(), success); - return success; + 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) { From 07e6483307e6a8b4126858850b7ad3f19b3adeeb Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 12 Jul 2020 11:23:58 +0200 Subject: [PATCH 08/12] Add tests for Contingent-FF planner, and rename existing ones for PlannerInterface to PopfPlannerInterface. --- rosplan_planning_system/CMakeLists.txt | 9 +- ...rface.test => popf_planner_interface.test} | 2 +- .../test/src/CffPlannerInterfaceTests.cpp | 153 ++++++++++++++++++ ...ests.cpp => PopfPlannerInterfaceTests.cpp} | 3 +- 4 files changed, 162 insertions(+), 5 deletions(-) rename rosplan_planning_system/test/launch/{planner_interface.test => popf_planner_interface.test} (93%) create mode 100644 rosplan_planning_system/test/src/CffPlannerInterfaceTests.cpp rename rosplan_planning_system/test/src/{PlannerInterfaceTests.cpp => PopfPlannerInterfaceTests.cpp} (98%) diff --git a/rosplan_planning_system/CMakeLists.txt b/rosplan_planning_system/CMakeLists.txt index 488c9b342..8ea19604b 100644 --- a/rosplan_planning_system/CMakeLists.txt +++ b/rosplan_planning_system/CMakeLists.txt @@ -167,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/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 98% rename from rosplan_planning_system/test/src/PlannerInterfaceTests.cpp rename to rosplan_planning_system/test/src/PopfPlannerInterfaceTests.cpp index 4600fc1eb..b60c23f24 100644 --- a/rosplan_planning_system/test/src/PlannerInterfaceTests.cpp +++ b/rosplan_planning_system/test/src/PopfPlannerInterfaceTests.cpp @@ -8,6 +8,7 @@ * Unit tests for ROSPlan planner interface */ +#include #include #include #include @@ -146,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(); } From cff5ef80f8820dcd4fdbff941c8204aeceba79d2 Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 12 Jul 2020 11:32:43 +0200 Subject: [PATCH 09/12] Add missed test launcher for Continget-FF planner. --- .../test/launch/cff_planner_interface.test | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 rosplan_planning_system/test/launch/cff_planner_interface.test 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 9fbb1296d64b60d8391a3218454dec2e734725de Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 12 Jul 2020 11:35:09 +0200 Subject: [PATCH 10/12] Fix that generated plan was not written in POPF format due to a wrong previously modification. And add a missed extra space in the POPF action. --- .../PlannerInterface/CFFPlannerInterface.cpp | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp index 478c1ae87..66e1c44b8 100644 --- a/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/CFFPlannerInterface.cpp @@ -65,7 +65,7 @@ namespace { } } - action = action_id + ": " + "(" + operator_name + " " + operator_parameters + ") [0.001]\n"; + 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()); } } @@ -185,24 +185,24 @@ void CFFPlannerInterface::convertPlanToPopfFormat(std::ifstream &plan_file) { std::getline(plan_file, line); - if(!line.empty() && - !(line.compare("-------------------------------------------------") == 0)) { - - std::vector tokens; - split_string(line, tokens, ' '); + 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 { + 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; } } From efa9014b45093ad11d1a4e66739362e04f18a778 Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 12 Jul 2020 11:44:14 +0200 Subject: [PATCH 11/12] Change some logs from INFO to DEBUG, and replace spaces by tabs in some files as tabs are used. --- .../src/PlanDispatch/SimplePlanDispatcher.cpp | 45 ++++++++--------- .../src/PlanParsing/PDDLSimplePlanParser.cpp | 48 ++++++++++--------- .../src/PlanParsing/PlanParser.cpp | 5 +- 3 files changed, 51 insertions(+), 47 deletions(-) diff --git a/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp b/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp index 71db791b8..834daac52 100644 --- a/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp +++ b/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp @@ -37,8 +37,9 @@ 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); + 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(); @@ -55,8 +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_INFO("KCL: (%s) Num actions: %d", ros::this_node::getName().c_str(), current_plan.plan.size()); - ROS_INFO("KCL: (%s) Current action: %d", ros::this_node::getName().c_str(), current_action); + ROS_DEBUG("KCL: (%s) Num actions: %d", 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; @@ -135,7 +136,7 @@ namespace KCL_rosplan { if(replan_requested) return false; } - ROS_INFO("KCL: (%s) Dispatch complete.", ros::this_node::getName().c_str()); + ROS_INFO("KCL: (%s) Dispatch complete.", ros::this_node::getName().c_str()); return true; } @@ -173,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 be6850dbe..8f69c392d 100644 --- a/rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp +++ b/rosplan_planning_system/src/PlanParsing/PDDLSimplePlanParser.cpp @@ -26,13 +26,15 @@ namespace KCL_rosplan { } 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_INFO("KCL: (%s) Num actions: %d", ros::this_node::getName().c_str(), action_list.size()); + + 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; @@ -145,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 2bdd49e97..fa375ff0e 100644 --- a/rosplan_planning_system/src/PlanParsing/PlanParser.cpp +++ b/rosplan_planning_system/src/PlanParsing/PlanParser.cpp @@ -7,8 +7,9 @@ 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) Is plan empty?: %d", ros::this_node::getName().c_str(), plannerOutput.data.size() == 0); + 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(); @@ -55,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(); From f505f1da1434a6b4521689541037d96022594497 Mon Sep 17 00:00:00 2001 From: Diego Escudero Date: Sun, 12 Jul 2020 11:59:37 +0200 Subject: [PATCH 12/12] Change some logs from INFO to DEBUG, and replace spaces by tabs in some files as tabs are used. --- .../src/PlanDispatch/SimplePlanDispatcher.cpp | 4 +-- .../src/PlannerInterface/PlannerInterface.cpp | 12 +++---- .../ProblemGeneration/ProblemInterface.cpp | 32 +++++++++---------- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp b/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp index 834daac52..54ef7049b 100644 --- a/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp +++ b/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp @@ -41,7 +41,7 @@ namespace KCL_rosplan { 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; + plan_received = true; mission_start_time = ros::WallTime::now().toSec(); current_plan = plan; } @@ -56,7 +56,7 @@ 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: %d", ros::this_node::getName().c_str(), current_plan.plan.size()); + 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); diff --git a/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp index d5759fb5c..2146a490f 100644 --- a/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/PlannerInterface.cpp @@ -8,7 +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); + 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; @@ -70,16 +70,16 @@ namespace KCL_rosplan { */ bool PlannerInterface::runPlanningServer(std::string domainPath, std::string problemPath, std::string dataPath, std::string plannerCommand, bool useProblemTopic) { - // save parameters + // save parameters data_path = dataPath; domain_path = domainPath; problem_path = problemPath; 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("/\\"); @@ -98,7 +98,7 @@ namespace KCL_rosplan { // publish planner output if(success) { - ROS_INFO("KCL: (%s) Plan published.", ros::this_node::getName().c_str()); + 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 fa8b849f8..a4f083f98 100644 --- a/rosplan_planning_system/src/ProblemGeneration/ProblemInterface.cpp +++ b/rosplan_planning_system/src/ProblemGeneration/ProblemInterface.cpp @@ -132,7 +132,7 @@ namespace KCL_rosplan { // publish problem std::ifstream problemIn(problem_path.c_str()); if(problemIn) { - ROS_INFO("KCL: (%s) (%s) Publish problem.", ros::this_node::getName().c_str(), problem_name.c_str()); + 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); @@ -142,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; +}